diff --git a/.github/scripts/check_people_model.py b/.github/scripts/check_people_model.py
index f617646..ce67a3c 100644
--- a/.github/scripts/check_people_model.py
+++ b/.github/scripts/check_people_model.py
@@ -16,8 +16,8 @@ def main(model: str, image: str, backends: list[str]) -> int:
     try:
         import cv2
 
-        sys.path.insert(0, 'src/robot_perception')
-        from robot_perception.detector import OnnxPersonDetector
+        sys.path.insert(0, 'src/pashupati_perception')
+        from pashupati_perception.detector import OnnxPersonDetector
     except Exception:  # noqa: B902  the environment itself is broken: say how
         annotate('error', 'imports', traceback.format_exc()[-1500:])
         return 1
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 7ab3d51..47b8cb2 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -16,8 +16,8 @@ env:
   ROSDEP_SKIP: >-
     livox_ros_driver2 fast_lio legkilo apriltag_draw
   PACKAGES: >-
-    robot_interfaces robot_odometry robot_localization robot_perception robot_navigation
-    robot_bridge robot_fleet robot_inspection robot_health robot_rviz pashupati robot_sim
+    pashupati_perception robot_bridge robot_fleet robot_inspection robot_health robot_rviz
+    pashupati robot_sim
 
 jobs:
   scripts:
@@ -87,6 +87,15 @@ jobs:
         shell: bash
     steps:
       - uses: actions/checkout@v4   # no submodules: they need SSH keys and vendor SDKs
+      - name: Dharma packages
+        env:
+          # the dharma repos are private: a fine-grained token with read access to them
+          DHARMA_TOKEN: ${{ secrets.DHARMA_TOKEN }}
+        run: |
+          apt-get update && apt-get install -y python3-vcstool git
+          git config --global url."https://x-access-token:${DHARMA_TOKEN}@github.com/".insteadOf \
+            "git@github.com:"
+          vcs import src < pashupati.repos
       - name: Dependencies
         run: |
           apt-get update
@@ -96,7 +105,7 @@ jobs:
       - name: Build
         run: |
           source /opt/ros/humble/setup.bash
-          colcon build --packages-select $PACKAGES --event-handlers console_cohesion+
+          colcon build --packages-up-to $PACKAGES --event-handlers console_cohesion+
       - name: Test
         run: |
           source /opt/ros/humble/setup.bash
@@ -118,7 +127,7 @@ jobs:
           source /opt/ros/humble/setup.bash
           source install/setup.bash
           ros2 launch pashupati robot.launch.py --show-args > /dev/null
-          ros2 launch robot_odometry odometry.launch.py --show-args > /dev/null
+          ros2 launch dharma_odometry odometry.launch.py --show-args > /dev/null
           ros2 launch robot_sim sim.launch.py --show-args > /dev/null
       - name: Test reports
         if: always()
diff --git a/.github/workflows/people-model.yml b/.github/workflows/people-model.yml
index 6ec9fda..6382a7e 100644
--- a/.github/workflows/people-model.yml
+++ b/.github/workflows/people-model.yml
@@ -9,7 +9,7 @@ on:
   push:
     branches: [dev]
     paths:
-      - src/robot_perception/robot_perception/detector.py
+      - src/pashupati_perception/pashupati_perception/detector.py
       - .github/scripts/check_people_model.py
       - .github/workflows/people-model.yml
   workflow_dispatch:
diff --git a/.gitignore b/.gitignore
index 3bc84be..98ee250 100644
--- a/.gitignore
+++ b/.gitignore
@@ -38,4 +38,7 @@ desktop.ini
 .cache/
 __MACOSX/
 
-.vscode/settings.json
\ No newline at end of file
+.vscode/settings.json
+
+# ---- dharma packages (vcs import src < pashupati.repos) ----
+src/dharma/
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 24fd273..227b299 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -48,7 +48,7 @@ pure-logic tests also run with plain `pytest` on a laptop.
   `OPENBLAS_NUM_THREADS=1`.
 - **No per-element Python objects at sensor rate.** rclpy builds one object per element of a
   message array (a Livox CustomMsg has ~20k points): read big messages as numpy straight from the
-  buffer (`PointCloud2.data`, or `raw=True` and the CDR bytes, see `robot_perception/livox_cdr.py`).
+  buffer (`PointCloud2.data`, or `raw=True` and the CDR bytes, see `pashupati_perception/livox_cdr.py`).
 - **Subscribe to heavy topics only while someone uses them** (the teleop camera and cloud).
 - **Validate everything from the network** (teleop UDP, NETRA commands, spool files): clamp
   numbers, reject non-finite values, keep file paths inside their directory, never raise in a
diff --git a/README.md b/README.md
index 4f327c3..051fdf2 100644
--- a/README.md
+++ b/README.md
@@ -132,11 +132,11 @@ flowchart LR
 
 | Package | Role |
 |---|---|
-| [`robot_interfaces`](src/robot_interfaces) | messages and services |
-| [`robot_odometry`](src/robot_odometry) | LiDAR-inertial odometry (FAST-LIO2 or Leg-KILO), stillness gate, odometry monitor, route recording |
-| [`robot_localization`](src/robot_localization) | FAST-LIO frame bridge, `map→odom` from AprilTags / RViz, tag recording |
-| [`robot_perception`](src/robot_perception) | LiDAR sectors, AprilTag, person detection (YOLO11n ONNX) |
-| [`robot_navigation`](src/robot_navigation) | route processing, 5 controllers, 4 avoidance algorithms, final approach, safety |
+| [`dharma_interfaces`](src/dharma_interfaces) | messages and services |
+| [`dharma_odometry`](src/dharma_odometry) | LiDAR-inertial odometry (FAST-LIO2 or Leg-KILO), stillness gate, odometry monitor, route recording |
+| [`dharma_localization`](src/dharma_localization) | FAST-LIO frame bridge, `map→odom` from AprilTags / RViz, tag recording |
+| [`pashupati_perception`](src/pashupati_perception) | LiDAR sectors, AprilTag, person detection (YOLO11n ONNX) |
+| [`dharma_navigation`](src/dharma_navigation) | route processing, 5 controllers, 4 avoidance algorithms, final approach, safety |
 | [`robot_bridge`](src/robot_bridge) | AUTO / REMOTE control mux, UDP teleop gateway ([protocol](src/robot_bridge/PROTOCOL.md)), WebRTC video |
 | [`robot_fleet`](src/robot_fleet) | NETRA uplink: telemetry, events, crowd, anomalies, routes ([API](src/robot_fleet/API.md)) |
 | [`robot_inspection`](src/robot_inspection) | VLM inspection (Ollama): continuous, interval queue or stop points, own finding types |
@@ -255,7 +255,7 @@ pnode robot_bridge teleop_udp_node bridge_params.yaml   # one node, as robot.lau
   **Start**), or by hand:
 
 ```bash
-ros2 service call /navigation/load_path robot_interfaces/srv/LoadPath "{waypoints_file: '/home/robot/dev/Pashupati/map/<date>/<time>_path.csv'}"
+ros2 service call /navigation/load_path dharma_interfaces/srv/LoadPath "{waypoints_file: '/home/robot/dev/Pashupati/map/<date>/<time>_path.csv'}"
 ros2 service call /navigation/start_nav std_srvs/srv/SetBool "{data: true}"
 ```
 
@@ -312,11 +312,11 @@ the tuned values with **Save YAML**.
 | Service | Type | Node |
 |---|---|---|
 | `/route/record` | `std_srvs/SetBool` | route_recorder_node |
-| `/route/mark_stop_point` | `robot_interfaces/MarkStopPoint` | route_recorder_node |
+| `/route/mark_stop_point` | `dharma_interfaces/MarkStopPoint` | route_recorder_node |
 | `/localization/start`, `/localization/record_tags` | `std_srvs/Trigger` | localization_node |
-| `/navigation/load_path` | `robot_interfaces/LoadPath` | path_loader_node |
+| `/navigation/load_path` | `dharma_interfaces/LoadPath` | path_loader_node |
 | `/navigation/start_nav`, `/navigation/pause` | `std_srvs/SetBool` | path_follower_node |
-| `/bridge/set_mode` | `robot_interfaces/SetMode` | control_mux_node |
+| `/bridge/set_mode` | `dharma_interfaces/SetMode` | control_mux_node |
 | `/bridge/estop` | `std_srvs/SetBool` (`true` engages, `false` releases) | control_mux_node |
 | `/anomaly_detector_node/check_now` | `std_srvs/Trigger` | anomaly_detector_node |
 | `/<robot_ns>/<command>` | `std_srvs/Trigger` | robot driver ([contract](src/xtras/README.md)) |
@@ -337,11 +337,11 @@ the tuned values with **Save YAML**.
 | `/odometry/status` | `std_msgs/String` (latched) | odometry_monitor_node |
 | `/odometry/restarted` | `std_msgs/Empty` | odometry_monitor_node |
 | `/localization/status` | `std_msgs/String` (latched) | localization_node |
-| `/navigation/waypoints` | `robot_interfaces/WaypointPath` (latched) | path_loader_node |
-| `/navigation/status`, `/navigation/mission_status`, `/navigation/stop_point_event` | robot_interfaces | path_follower_node |
+| `/navigation/waypoints` | `dharma_interfaces/WaypointPath` (latched) | path_loader_node |
+| `/navigation/status`, `/navigation/mission_status`, `/navigation/stop_point_event` | dharma_interfaces | path_follower_node |
 | `/navigation/markers`, `/navigation/route_markers`, `/navigation/avoidance_markers` | `MarkerArray` | navigation |
-| `/perception/sector_scan` | `robot_interfaces/SectorScan` | lidar_sector_node |
-| `/perception/safety_scan` | `robot_interfaces/SectorScan` (360°, all blocked when the LiDAR is blind) | lidar_sector_node |
+| `/perception/sector_scan` | `dharma_interfaces/SectorScan` | lidar_sector_node |
+| `/perception/safety_scan` | `dharma_interfaces/SectorScan` (360°, all blocked when the LiDAR is blind) | lidar_sector_node |
 | `/perception/people`, `/perception/people_status` | `geometry_msgs/PoseArray`, `std_msgs/String` (JSON, latched) | person_detector_node |
 | `/anomaly_detector/result`, `/anomaly_detector/status` | `std_msgs/String` (JSON; status latched) | anomaly_detector_node |
 | `/health/status` | `std_msgs/String` (JSON, latched) | health_monitor_node |
diff --git a/docs/pdf/build.py b/docs/pdf/build.py
index 3504ad2..28c6b8b 100644
--- a/docs/pdf/build.py
+++ b/docs/pdf/build.py
@@ -31,12 +31,12 @@
 
 # Config files of the parameter reference, in reading order: (package, file, what it covers)
 PARAMETER_FILES = [
-    ('robot_navigation', 'navigation_params.yaml', 'route following, avoidance, stops'),
+    ('pashupati', 'navigation_params.yaml', 'route following, avoidance, stops'),
     ('robot_bridge', 'bridge_params.yaml', 'control mux, protective field, handheld, video'),
-    ('robot_perception', 'perception_params.yaml', 'LiDAR sectors, people, AprilTags'),
-    ('robot_localization', 'localization_params.yaml', 'map frame from AprilTags'),
+    ('pashupati', 'perception_params.yaml', 'LiDAR sectors, people, AprilTags'),
+    ('pashupati', 'localization_params.yaml', 'map frame from AprilTags'),
     ('robot_inspection', 'inspection_params.yaml', 'AI inspection'),
-    ('robot_odometry', 'odometry_params.yaml', 'odometry start, monitor, route recording'),
+    ('pashupati', 'odometry_params.yaml', 'odometry start, monitor, route recording'),
     ('robot_health', 'health_params.yaml', 'health monitor'),
     ('robot_fleet', 'fleet_params.yaml', 'NETRA uplink'),
 ]
diff --git a/pashupati.repos b/pashupati.repos
new file mode 100644
index 0000000..1f1fd97
--- /dev/null
+++ b/pashupati.repos
@@ -0,0 +1,22 @@
+# Dharma packages Pashupati runs on: vcs import src < pashupati.repos
+repositories:
+  dharma/dharma_interfaces:
+    type: git
+    url: git@github.com:DarmawanRobotics/dharma_interfaces.git
+    version: dev
+  dharma/dharma_odometry:
+    type: git
+    url: git@github.com:DarmawanRobotics/dharma_odometry.git
+    version: dev
+  dharma/dharma_localization:
+    type: git
+    url: git@github.com:DarmawanRobotics/dharma_localization.git
+    version: dev
+  dharma/dharma_navigation:
+    type: git
+    url: git@github.com:DarmawanRobotics/dharma_navigation.git
+    version: dev
+  dharma/dharma_perception:
+    type: git
+    url: git@github.com:DarmawanRobotics/dharma_perception.git
+    version: dev
diff --git a/script/setup.sh b/script/setup.sh
index 411bec3..51ae1db 100755
--- a/script/setup.sh
+++ b/script/setup.sh
@@ -7,7 +7,7 @@
 #   script/setup.sh install livox build  # install only these
 #   script/setup.sh install all          # every component, including ros and service
 #
-# Components: ros tools submodules livox robot legkilo rosdep realsense webrtc ollama people_model
+# Components: ros tools submodules dharma livox robot legkilo rosdep realsense webrtc ollama people_model
 #             build config service network
 # Settings come from the environment, then from /etc/pashupati/pashupati.env (ROBOT_SETUP,
 # ROBOT_IP, PEOPLE, INSPECTION, ...): no need to source it first.
@@ -64,9 +64,9 @@ BUILD_STAMP="$ROOT/install/.pashupati_build"
 UVC_BLACKLIST=/etc/modprobe.d/blacklist-uvcvideo-realsense.conf
 REALSENSE_BLACKLIST_UVC="${REALSENSE_BLACKLIST_UVC:-$([[ "$(uname -m)" == aarch64 ]] && echo 1 || echo 0)}"
 
-ALL=(ros tools submodules livox robot legkilo rosdep realsense webrtc ollama people_model build
+ALL=(ros tools submodules dharma livox robot legkilo rosdep realsense webrtc ollama people_model build
      config service network)
-DEFAULT_INSTALL=(tools submodules livox robot legkilo rosdep realsense webrtc ollama build config)
+DEFAULT_INSTALL=(tools submodules dharma livox robot legkilo rosdep realsense webrtc ollama build config)
 
 SUDO=$([[ "$(id -u)" -eq 0 ]] && echo "" || echo "sudo")
 IN_DOCKER=$([[ -f /.dockerenv ]] && echo 1 || echo 0)
@@ -147,6 +147,25 @@ install_submodules() {
         submodule update --init --recursive
 }
 
+check_dharma() {
+    local missing=() name
+    for name in $(awk '/^  dharma\// {sub(":", ""); print $1}' "$ROOT/pashupati.repos"); do
+        [[ -f "$ROOT/src/$name/README.md" ]] || missing+=("$name")
+    done
+    [[ ${#missing[@]} -eq 0 ]] && echo "src/dharma: all checked out" && return 0
+    echo "not checked out: ${missing[*]}"; return 1
+}
+install_dharma() {
+    command -v vcs >/dev/null || apt_install python3-vcstool
+    if ! vcs import --skip-existing "$ROOT/src" < "$ROOT/pashupati.repos"; then
+        log "SSH clone failed, retrying the dharma packages over HTTPS"
+        GIT_CONFIG_COUNT=1 GIT_CONFIG_KEY_0=url.https://github.com/.insteadOf \
+            GIT_CONFIG_VALUE_0=git@github.com: \
+            vcs import --skip-existing "$ROOT/src" < "$ROOT/pashupati.repos"
+    fi
+    vcs pull "$ROOT/src/dharma"
+}
+
 check_livox() {
     [[ -f /usr/local/lib/liblivox_lidar_sdk_shared.so ]] \
         && echo "/usr/local/lib/liblivox_lidar_sdk_shared.so" && return 0
diff --git a/script/start_tmux.sh b/script/start_tmux.sh
index bfa999d..e49ae73 100755
--- a/script/start_tmux.sh
+++ b/script/start_tmux.sh
@@ -63,9 +63,13 @@ else
     tmux send-keys -t "$SESSION:robot.0" "# ROBOT_BRINGUP unset: start the robot driver here" C-m
 fi
 sleep 3
-run robot.1 "ros2 launch robot_odometry odometry.launch.py method:=$ODOMETRY save_map:=$SAVE_MAP \
-localization:=true tags_config_file:=$TAGS leg_odom_topic:=/$ROBOT_NS/odom"
-run robot.2 "ros2 launch robot_perception perception.launch.py people:=$PEOPLE"
+CONFIG="\$(ros2 pkg prefix pashupati)/share/pashupati/config"
+run robot.1 "ros2 launch dharma_odometry odometry.launch.py method:=$ODOMETRY save_map:=$SAVE_MAP \
+localization:=true tags_config_file:=$TAGS leg_odom_topic:=/$ROBOT_NS/odom \
+params_file:=$CONFIG/odometry_params.yaml fast_lio_params_file:=$CONFIG/fast_lio.yaml"
+PERCEPTION="ros2 launch dharma_perception perception.launch.py visitors:=false params_file:=$CONFIG/perception_params.yaml"
+[[ "$PEOPLE" == "true" ]] && PERCEPTION+=" & ros2 launch pashupati_perception people.launch.py"
+run robot.2 "$PERCEPTION"
 if [[ "$RVIZ" == "1" ]]; then
     run robot.3 "ros2 run rviz2 rviz2 -d \$(ros2 pkg prefix robot_rviz)/share/robot_rviz/rviz/robot.rviz \
 --ros-args -r battery:=/$ROBOT_NS/battery -r cmd_vel:=/$ROBOT_NS/cmd_vel \
@@ -77,7 +81,7 @@ tmux select-layout -t "$SESSION:robot" tiled
 tmux new-window -t "$SESSION" -n nav
 tmux split-window -v -t "$SESSION:nav"
 if [[ "$MODE" == "patrol" ]]; then
-    run nav.0 "ros2 launch robot_navigation navigation.launch.py waypoints_file:=$ROUTE \
+    run nav.0 "ros2 launch dharma_navigation navigation.launch.py params_file:=$CONFIG/navigation_params.yaml waypoints_file:=$ROUTE \
 cmd_vel_topic:=/bridge/nav_cmd_vel battery_topic:=/$ROBOT_NS/battery"
 fi
 tmux split-window -h -t "$SESSION:nav.0"
diff --git a/src/pashupati/README.md b/src/pashupati/README.md
index 8a03e73..e596101 100644
--- a/src/pashupati/README.md
+++ b/src/pashupati/README.md
@@ -22,7 +22,7 @@ provide.
 | `robot_ns` | `$ROBOT_NS` or `robot` | robot driver namespace (`/robot/cmd_vel`, `/robot/odom`, ...) |
 | `robot_bringup` | `$ROBOT_BRINGUP` | robot package `<package>[/<file>.launch.py]`, started with `namespace:=<robot_ns>`; empty: the robot runs elsewhere (simulator, vendor service) |
 | `mode` | `$MODE` or `patrol` | `teach` keeps the map (FAST-LIO PCD / Leg-KILO backend, RAM grows); `mapping` still works |
-| `odometry` | `$ODOMETRY` or `fast_lio` | `fast_lio`, `leg_kilo` or `external` (simulator) ([robot_odometry](../robot_odometry)) |
+| `odometry` | `$ODOMETRY` or `fast_lio` | `fast_lio`, `leg_kilo` or `external` (simulator) ([dharma_odometry](../dharma_odometry)) |
 | `route` | `$ROUTE` | route CSV; empty reloads the last route loaded through `navigation/load_path` |
 | `tags` | `$TAGS` | AprilTag map poses (JSON), created by `localization/record_tags` |
 | `cc_url`, `cc_token` | `$CC_URL`, `$CC_TOKEN` | NETRA command center |
diff --git a/src/robot_odometry/config/fast_lio.yaml b/src/pashupati/config/fast_lio.yaml
similarity index 100%
rename from src/robot_odometry/config/fast_lio.yaml
rename to src/pashupati/config/fast_lio.yaml
diff --git a/src/robot_odometry/config/leg_kilo.yaml b/src/pashupati/config/leg_kilo.yaml
similarity index 100%
rename from src/robot_odometry/config/leg_kilo.yaml
rename to src/pashupati/config/leg_kilo.yaml
diff --git a/src/robot_localization/config/localization_params.yaml b/src/pashupati/config/localization_params.yaml
similarity index 100%
rename from src/robot_localization/config/localization_params.yaml
rename to src/pashupati/config/localization_params.yaml
diff --git a/src/robot_navigation/config/navigation_params.yaml b/src/pashupati/config/navigation_params.yaml
similarity index 100%
rename from src/robot_navigation/config/navigation_params.yaml
rename to src/pashupati/config/navigation_params.yaml
diff --git a/src/robot_odometry/config/odometry_params.yaml b/src/pashupati/config/odometry_params.yaml
similarity index 100%
rename from src/robot_odometry/config/odometry_params.yaml
rename to src/pashupati/config/odometry_params.yaml
diff --git a/src/robot_perception/config/perception_params.yaml b/src/pashupati/config/perception_params.yaml
similarity index 100%
rename from src/robot_perception/config/perception_params.yaml
rename to src/pashupati/config/perception_params.yaml
diff --git a/src/pashupati/launch/robot.launch.py b/src/pashupati/launch/robot.launch.py
index 59960ec..451b334 100644
--- a/src/pashupati/launch/robot.launch.py
+++ b/src/pashupati/launch/robot.launch.py
@@ -159,8 +159,8 @@ def node(package, executable, defaults, overrides=None, remappings=None, **kwarg
             **kwargs,
         )
 
-    nav = share('robot_navigation', 'config', 'navigation_params.yaml')
-    perception = share('robot_perception', 'config', 'perception_params.yaml')
+    nav = share('pashupati', 'config', 'navigation_params.yaml')
+    perception = share('pashupati', 'config', 'perception_params.yaml')
     bridge = share('robot_bridge', 'config', 'bridge_params.yaml')
     version = package_version(share('pashupati', 'package.xml'))
     git = os.environ.get('PASHUPATI_GIT', 'unknown')
@@ -185,7 +185,9 @@ def node(package, executable, defaults, overrides=None, remappings=None, **kwarg
         actions.append(mediamtx)
     actions.append(
         IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(share('robot_odometry', 'launch', 'odometry.launch.py')),
+            PythonLaunchDescriptionSource(
+                share('dharma_odometry', 'launch', 'odometry.launch.py')
+            ),
             launch_arguments={
                 'method': odometry,
                 'save_map': str(mode == 'teach'),
@@ -193,20 +195,23 @@ def node(package, executable, defaults, overrides=None, remappings=None, **kwarg
                 'record': 'true',
                 'leg_odom_topic': topic('odom'),
                 'site_config': site[0] if site else '',
+                'params_file': share('pashupati', 'config', 'odometry_params.yaml'),
+                'fast_lio_params_file': share('pashupati', 'config', 'fast_lio.yaml'),
+                'leg_kilo_config_file': share('pashupati', 'config', 'leg_kilo.yaml'),
             }.items(),
         )
     )
     actions += [
         node(
-            'robot_localization',
+            'dharma_localization',
             'localization_node',
-            [share('robot_localization', 'config', 'localization_params.yaml')],
+            [share('pashupati', 'config', 'localization_params.yaml')],
             {'tags_config_file': arg('tags')},
         ),
-        node('robot_perception', 'lidar_sector_node', [perception]),
+        node('dharma_perception', 'lidar_sector_node', [perception]),
         # the tag detector sees the camera at rate_hz only: localization needs a few poses/s
         node(
-            'robot_perception',
+            'dharma_perception',
             'image_throttle_node',
             [perception],
             name='apriltag_throttle',
@@ -231,16 +236,16 @@ def node(package, executable, defaults, overrides=None, remappings=None, **kwarg
             respawn_delay=3.0,
             output='screen',
         ),
-        node('robot_navigation', 'path_loader_node', [nav], {'waypoints_file': arg('route')}),
-        node('robot_navigation', 'obstacle_avoidance_node', [nav]),
+        node('dharma_navigation', 'path_loader_node', [nav], {'waypoints_file': arg('route')}),
+        node('dharma_navigation', 'obstacle_avoidance_node', [nav]),
         node(
-            'robot_navigation',
+            'dharma_navigation',
             'path_follower_node',
             [nav],
             remappings=[('cmd_vel', '/bridge/nav_cmd_vel'), ('battery', topic('battery'))],
         ),
         node(
-            'robot_bridge',
+            'dharma_control',
             'control_mux_node',
             [bridge],
             remappings=[('cmd_vel', topic('cmd_vel'))],
@@ -277,7 +282,7 @@ def node(package, executable, defaults, overrides=None, remappings=None, **kwarg
     else:
         actions.append(LogInfo(msg='NETRA uplink off: no cc_url (CC_URL)'))
     if truthy(arg('people')):
-        actions.append(node('robot_perception', 'person_detector_node', [perception]))
+        actions.append(node('pashupati_perception', 'person_detector_node', [perception]))
     if truthy(arg('inspection')):
         actions.append(
             node(
diff --git a/src/pashupati/package.xml b/src/pashupati/package.xml
index aec76f2..dcd9dac 100644
--- a/src/pashupati/package.xml
+++ b/src/pashupati/package.xml
@@ -11,10 +11,12 @@
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>apriltag_ros</exec_depend>
-  <exec_depend>robot_odometry</exec_depend>
-  <exec_depend>robot_localization</exec_depend>
-  <exec_depend>robot_perception</exec_depend>
-  <exec_depend>robot_navigation</exec_depend>
+  <exec_depend>dharma_odometry</exec_depend>
+  <exec_depend>dharma_localization</exec_depend>
+  <exec_depend>dharma_perception</exec_depend>
+  <exec_depend>pashupati_perception</exec_depend>
+  <exec_depend>dharma_control</exec_depend>
+  <exec_depend>dharma_navigation</exec_depend>
   <exec_depend>robot_bridge</exec_depend>
   <exec_depend>robot_fleet</exec_depend>
   <exec_depend>robot_inspection</exec_depend>
diff --git a/src/robot_perception/README.md b/src/pashupati_perception/README.md
similarity index 96%
rename from src/robot_perception/README.md
rename to src/pashupati_perception/README.md
index afdca9e..2a39e6d 100644
--- a/src/robot_perception/README.md
+++ b/src/pashupati_perception/README.md
@@ -1,4 +1,4 @@
-# robot_perception
+# pashupati_perception
 
 | Node | Output |
 |---|---|
@@ -33,7 +33,7 @@ crowd heatmap) while the robot is localized. `enabled`, `rate_hz` and `confidenc
 | `perception/people_status` | `std_msgs/String` (JSON, latched, every 2 s and per detection) | `{enabled, state (loading, running, off, error), error, backend, model, count, boxes ([x1, y1, x2, y2, score] as fractions of the image), placed, latency_ms, at, rate_hz}` |
 
 ```bash
-ros2 launch robot_perception perception.launch.py people:=true draw_tags:=false
+ros2 launch pashupati_perception perception.launch.py people:=true draw_tags:=false
 ```
 
 Parameters: [config/perception_params.yaml](config/perception_params.yaml).
diff --git a/src/robot_navigation/launch/obstacle_avoidance.launch.py b/src/pashupati_perception/launch/people.launch.py
similarity index 56%
rename from src/robot_navigation/launch/obstacle_avoidance.launch.py
rename to src/pashupati_perception/launch/people.launch.py
index 70d5afa..940c7fd 100644
--- a/src/robot_navigation/launch/obstacle_avoidance.launch.py
+++ b/src/pashupati_perception/launch/people.launch.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import os
 
 from ament_index_python.packages import get_package_share_directory
@@ -8,20 +9,19 @@
 
 
 def generate_launch_description():
+    """Launch the camera person detector (perception/people) with Pashupati's parameters."""
     default_params = os.path.join(
-        get_package_share_directory('robot_navigation'), 'config', 'navigation_params.yaml'
+        get_package_share_directory('pashupati'), 'config', 'perception_params.yaml'
     )
-    params_file = LaunchConfiguration('params_file')
-
     return LaunchDescription(
         [
             DeclareLaunchArgument('params_file', default_value=default_params),
             Node(
-                package='robot_navigation',
-                executable='obstacle_avoidance_node',
-                name='obstacle_avoidance_node',
+                package='pashupati_perception',
+                executable='person_detector_node',
+                name='person_detector_node',
                 output='screen',
-                parameters=[params_file],
+                parameters=[LaunchConfiguration('params_file')],
             ),
         ]
     )
diff --git a/src/robot_perception/package.xml b/src/pashupati_perception/package.xml
similarity index 81%
rename from src/robot_perception/package.xml
rename to src/pashupati_perception/package.xml
index bef6949..27dc201 100644
--- a/src/robot_perception/package.xml
+++ b/src/pashupati_perception/package.xml
@@ -1,15 +1,14 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>robot_perception</name>
+  <name>pashupati_perception</name>
   <version>0.2.0</version>
-  <description>FAST-LIO2 perception bringup and apriltag detection (perception/apriltag_detection).</description>
+  <description>Camera person detector (YOLO, ONNX) publishing perception/people for the handheld and NETRA.</description>
   <maintainer email="darmawandeveloper@gmail.com">Agus Darmawan</maintainer>
   <license>Apache-2.0</license>
 
   <exec_depend>rclpy</exec_depend>
   <exec_depend>rcl_interfaces</exec_depend>
-  <exec_depend>livox_ros_driver2</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
@@ -17,7 +16,7 @@
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>python3-transforms3d</exec_depend>
   <exec_depend>tf_transformations</exec_depend>
-  <exec_depend>robot_interfaces</exec_depend>
+  <exec_depend>dharma_interfaces</exec_depend>
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-opencv</exec_depend>
 
diff --git a/src/robot_localization/robot_localization/__init__.py b/src/pashupati_perception/pashupati_perception/__init__.py
similarity index 100%
rename from src/robot_localization/robot_localization/__init__.py
rename to src/pashupati_perception/pashupati_perception/__init__.py
diff --git a/src/robot_perception/robot_perception/detector.py b/src/pashupati_perception/pashupati_perception/detector.py
similarity index 100%
rename from src/robot_perception/robot_perception/detector.py
rename to src/pashupati_perception/pashupati_perception/detector.py
diff --git a/src/robot_perception/robot_perception/person_detector_node.py b/src/pashupati_perception/pashupati_perception/person_detector_node.py
similarity index 98%
rename from src/robot_perception/robot_perception/person_detector_node.py
rename to src/pashupati_perception/pashupati_perception/person_detector_node.py
index b07db72..4be72a8 100644
--- a/src/robot_perception/robot_perception/person_detector_node.py
+++ b/src/pashupati_perception/pashupati_perception/person_detector_node.py
@@ -6,6 +6,7 @@
 
 from geometry_msgs.msg import Pose, PoseArray
 import numpy as np
+from pashupati_perception.person_projection import feet_pixel, ground_point, pixel_ray
 from rcl_interfaces.msg import SetParametersResult
 import rclpy
 from rclpy.executors import ExternalShutdownException
@@ -13,7 +14,6 @@
 from rclpy.parameter import Parameter
 from rclpy.qos import qos_profile_sensor_data, QoSDurabilityPolicy, QoSProfile
 from rclpy.time import Time
-from robot_perception.person_projection import feet_pixel, ground_point, pixel_ray
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 from std_msgs.msg import String
 from tf2_ros import Buffer, TransformException, TransformListener
@@ -216,7 +216,7 @@ def ensure_detector(self) -> bool:
             return False
         self._load_tried = now
         try:
-            from robot_perception.detector import create_detector
+            from pashupati_perception.detector import create_detector
 
             self._detector = create_detector(
                 self._model_path, self._confidence, self._image_size, self._threads, self._backend
diff --git a/src/robot_perception/robot_perception/person_projection.py b/src/pashupati_perception/pashupati_perception/person_projection.py
similarity index 100%
rename from src/robot_perception/robot_perception/person_projection.py
rename to src/pashupati_perception/pashupati_perception/person_projection.py
diff --git a/src/robot_localization/resource/robot_localization b/src/pashupati_perception/resource/pashupati_perception
similarity index 100%
rename from src/robot_localization/resource/robot_localization
rename to src/pashupati_perception/resource/pashupati_perception
diff --git a/src/pashupati_perception/setup.cfg b/src/pashupati_perception/setup.cfg
new file mode 100644
index 0000000..dedae7e
--- /dev/null
+++ b/src/pashupati_perception/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pashupati_perception
+[install]
+install_scripts=$base/lib/pashupati_perception
diff --git a/src/robot_localization/setup.py b/src/pashupati_perception/setup.py
similarity index 64%
rename from src/robot_localization/setup.py
rename to src/pashupati_perception/setup.py
index 9e3c712..2274237 100644
--- a/src/robot_localization/setup.py
+++ b/src/pashupati_perception/setup.py
@@ -3,7 +3,7 @@
 
 from setuptools import find_packages, setup
 
-package_name = 'robot_localization'
+package_name = 'pashupati_perception'
 
 setup(
     name=package_name,
@@ -13,22 +13,17 @@
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
-        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
     maintainer='Agus Darmawan',
     maintainer_email='darmawandeveloper@gmail.com',
-    description='AprilTag-based map->odom localization stitched onto FAST-LIO TF',
+    description='Camera person detector (YOLO) for the handheld and NETRA',
     license='Apache-2.0',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
+    tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'localization_node = robot_localization.localization_node:main',
+            'person_detector_node = pashupati_perception.person_detector_node:main',
         ],
     },
 )
diff --git a/src/robot_localization/test/test_copyright.py b/src/pashupati_perception/test/test_copyright.py
similarity index 100%
rename from src/robot_localization/test/test_copyright.py
rename to src/pashupati_perception/test/test_copyright.py
diff --git a/src/robot_perception/test/test_detector.py b/src/pashupati_perception/test/test_detector.py
similarity index 96%
rename from src/robot_perception/test/test_detector.py
rename to src/pashupati_perception/test/test_detector.py
index c0370e5..41cf656 100644
--- a/src/robot_perception/test/test_detector.py
+++ b/src/pashupati_perception/test/test_detector.py
@@ -1,8 +1,8 @@
 """Person detector pre- and post-processing for Ultralytics YOLO ONNX exports (no model needed)."""
 
 import numpy as np
+from pashupati_perception.detector import letterbox, nms, OnnxPersonDetector, person_boxes
 import pytest
-from robot_perception.detector import letterbox, nms, OnnxPersonDetector, person_boxes
 
 
 def head(rows) -> np.ndarray:
diff --git a/src/robot_localization/test/test_flake8.py b/src/pashupati_perception/test/test_flake8.py
similarity index 100%
rename from src/robot_localization/test/test_flake8.py
rename to src/pashupati_perception/test/test_flake8.py
diff --git a/src/robot_localization/test/test_node_members.py b/src/pashupati_perception/test/test_node_members.py
similarity index 100%
rename from src/robot_localization/test/test_node_members.py
rename to src/pashupati_perception/test/test_node_members.py
diff --git a/src/robot_localization/test/test_pep257.py b/src/pashupati_perception/test/test_pep257.py
similarity index 100%
rename from src/robot_localization/test/test_pep257.py
rename to src/pashupati_perception/test/test_pep257.py
diff --git a/src/robot_perception/test/test_person_projection.py b/src/pashupati_perception/test/test_person_projection.py
similarity index 94%
rename from src/robot_perception/test/test_person_projection.py
rename to src/pashupati_perception/test/test_person_projection.py
index cb23e53..ab75c2b 100644
--- a/src/robot_perception/test/test_person_projection.py
+++ b/src/pashupati_perception/test/test_person_projection.py
@@ -1,7 +1,7 @@
 import math
 
 import numpy as np
-from robot_perception.person_projection import ground_point, pixel_ray
+from pashupati_perception.person_projection import ground_point, pixel_ray
 
 K = np.array([[600.0, 0.0, 320.0], [0.0, 600.0, 240.0], [0.0, 0.0, 1.0]])
 OPTICAL_TO_BODY = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]])
diff --git a/src/robot_bridge/README.md b/src/robot_bridge/README.md
index 9de0745..7d53386 100644
--- a/src/robot_bridge/README.md
+++ b/src/robot_bridge/README.md
@@ -17,7 +17,7 @@ velocity, and holds the robot still while the software e-stop is engaged.
 | pub | `cmd_vel` | `geometry_msgs/Twist` (remap to the robot, e.g. `/robot/cmd_vel`) |
 | pub | `bridge/mode` | `std_msgs/String`, latched |
 | pub | `bridge/estop_engaged`, `bridge/estop_reason` | `std_msgs/Bool`, `std_msgs/String`, latched |
-| srv | `bridge/set_mode` | `robot_interfaces/SetMode` (`auto` \| `remote`) |
+| srv | `bridge/set_mode` | `dharma_interfaces/SetMode` (`auto` \| `remote`) |
 | srv | `bridge/estop` | `std_srvs/SetBool`: `true` engages, `false` releases |
 
 In `remote` mode navigation is paused (and resumed when switching back). The robot gets one zero
@@ -74,7 +74,7 @@ MediaMTX serves WebRTC (WHEP) to the handheld. MediaMTX, its configuration and t
 
 ```bash
 ros2 launch robot_bridge bridge.launch.py robot_ns:=robot
-ros2 launch robot_navigation navigation.launch.py cmd_vel_topic:=/bridge/nav_cmd_vel ...
+ros2 launch dharma_navigation navigation.launch.py cmd_vel_topic:=/bridge/nav_cmd_vel ...
 ```
 
 Parameters: [config/bridge_params.yaml](config/bridge_params.yaml). For a lighter camera stream
diff --git a/src/robot_bridge/launch/bridge.launch.py b/src/robot_bridge/launch/bridge.launch.py
index 4a6a04a..d666409 100644
--- a/src/robot_bridge/launch/bridge.launch.py
+++ b/src/robot_bridge/launch/bridge.launch.py
@@ -18,7 +18,7 @@ def generate_launch_description():
             DeclareLaunchArgument('params_file', default_value=params),
             DeclareLaunchArgument('robot_ns', default_value='robot'),
             Node(
-                package='robot_bridge',
+                package='dharma_control',
                 executable='control_mux_node',
                 name='control_mux_node',
                 output='screen',
diff --git a/src/robot_bridge/package.xml b/src/robot_bridge/package.xml
index fbc6719..172e85a 100644
--- a/src/robot_bridge/package.xml
+++ b/src/robot_bridge/package.xml
@@ -15,7 +15,8 @@
   <exec_depend>std_srvs</exec_depend>
   <exec_depend>diagnostic_msgs</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>robot_interfaces</exec_depend>
+  <exec_depend>dharma_interfaces</exec_depend>
+  <exec_depend>dharma_control</exec_depend>
   <exec_depend>apriltag_msgs</exec_depend>
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-opencv</exec_depend>
diff --git a/src/robot_bridge/robot_bridge/control_mux_node.py b/src/robot_bridge/robot_bridge/control_mux_node.py
deleted file mode 100644
index 7e95468..0000000
--- a/src/robot_bridge/robot_bridge/control_mux_node.py
+++ /dev/null
@@ -1,263 +0,0 @@
-#!/usr/bin/env python3
-import math
-import time
-
-from geometry_msgs.msg import Twist
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSProfile
-from robot_bridge import protective_field
-from robot_bridge.mux import ControlMux, envelope, MODES
-from robot_interfaces.msg import SectorScan
-from robot_interfaces.srv import SetMode
-from std_msgs.msg import Bool, String
-from std_srvs.srv import SetBool
-
-
-class ControlMuxNode(Node):
-    """
-    The single gate between every velocity source and the robot.
-
-    Forwards navigation (auto) or teleop (remote) velocity, zeroes the robot when the active
-    source goes silent, holds it still while the software e-stop is engaged, and limits every
-    command to a speed the robot can still stop at before the nearest obstacle in its path
-    (protective field on perception/safety_scan). Nothing non-finite or outside the robot's
-    speed envelope (max_vx, max_vy, max_wz) is ever forwarded, whichever source sent it.
-    """
-
-    def __init__(self):
-        """Declare params, wire topics and the mode / e-stop services."""
-        super().__init__('control_mux_node')
-        self.declare_parameter('mode', 'auto')
-        self.declare_parameter('remote_timeout_sec', 0.3)
-        self.declare_parameter('nav_timeout_sec', 0.5)
-        self.declare_parameter('pause_nav_on_remote', True)
-        self.declare_parameter('estop_hold_hz', 10.0)
-        self.declare_parameter('max_vx', 1.0)
-        self.declare_parameter('max_vy', 0.5)
-        self.declare_parameter('max_wz', 1.5)
-        self.declare_parameter('field_in_auto', True)
-        self.declare_parameter('field_in_remote', False)
-        self.declare_parameter('safety_scan_timeout_sec', 0.5)
-        self.declare_parameter('stale_remote_speed', 0.15)
-        defaults = protective_field.FieldConfig()
-        for name in defaults.__dataclass_fields__:
-            self.declare_parameter(f'field.{name}', float(getattr(defaults, name)))
-        self._field = protective_field.FieldConfig(
-            **{
-                name: float(self.get_parameter(f'field.{name}').value)
-                for name in defaults.__dataclass_fields__
-            }
-        )
-        self._field_modes = {
-            'auto': bool(self.get_parameter('field_in_auto').value),
-            'remote': bool(self.get_parameter('field_in_remote').value),
-        }
-        self._scan_timeout = float(self.get_parameter('safety_scan_timeout_sec').value)
-        self._stale_remote_speed = float(self.get_parameter('stale_remote_speed').value)
-        self._envelope = tuple(
-            abs(float(self.get_parameter(name).value)) for name in ('max_vx', 'max_vy', 'max_wz')
-        )
-        self._obstacles = []
-        self._scan_time = None
-        self._field_state = None
-        self._mux = ControlMux(
-            self.get_parameter('mode').value,
-            float(self.get_parameter('remote_timeout_sec').value),
-            float(self.get_parameter('nav_timeout_sec').value),
-        )
-        self._pause_nav = bool(self.get_parameter('pause_nav_on_remote').value)
-        self._paused_by_us = False
-
-        latched = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
-        self._cmd_pub = self.create_publisher(Twist, 'cmd_vel', 10)
-        self._mode_pub = self.create_publisher(String, 'bridge/mode', latched)
-        self._estop_pub = self.create_publisher(Bool, 'bridge/estop_engaged', latched)
-        self._estop_reason_pub = self.create_publisher(String, 'bridge/estop_reason', latched)
-        self._field_pub = self.create_publisher(String, 'bridge/protective_field', latched)
-        self.create_subscription(SectorScan, 'perception/safety_scan', self.scan_callback, 10)
-        self.create_subscription(Twist, 'bridge/nav_cmd_vel', self.nav_callback, 10)
-        self.create_subscription(Twist, 'bridge/remote_cmd_vel', self.remote_callback, 10)
-        self.create_subscription(String, 'bridge/estop_request', self.estop_request_callback, 10)
-        self.create_service(SetMode, 'bridge/set_mode', self.set_mode_callback)
-        self.create_service(SetBool, 'bridge/estop', self.estop_callback)
-        self._pause_client = self.create_client(SetBool, 'navigation/pause')
-        self.create_timer(0.05, self.watchdog)
-        self.create_timer(1.0 / float(self.get_parameter('estop_hold_hz').value), self.estop_hold)
-        self.publish_mode()
-        self.publish_estop()
-
-    def now(self) -> float:
-        """Return seconds from the node clock."""
-        return self.get_clock().now().nanoseconds / 1e9
-
-    def publish_mode(self):
-        """Publish the current mode (latched)."""
-        self._mode_pub.publish(String(data=self._mux.mode))
-
-    def nav_callback(self, msg: Twist):
-        """Forward navigation commands in auto mode."""
-        out = self._mux.on_nav(msg, self.now())
-        if out is not None:
-            self._cmd_pub.publish(self.gate(out))
-
-    def remote_callback(self, msg: Twist):
-        """Forward teleop commands in remote mode."""
-        out = self._mux.on_remote(msg, self.now())
-        if out is not None:
-            self._cmd_pub.publish(self.gate(out))
-
-    def gate(self, cmd: Twist) -> Twist:
-        """Last checks before the robot: finite values, speed envelope, protective field."""
-        rest = (cmd.linear.z, cmd.angular.x, cmd.angular.y)
-        limited = envelope(cmd.linear.x, cmd.linear.y, cmd.angular.z, *self._envelope)
-        if limited is None or not all(math.isfinite(v) for v in rest):
-            self.get_logger().error(
-                f'dropped a non-finite {self._mux.mode} command', throttle_duration_sec=1.0
-            )
-            return Twist()
-        if limited != (cmd.linear.x, cmd.linear.y, cmd.angular.z):
-            self.get_logger().warn(
-                f'{self._mux.mode} command above the speed envelope, scaled down',
-                throttle_duration_sec=5.0,
-            )
-            cmd.linear.x, cmd.linear.y, cmd.angular.z = limited
-        return self.apply_field(cmd)
-
-    def scan_callback(self, msg: SectorScan):
-        """Keep the obstacle points of the latest 360 deg safety scan."""
-        self._obstacles = protective_field.scan_points(
-            msg.angle_min, msg.angle_increment, msg.ranges, msg.range_max
-        )
-        self._scan_time = time.monotonic()
-
-    def apply_field(self, cmd: Twist) -> Twist:
-        """Scale the linear velocity so the robot can stop before what is in its path."""
-        if not self._field_modes.get(self._mux.mode, False):
-            self.set_field_state('off')
-            return cmd
-        stale = self._scan_time is None or time.monotonic() - self._scan_time > self._scan_timeout
-        if stale:
-            self.set_field_state('stale')
-            if self._mux.mode != 'remote':
-                return Twist()  # navigation never drives blind
-            speed = math.hypot(cmd.linear.x, cmd.linear.y)
-            scale = min(1.0, self._stale_remote_speed / speed) if speed > 0.0 else 1.0
-        else:
-            result = protective_field.limit(
-                self._field, cmd.linear.x, cmd.linear.y, self._obstacles
-            )
-            self.set_field_state(result.state)
-            scale = result.scale
-        if scale < 1.0:
-            cmd.linear.x *= scale
-            cmd.linear.y *= scale
-        return cmd
-
-    def set_field_state(self, state: str):
-        """Publish and log protective-field state changes (latched)."""
-        if state == self._field_state:
-            return
-        if state in ('stop', 'stale'):
-            self.get_logger().warn(f'protective field: {state}')
-        else:
-            self.get_logger().info(f'protective field: {state}')
-        self._field_state = state
-        self._field_pub.publish(String(data=state))
-
-    def watchdog(self):
-        """Send one zero command when the active source goes silent or the mode just changed."""
-        if self._mux.watchdog(self.now()):
-            self._cmd_pub.publish(Twist())
-
-    def estop_hold(self):
-        """Keep sending zero while the e-stop is engaged (drivers with a long command timeout)."""
-        if self._mux.estop:
-            self._cmd_pub.publish(Twist())
-
-    def publish_estop(self):
-        """Publish the e-stop state and reason (latched)."""
-        self._estop_pub.publish(Bool(data=self._mux.estop))
-        self._estop_reason_pub.publish(String(data=self._mux.estop_reason))
-
-    def engage(self, reason: str) -> bool:
-        """Engage the e-stop: zero now, pause navigation, latch until released."""
-        changed = self._mux.engage_estop(reason)
-        self._cmd_pub.publish(Twist())
-        if changed:
-            self.get_logger().error(f'E-STOP engaged ({self._mux.estop_reason})')
-            self.call_pause(True)
-            self._paused_by_us = False  # a mode switch must not resume after an e-stop
-            self.publish_estop()
-        return changed
-
-    def estop_callback(self, request, response):
-        """Service: data=true engages the e-stop, data=false releases it."""
-        if request.data:
-            self.engage('service')
-            response.message = f'e-stop engaged ({self._mux.estop_reason})'
-        elif self._mux.release_estop():
-            self._cmd_pub.publish(Twist())
-            self.publish_estop()
-            self.get_logger().warn('E-STOP released, navigation stays paused until resumed')
-            response.message = 'e-stop released; resume navigation with navigation/pause false'
-        else:
-            response.message = 'e-stop was not engaged'
-        response.success = True
-        return response
-
-    def estop_request_callback(self, msg: String):
-        """Topic for any node (teleop, fleet) to engage the e-stop with a reason, no reply."""
-        self.engage(msg.data or 'topic')
-
-    def set_mode_callback(self, request, response):
-        """Switch mode, pausing navigation while the remote drives."""
-        if request.mode not in MODES:
-            response.success = False
-            response.message = f'unknown mode {request.mode!r}, use {list(MODES)}'
-            return response
-        changed = self._mux.set_mode(request.mode)
-        self._cmd_pub.publish(Twist())
-        if changed and self._pause_nav:
-            if request.mode == 'remote':
-                self._paused_by_us = self.call_pause(True)
-            elif self._paused_by_us:
-                self.call_pause(False)
-                self._paused_by_us = False
-        self.publish_mode()
-        self.get_logger().info(f'control mode: {request.mode}')
-        response.success = True
-        response.message = f'mode {request.mode}'
-        return response
-
-    def call_pause(self, pause: bool) -> bool:
-        """Ask navigation to pause or resume without waiting; False when it is not running."""
-        if not self._pause_client.service_is_ready():
-            return False
-        self._pause_client.call_async(SetBool.Request(data=pause))
-        return True
-
-
-def main(args=None):
-    """Spin the control mux node."""
-    rclpy.init(args=args)
-    node = ControlMuxNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_bridge/robot_bridge/mux.py b/src/robot_bridge/robot_bridge/mux.py
deleted file mode 100644
index b1c57fd..0000000
--- a/src/robot_bridge/robot_bridge/mux.py
+++ /dev/null
@@ -1,94 +0,0 @@
-import math
-
-MODES = ('auto', 'remote')
-
-
-def envelope(vx: float, vy: float, wz: float, max_vx: float, max_vy: float, max_wz: float):
-    """
-    Return (vx, vy, wz) inside the robot's speed envelope, or None if a value is not finite.
-
-    Linear x and y are scaled together, so the direction of travel is kept; the yaw rate is
-    clamped on its own.
-    """
-    if not all(math.isfinite(v) for v in (vx, vy, wz)):
-        return None
-    scale = 1.0
-    if abs(vx) > max_vx:
-        scale = min(scale, max_vx / abs(vx))
-    if abs(vy) > max_vy:
-        scale = min(scale, max_vy / abs(vy))
-    return vx * scale, vy * scale, max(-max_wz, min(max_wz, wz))
-
-
-class ControlMux:
-    """
-    Chooses which velocity command reaches the robot: navigation (auto) or teleop (remote).
-
-    A latched software e-stop overrides both: while it is engaged nothing but zero reaches the
-    robot, and only an explicit release clears it. The watchdog zeroes the robot once when the
-    active source goes silent (teleop after remote_timeout, navigation after nav_timeout).
-    """
-
-    def __init__(self, mode: str = 'auto', remote_timeout: float = 0.3, nav_timeout: float = 0.5):
-        if mode not in MODES:
-            raise ValueError(f'mode must be one of {MODES}')
-        self.mode = mode
-        self._remote_timeout = remote_timeout
-        self._nav_timeout = nav_timeout
-        self._stamp = None  # last forwarded command time of the active source
-        self._stale_sent = True
-        self.estop = False
-        self.estop_reason = ''
-
-    def set_mode(self, mode: str) -> bool:
-        """Switch mode; returns True when it changed."""
-        if mode not in MODES:
-            raise ValueError(f'mode must be one of {MODES}')
-        changed = mode != self.mode
-        self.mode = mode
-        self._stamp = None
-        self._stale_sent = False
-        return changed
-
-    def engage_estop(self, reason: str) -> bool:
-        """Latch the e-stop; returns True when it was not engaged yet."""
-        changed = not self.estop
-        self.estop = True
-        self.estop_reason = reason or 'unspecified'
-        self._stale_sent = False
-        return changed
-
-    def release_estop(self) -> bool:
-        """Clear the e-stop; returns True when it was engaged."""
-        changed = self.estop
-        self.estop = False
-        self.estop_reason = ''
-        self._stamp = None
-        return changed
-
-    def on_nav(self, cmd, now: float = 0.0):
-        """Return the command to forward for a navigation command, or None to drop it."""
-        if self.estop or self.mode != 'auto':
-            return None
-        self._stamp = now
-        self._stale_sent = False
-        return cmd
-
-    def on_remote(self, cmd, now: float):
-        """Return the command to forward for a teleop command, or None to drop it."""
-        if self.estop or self.mode != 'remote':
-            return None
-        self._stamp = now
-        self._stale_sent = False
-        return cmd
-
-    def watchdog(self, now: float) -> bool:
-        """Return True once when the active source went silent, after a switch or an e-stop."""
-        if self._stale_sent:
-            return False
-        if not self.estop and self._stamp is not None:
-            timeout = self._remote_timeout if self.mode == 'remote' else self._nav_timeout
-            if now - self._stamp < timeout:
-                return False
-        self._stale_sent = True
-        return True
diff --git a/src/robot_bridge/robot_bridge/protective_field.py b/src/robot_bridge/robot_bridge/protective_field.py
deleted file mode 100644
index 5991481..0000000
--- a/src/robot_bridge/robot_bridge/protective_field.py
+++ /dev/null
@@ -1,102 +0,0 @@
-"""
-Speed and separation monitoring: limit a velocity command to what the robot can stop within.
-
-Pure math, no ROS. The robot is a rectangle around base_link; for the direction it is about to
-move in, the free distance from its edge to the nearest obstacle in its swept path (from a 360 deg
-sector scan) bounds the speed so that, after a reaction time and braking at max_decel, it still
-stops `margin` short of the obstacle:
-
-    v * t_react + v^2 / (2 a) <= free - margin
-"""
-
-from dataclasses import dataclass
-import math
-
-
-@dataclass
-class FieldConfig:
-    """Robot footprint and stopping model, all in metres, seconds and m/s^2."""
-
-    front: float = 0.40          # base_link to the front edge
-    rear: float = 0.40           # base_link to the rear edge
-    half_width: float = 0.28     # base_link to the side edges
-    max_decel: float = 0.6       # braking the robot reliably achieves
-    reaction_time: float = 0.25  # sensing + command latency
-    margin: float = 0.15         # gap left between the robot and the obstacle
-    lateral_margin: float = 0.10  # extra width of the swept path
-    min_speed: float = 0.02      # m/s, slower commands are not limited (creeping, turning)
-
-
-@dataclass
-class FieldResult:
-    """Outcome of one check."""
-
-    scale: float      # 0..1 applied to the linear velocity
-    clearance: float  # m from the robot edge to the nearest obstacle in the path, inf if none
-    state: str        # clear | limited | stop
-
-
-def footprint_extent(cfg: FieldConfig, ux: float, uy: float) -> tuple[float, float]:
-    """Return (distance from base_link to the edge along u, half width across u) of the box."""
-    length = cfg.front if ux >= 0.0 else cfg.rear
-    along = abs(ux) * length + abs(uy) * cfg.half_width
-    across = abs(uy) * length + abs(ux) * cfg.half_width
-    return along, across
-
-
-def path_clearance(cfg: FieldConfig, ux: float, uy: float, points) -> float:
-    """
-    Free distance from the robot edge to the nearest point in the swept path along (ux, uy).
-
-    Ahead of the leading edge the path is the robot width plus lateral_margin on each side.
-    Beside the robot only points inside the footprint itself count (negative clearance): the
-    robot slides past anything next to it.
-    """
-    along_edge, half = footprint_extent(cfg, ux, uy)
-    clearance = math.inf
-    for x, y in points:
-        along = x * ux + y * uy
-        across = abs(-x * uy + y * ux)
-        if along <= 0.0:
-            continue
-        if along >= along_edge:
-            if across <= half + cfg.lateral_margin:
-                clearance = min(clearance, along - along_edge)
-        elif across <= half:
-            clearance = min(clearance, along - along_edge)
-    return clearance
-
-
-def max_speed(cfg: FieldConfig, clearance: float) -> float:
-    """Highest speed that still stops `margin` before an obstacle `clearance` away."""
-    room = clearance - cfg.margin
-    if room <= 0.0:
-        return 0.0
-    if math.isinf(room):
-        return math.inf
-    a, t = cfg.max_decel, cfg.reaction_time
-    return a * (-t + math.sqrt(t * t + 2.0 * room / a))
-
-
-def scan_points(angle_min: float, angle_increment: float, ranges, range_max: float):
-    """Return the (x, y) sector centres with an obstacle (range below range_max)."""
-    out = []
-    for i, r in enumerate(ranges):
-        if r < range_max - 1e-3:
-            theta = angle_min + (i + 0.5) * angle_increment
-            out.append((r * math.cos(theta), r * math.sin(theta)))
-    return out
-
-
-def limit(cfg: FieldConfig, vx: float, vy: float, points) -> FieldResult:
-    """Return the scale for a planar velocity (vx, vy) given obstacle points in base_link."""
-    speed = math.hypot(vx, vy)
-    if speed < cfg.min_speed:
-        return FieldResult(1.0, math.inf, 'clear')
-    clearance = path_clearance(cfg, vx / speed, vy / speed, points)
-    allowed = max_speed(cfg, clearance)
-    if allowed >= speed:
-        return FieldResult(1.0, clearance, 'clear')
-    if allowed <= cfg.min_speed:
-        return FieldResult(0.0, clearance, 'stop')
-    return FieldResult(allowed / speed, clearance, 'limited')
diff --git a/src/robot_bridge/robot_bridge/teleop_udp_node.py b/src/robot_bridge/robot_bridge/teleop_udp_node.py
index f4765fc..ec272b2 100644
--- a/src/robot_bridge/robot_bridge/teleop_udp_node.py
+++ b/src/robot_bridge/robot_bridge/teleop_udp_node.py
@@ -10,6 +10,8 @@
 import threading
 import time
 
+from dharma_interfaces.msg import MissionStatus, NavigationStatus, WaypointPath
+from dharma_interfaces.srv import LoadPath, MarkStopPoint, SetMode
 from diagnostic_msgs.msg import DiagnosticArray
 from geometry_msgs.msg import Twist
 import numpy as np
@@ -22,8 +24,6 @@
 from rclpy.qos import qos_profile_sensor_data, QoSDurabilityPolicy, QoSProfile
 from rclpy.time import Time
 from robot_bridge import protocol
-from robot_interfaces.msg import MissionStatus, NavigationStatus, WaypointPath
-from robot_interfaces.srv import LoadPath, MarkStopPoint, SetMode
 from sensor_msgs.msg import BatteryState, CameraInfo, CompressedImage, Image, PointCloud2
 from std_msgs.msg import Bool, String
 from std_srvs.srv import SetBool, Trigger
diff --git a/src/robot_bridge/setup.py b/src/robot_bridge/setup.py
index 927c6bb..a1e88c8 100644
--- a/src/robot_bridge/setup.py
+++ b/src/robot_bridge/setup.py
@@ -24,7 +24,6 @@
     license='Apache-2.0',
     entry_points={
         'console_scripts': [
-            'control_mux_node = robot_bridge.control_mux_node:main',
             'teleop_udp_node = robot_bridge.teleop_udp_node:main',
             'video_stream_node = robot_bridge.video_stream_node:main',
         ],
diff --git a/src/robot_bridge/test/test_mux.py b/src/robot_bridge/test/test_mux.py
deleted file mode 100644
index 92a95d5..0000000
--- a/src/robot_bridge/test/test_mux.py
+++ /dev/null
@@ -1,64 +0,0 @@
-import pytest
-from robot_bridge.mux import ControlMux, envelope
-
-
-def test_auto_forwards_navigation_only():
-    """Auto mode passes navigation and drops teleop."""
-    mux = ControlMux('auto')
-    assert mux.on_nav('nav') == 'nav'
-    assert mux.on_remote('joy', 0.0) is None
-
-
-def test_remote_forwards_teleop_only_and_stops_when_silent():
-    """Remote mode passes teleop, drops navigation and zeroes once after the timeout."""
-    mux = ControlMux('auto', remote_timeout=0.3)
-    assert mux.set_mode('remote')
-    assert mux.watchdog(0.0)  # zero right after the switch
-    assert mux.on_nav('nav') is None
-    assert mux.on_remote('joy', 1.0) == 'joy'
-    assert not mux.watchdog(1.2)
-    assert mux.watchdog(1.4)
-    assert not mux.watchdog(1.5)
-
-
-def test_unknown_mode_rejected():
-    """Only auto and remote exist."""
-    with pytest.raises(ValueError):
-        ControlMux('manual')
-
-
-def test_estop_blocks_both_sources_until_released():
-    """Nothing but zero passes while the e-stop is latched; release restores forwarding."""
-    mux = ControlMux('auto')
-    assert mux.engage_estop('test')
-    assert not mux.engage_estop('again')  # already engaged
-    assert mux.watchdog(0.0)  # one zero right away
-    assert mux.on_nav('nav', 0.1) is None
-    mux.set_mode('remote')
-    assert mux.on_remote('joy', 0.2) is None
-    assert mux.estop and mux.estop_reason == 'again'
-    assert mux.release_estop()
-    assert not mux.release_estop()
-    assert mux.on_remote('joy', 0.3) == 'joy'
-
-
-def test_navigation_silence_zeroes_once():
-    """In auto mode a silent navigation source gets exactly one zero command."""
-    mux = ControlMux('auto', nav_timeout=0.5)
-    mux.watchdog(0.0)  # settle the initial zero
-    assert mux.on_nav('nav', 1.0) == 'nav'
-    assert not mux.watchdog(1.3)
-    assert mux.watchdog(1.6)
-    assert not mux.watchdog(2.0)
-
-
-def test_envelope_keeps_direction_and_rejects_non_finite():
-    """Over-limit commands are scaled down without turning; NaN / inf are rejected."""
-    assert envelope(0.4, 0.1, 0.5, 1.0, 0.5, 1.5) == (0.4, 0.1, 0.5)
-    vx, vy, wz = envelope(2.0, 1.0, -3.0, 1.0, 0.5, 1.5)
-    assert (vx, vy, wz) == (1.0, 0.5, -1.5)  # both halved: same heading
-    vx, vy, _ = envelope(0.5, -1.0, 0.0, 1.0, 0.5, 1.5)
-    assert (vx, vy) == (0.25, -0.5)
-    for bad in (float('nan'), float('inf'), -float('inf')):
-        assert envelope(bad, 0.0, 0.0, 1.0, 0.5, 1.5) is None
-        assert envelope(0.0, 0.0, bad, 1.0, 0.5, 1.5) is None
diff --git a/src/robot_bridge/test/test_mux_node.py b/src/robot_bridge/test/test_mux_node.py
deleted file mode 100644
index 7039802..0000000
--- a/src/robot_bridge/test/test_mux_node.py
+++ /dev/null
@@ -1,197 +0,0 @@
-"""In-process tests of control_mux_node: e-stop, forwarding and the protective field."""
-
-import math
-import time
-
-import pytest
-
-rclpy = pytest.importorskip('rclpy')
-Twist = pytest.importorskip('geometry_msgs.msg').Twist
-std_msgs = pytest.importorskip('std_msgs.msg')
-SetBool = pytest.importorskip('std_srvs.srv').SetBool
-SectorScan = pytest.importorskip('robot_interfaces.msg').SectorScan
-SetMode = pytest.importorskip('robot_interfaces.srv').SetMode
-qos = pytest.importorskip('rclpy.qos')
-SingleThreadedExecutor = pytest.importorskip('rclpy.executors').SingleThreadedExecutor
-ControlMuxNode = pytest.importorskip('robot_bridge.control_mux_node').ControlMuxNode
-
-
-class Rig:
-    """The mux, a fake navigation pause service and recorders for its outputs."""
-
-    def __init__(self):
-        rclpy.init()
-        self.mux = ControlMuxNode()
-        self.helper = rclpy.create_node('mux_test_helper')
-        self.out, self.engaged, self.pauses = [], [], []
-        self.helper.create_subscription(Twist, 'cmd_vel', self.out.append, 50)
-        latched = qos.QoSProfile(depth=1, durability=qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)
-        self.helper.create_subscription(
-            std_msgs.Bool, 'bridge/estop_engaged', lambda m: self.engaged.append(m.data), latched
-        )
-        self.helper.create_service(SetBool, 'navigation/pause', self.on_pause)
-        self.nav = self.helper.create_publisher(Twist, 'bridge/nav_cmd_vel', 10)
-        self.remote = self.helper.create_publisher(Twist, 'bridge/remote_cmd_vel', 10)
-        self.set_mode = self.helper.create_client(SetMode, 'bridge/set_mode')
-        self.request = self.helper.create_publisher(std_msgs.String, 'bridge/estop_request', 10)
-        self.estop = self.helper.create_client(SetBool, 'bridge/estop')
-        self.field_states = []
-        self.helper.create_subscription(
-            std_msgs.String,
-            'bridge/protective_field',
-            lambda m: self.field_states.append(m.data),
-            latched,
-        )
-        self.scan_pub = self.helper.create_publisher(SectorScan, 'perception/safety_scan', 10)
-        self.obstacles = {}  # sector index -> range
-        self.scanning = True
-        self.helper.create_timer(0.1, self.publish_scan)
-        self.executor = SingleThreadedExecutor()
-        self.executor.add_node(self.mux)
-        self.executor.add_node(self.helper)
-
-    def publish_scan(self):
-        """Publish a 72-sector safety scan with the configured obstacles."""
-        if not self.scanning:
-            return
-        msg = SectorScan()
-        msg.num_sectors = 72
-        msg.angle_min = -math.pi
-        msg.angle_increment = 2 * math.pi / 72
-        msg.range_max = 4.0
-        msg.ranges = [self.obstacles.get(i, 4.0) for i in range(72)]
-        self.scan_pub.publish(msg)
-
-    def on_pause(self, request, response):
-        """Record navigation/pause calls."""
-        self.pauses.append(request.data)
-        response.success = True
-        return response
-
-    def spin(self, seconds: float):
-        """Spin both nodes."""
-        end = time.monotonic() + seconds
-        while time.monotonic() < end:
-            self.executor.spin_once(timeout_sec=0.01)
-
-    def call(self, data: bool):
-        """Call bridge/estop and wait for the answer."""
-        return self.wait(self.estop.call_async(SetBool.Request(data=data)))
-
-    def mode(self, mode: str):
-        """Call bridge/set_mode and wait for the answer."""
-        return self.wait(self.set_mode.call_async(SetMode.Request(mode=mode)))
-
-    def wait(self, future):
-        """Spin until a service answer arrives (3 s at most)."""
-        end = time.monotonic() + 3.0
-        while not future.done() and time.monotonic() < end:
-            self.executor.spin_once(timeout_sec=0.01)
-        return future.result()
-
-    def drive(self, vx: float, seconds: float = 0.3, vy: float = 0.0):
-        """Publish a navigation command and spin."""
-        cmd = Twist()
-        cmd.linear.x, cmd.linear.y = vx, vy
-        self.nav.publish(cmd)
-        self.spin(seconds)
-
-    def close(self):
-        """Tear down."""
-        self.executor.shutdown()
-        self.helper.destroy_node()
-        self.mux.destroy_node()
-        rclpy.shutdown()
-
-
-@pytest.fixture
-def rig():
-    """Provide a running mux."""
-    r = Rig()
-    r.spin(0.5)  # discovery
-    yield r
-    r.close()
-
-
-def test_estop_service_holds_the_robot_still(rig):
-    """Engaged: nav is blocked, zero is streamed, navigation paused; release lets nav through."""
-    rig.drive(0.4)
-    assert rig.out and rig.out[-1].linear.x == 0.4
-    assert rig.call(True).success
-    rig.spin(0.3)
-    assert rig.engaged[-1] is True and rig.pauses == [True]
-    rig.out.clear()
-    rig.drive(0.4, 0.5)
-    assert rig.out and all(t.linear.x == 0.0 for t in rig.out)  # only the hold stream
-    assert rig.call(False).success
-    rig.spin(0.2)
-    assert rig.engaged[-1] is False and rig.pauses == [True]  # release never resumes
-    rig.drive(0.3)
-    assert rig.out[-1].linear.x == 0.3
-
-
-def test_estop_request_topic_engages(rig):
-    """Any node can engage the e-stop with a reason on bridge/estop_request."""
-    rig.request.publish(std_msgs.String(data='unit test'))
-    rig.spin(0.3)
-    assert rig.engaged[-1] is True
-    assert rig.mux._mux.estop_reason == 'unit test'
-
-
-def test_field_stops_forward_motion_toward_an_obstacle(rig):
-    """An obstacle 0.5 m ahead stops forward motion, sideways motion still passes."""
-    rig.obstacles = {36: 0.5}  # sector 36 is straight ahead (0 deg)
-    rig.spin(0.3)
-    rig.drive(0.4)
-    assert rig.out[-1].linear.x == 0.0 and rig.field_states[-1] == 'stop'
-    rig.drive(0.0, vy=0.3)
-    assert abs(rig.out[-1].linear.y - 0.3) < 1e-6
-    rig.obstacles = {}
-    rig.spin(0.3)
-    rig.drive(0.4)
-    assert abs(rig.out[-1].linear.x - 0.4) < 1e-6 and rig.field_states[-1] == 'clear'
-
-
-def test_remote_is_not_limited_by_the_field(rig):
-    """REMOTE: an obstacle ahead limits nothing, the operator watches the robot."""
-    rig.obstacles = {36: 0.5}
-    rig.spin(0.3)
-    assert rig.mode('remote').success
-    cmd = Twist()
-    cmd.linear.x, cmd.angular.z = 0.4, 0.8
-    rig.remote.publish(cmd)
-    rig.spin(0.15)  # within remote_timeout_sec, before the watchdog zeroes the robot
-    moving = [t for t in rig.out if t.linear.x > 0.0]
-    assert moving and abs(moving[-1].linear.x - 0.4) < 1e-6
-    assert abs(moving[-1].angular.z - 0.8) < 1e-6
-    assert rig.field_states[-1] == 'off'
-
-
-def test_field_stops_navigation_without_a_fresh_scan(rig):
-    """No safety scan: navigation never drives blind."""
-    rig.scanning = False
-    rig.spin(0.8)
-    rig.drive(0.4)
-    assert rig.out[-1].linear.x == 0.0 and rig.field_states[-1] == 'stale'
-
-
-def test_non_finite_commands_never_reach_the_robot(rig):
-    """A NaN or inf from any source becomes zero, even with nothing in the way."""
-    rig.drive(0.3)
-    assert abs(rig.out[-1].linear.x - 0.3) < 1e-6
-    for bad in (float('nan'), float('inf')):
-        rig.drive(bad)
-        out = rig.out[-1]
-        assert (out.linear.x, out.linear.y, out.angular.z) == (0.0, 0.0, 0.0)
-    cmd = Twist()
-    cmd.linear.x, cmd.angular.y = 0.3, float('nan')  # a field the gate does not limit
-    rig.nav.publish(cmd)
-    rig.spin(0.3)
-    assert rig.out[-1].linear.x == 0.0
-
-
-def test_commands_are_capped_at_the_speed_envelope(rig):
-    """2 m/s forward + 1 m/s left becomes 1.0 / 0.5 m/s: the same heading, within the limits."""
-    rig.drive(2.0, vy=1.0)
-    out = rig.out[-1]
-    assert abs(out.linear.x - 1.0) < 1e-6 and abs(out.linear.y - 0.5) < 1e-6
diff --git a/src/robot_bridge/test/test_protective_field.py b/src/robot_bridge/test/test_protective_field.py
deleted file mode 100644
index 0f330d8..0000000
--- a/src/robot_bridge/test/test_protective_field.py
+++ /dev/null
@@ -1,84 +0,0 @@
-import math
-
-from robot_bridge.protective_field import (
-    FieldConfig,
-    footprint_extent,
-    limit,
-    max_speed,
-    path_clearance,
-    scan_points,
-)
-
-CFG = FieldConfig()
-
-
-def wall(x0: float, y_from: float = -2.0, y_to: float = 2.0, step: float = 0.05):
-    """Points of a wall across the robot at x = x0."""
-    n = int((y_to - y_from) / step) + 1
-    return [(x0, y_from + i * step) for i in range(n)]
-
-
-def test_footprint_extent_is_the_box_support():
-    """Forward: front edge; sideways: half width; diagonal: projected box."""
-    assert footprint_extent(CFG, 1.0, 0.0) == (0.40, 0.28)
-    assert footprint_extent(CFG, 0.0, 1.0) == (0.28, 0.40)
-    along, _ = footprint_extent(CFG, math.sqrt(0.5), math.sqrt(0.5))
-    assert abs(along - (0.40 + 0.28) * math.sqrt(0.5)) < 1e-9
-
-
-def test_max_speed_inverts_the_stopping_distance():
-    """At the returned speed, reaction + braking distance equals the free room."""
-    for clearance in (0.3, 0.6, 1.5, 3.0):
-        v = max_speed(CFG, clearance)
-        room = clearance - CFG.margin
-        stopping = v * CFG.reaction_time + v * v / (2.0 * CFG.max_decel)
-        assert abs(stopping - room) < 1e-9
-    assert max_speed(CFG, CFG.margin) == 0.0
-    assert max_speed(CFG, math.inf) == math.inf
-
-
-def test_free_path_is_clear():
-    """Nothing in the way: full speed in every direction."""
-    for vx, vy in ((0.6, 0.0), (0.0, 0.4), (-0.3, 0.0)):
-        assert limit(CFG, vx, vy, []).state == 'clear'
-
-
-def test_wall_ahead_limits_then_stops_forward_motion_only():
-    """A wall 1.2 m ahead limits a fast forward command, 0.5 m stops it; sideways stays free."""
-    far = limit(CFG, 1.0, 0.0, wall(1.2))
-    assert far.state == 'limited' and 0.0 < far.scale < 1.0
-    assert abs(far.clearance - 0.8) < 1e-9
-    near = limit(CFG, 0.3, 0.0, wall(0.5))
-    assert near.state == 'stop' and near.scale == 0.0
-    assert limit(CFG, 0.0, 0.3, wall(0.6)).state == 'clear'
-    assert limit(CFG, -0.3, 0.0, wall(0.5)).state == 'clear'  # backing away
-
-
-def test_reverse_and_sideways_are_protected_too():
-    """Obstacles behind or beside the robot limit motion toward them (holonomic approach)."""
-    assert limit(CFG, -0.3, 0.0, [(-0.6, 0.0)]).state == 'limited'  # 0.2 m behind the edge
-    assert limit(CFG, -0.3, 0.0, [(-0.54, 0.0)]).state == 'stop'  # inside the margin
-    left = [(0.0, 0.42)]
-    assert limit(CFG, 0.0, 0.3, left).state == 'stop'
-    assert limit(CFG, 0.3, 0.0, left).state == 'clear'
-
-
-def test_point_inside_the_footprint_blocks_motion_toward_it():
-    """Something already touching the robot stops motion toward it."""
-    assert path_clearance(CFG, 1.0, 0.0, [(0.3, 0.0)]) < 0.0
-    assert limit(CFG, 0.2, 0.0, [(0.3, 0.0)]).scale == 0.0
-
-
-def test_scan_points_skip_empty_sectors():
-    """Only sectors that saw something become points, at the sector centre."""
-    pts = scan_points(-math.pi, math.pi / 2, [4.0, 1.0, 4.0, 2.0], 4.0)
-    assert len(pts) == 2
-    x, y = pts[0]
-    assert abs(math.atan2(y, x) - (-math.pi / 4)) < 1e-9 and abs(math.hypot(x, y) - 1.0) < 1e-9
-
-
-def test_sliding_past_something_beside_the_robot_is_allowed():
-    """An obstacle just in front of the nose does not block sideways motion."""
-    nose = [(0.48, 0.02)]
-    assert limit(CFG, 0.0, 0.3, nose).state == 'clear'
-    assert limit(CFG, 0.3, 0.0, nose).state == 'stop'
diff --git a/src/robot_fleet/package.xml b/src/robot_fleet/package.xml
index 779cec5..6e26516 100644
--- a/src/robot_fleet/package.xml
+++ b/src/robot_fleet/package.xml
@@ -15,7 +15,7 @@
   <exec_depend>std_srvs</exec_depend>
   <exec_depend>diagnostic_msgs</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>robot_interfaces</exec_depend>
+  <exec_depend>dharma_interfaces</exec_depend>
   <exec_depend>python3-websocket</exec_depend>
   <exec_depend>python3-requests</exec_depend>
 
diff --git a/src/robot_fleet/robot_fleet/fleet_uplink_node.py b/src/robot_fleet/robot_fleet/fleet_uplink_node.py
index fdf0cd1..4961e91 100644
--- a/src/robot_fleet/robot_fleet/fleet_uplink_node.py
+++ b/src/robot_fleet/robot_fleet/fleet_uplink_node.py
@@ -9,6 +9,8 @@
 import threading
 import time
 
+from dharma_interfaces.msg import MissionStatus, NavigationStatus, StopPointEvent
+from dharma_interfaces.srv import LoadPath
 from diagnostic_msgs.msg import DiagnosticArray
 from geometry_msgs.msg import PoseArray
 from rcl_interfaces.srv import SetParameters
@@ -23,8 +25,6 @@
 from robot_fleet.routes import safe_route_id, summarize_csv
 from robot_fleet.spool import Spool
 from robot_fleet.switches import live_parameters, status_summary, SWITCHES
-from robot_interfaces.msg import MissionStatus, NavigationStatus, StopPointEvent
-from robot_interfaces.srv import LoadPath
 from sensor_msgs.msg import BatteryState, CompressedImage
 from std_msgs.msg import Bool, String
 from std_srvs.srv import SetBool, Trigger
diff --git a/src/robot_interfaces/CMakeLists.txt b/src/robot_interfaces/CMakeLists.txt
deleted file mode 100644
index eaeddae..0000000
--- a/src/robot_interfaces/CMakeLists.txt
+++ /dev/null
@@ -1,23 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(robot_interfaces)
-
-find_package(ament_cmake REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-find_package(std_msgs REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/SectorScan.msg"
-  "msg/AvoidanceCommand.msg"
-  "msg/Waypoint.msg"
-  "msg/WaypointPath.msg"
-  "msg/NavigationStatus.msg"
-  "msg/MissionStatus.msg"
-  "msg/StopPointEvent.msg"
-  "srv/MarkStopPoint.srv"
-  "srv/LoadPath.srv"
-  "srv/SetMode.srv"
-  DEPENDENCIES std_msgs
-)
-
-ament_export_dependencies(rosidl_default_runtime)
-ament_package()
\ No newline at end of file
diff --git a/src/robot_interfaces/README.md b/src/robot_interfaces/README.md
deleted file mode 100644
index f3c619b..0000000
--- a/src/robot_interfaces/README.md
+++ /dev/null
@@ -1,13 +0,0 @@
-# robot_interfaces
-
-Messages and services shared by the patrol stack.
-
-| Kind | Name | Used by |
-|---|---|---|
-| msg | `SectorScan` | lidar_sector_node → obstacle_avoidance_node |
-| msg | `AvoidanceCommand` | obstacle_avoidance_node → path_follower_node |
-| msg | `Waypoint`, `WaypointPath` | path_loader_node → path_follower_node |
-| msg | `NavigationStatus`, `MissionStatus`, `StopPointEvent` | path_follower_node → panel, bridge, fleet |
-| srv | `LoadPath` | `navigation/load_path` |
-| srv | `MarkStopPoint` | `route/mark_stop_point` |
-| srv | `SetMode` | `bridge/set_mode` (`auto` \| `remote`) |
diff --git a/src/robot_interfaces/msg/AvoidanceCommand.msg b/src/robot_interfaces/msg/AvoidanceCommand.msg
deleted file mode 100644
index 4fae17a..0000000
--- a/src/robot_interfaces/msg/AvoidanceCommand.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-# Output of obstacle_avoidance_node, consumed by path_follower_node.
-
-std_msgs/Header header
-float32 steering_bias      # rad/s, added to the controller's angular command
-float32 lateral_bias       # m/s, sideways push (+left), used only by holonomic robots
-float32 velocity_scale     # 0..1 multiplier on the target linear velocity
-bool emergency             # hard stop, obstacle inside the emergency cone/distance
diff --git a/src/robot_interfaces/msg/MissionStatus.msg b/src/robot_interfaces/msg/MissionStatus.msg
deleted file mode 100644
index 521d7f0..0000000
--- a/src/robot_interfaces/msg/MissionStatus.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-std_msgs/Header header
-bool active
-string root_status
-string active_behavior
\ No newline at end of file
diff --git a/src/robot_interfaces/msg/NavigationStatus.msg b/src/robot_interfaces/msg/NavigationStatus.msg
deleted file mode 100644
index dd87c5d..0000000
--- a/src/robot_interfaces/msg/NavigationStatus.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-std_msgs/Header header
-string state
-string message
\ No newline at end of file
diff --git a/src/robot_interfaces/msg/SectorScan.msg b/src/robot_interfaces/msg/SectorScan.msg
deleted file mode 100644
index 6b33aa3..0000000
--- a/src/robot_interfaces/msg/SectorScan.msg
+++ /dev/null
@@ -1,10 +0,0 @@
-# Output of lidar_sector_node (robot_perception).
-# Fixed-count angular sectors, each holding the nearest obstacle distance.
-# Sector i center angle = angle_min + (i + 0.5) * angle_increment (rad, REP-103: +left/CCW).
-
-std_msgs/Header header
-uint32 num_sectors
-float32 angle_min
-float32 angle_increment
-float32 range_max
-float32[] ranges
\ No newline at end of file
diff --git a/src/robot_interfaces/msg/StopPointEvent.msg b/src/robot_interfaces/msg/StopPointEvent.msg
deleted file mode 100644
index 6fe7583..0000000
--- a/src/robot_interfaces/msg/StopPointEvent.msg
+++ /dev/null
@@ -1,11 +0,0 @@
-# Published by path_follower_node at each inspection stop point.
-# event: arrived | inspected | departed | skipped
-
-std_msgs/Header header
-uint32 index
-float32 x
-float32 y
-float32 yaw_deg
-float32 dwell_sec
-string event
-string message
diff --git a/src/robot_interfaces/msg/Waypoint.msg b/src/robot_interfaces/msg/Waypoint.msg
deleted file mode 100644
index 9b3c59b..0000000
--- a/src/robot_interfaces/msg/Waypoint.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-float32 x
-float32 y
-float32 yaw
-float32 dwell_sec
\ No newline at end of file
diff --git a/src/robot_interfaces/msg/WaypointPath.msg b/src/robot_interfaces/msg/WaypointPath.msg
deleted file mode 100644
index 1c99635..0000000
--- a/src/robot_interfaces/msg/WaypointPath.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/Header header
-Waypoint[] waypoints
\ No newline at end of file
diff --git a/src/robot_interfaces/package.xml b/src/robot_interfaces/package.xml
deleted file mode 100644
index 2fc99f2..0000000
--- a/src/robot_interfaces/package.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>robot_interfaces</name>
-  <version>0.6.0</version>
-  <description>Custom interfaces for the patrol stack: SectorScan, AvoidanceCommand, Waypoint, WaypointPath, NavigationStatus, MissionStatus, StopPointEvent, LoadPath, MarkStopPoint.</description>
-  <maintainer email="darmawandeveloper@gmail.com">Agus Darmawan</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>std_msgs</depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <member_of_group>rosidl_interface_packages</member_of_group>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-
-</package>
\ No newline at end of file
diff --git a/src/robot_interfaces/srv/LoadPath.srv b/src/robot_interfaces/srv/LoadPath.srv
deleted file mode 100644
index 5bf72d8..0000000
--- a/src/robot_interfaces/srv/LoadPath.srv
+++ /dev/null
@@ -1,4 +0,0 @@
-string waypoints_file
----
-bool success
-string message
\ No newline at end of file
diff --git a/src/robot_interfaces/srv/MarkStopPoint.srv b/src/robot_interfaces/srv/MarkStopPoint.srv
deleted file mode 100644
index 4531bae..0000000
--- a/src/robot_interfaces/srv/MarkStopPoint.srv
+++ /dev/null
@@ -1,4 +0,0 @@
-float32 dwell_sec
----
-bool success
-string message
\ No newline at end of file
diff --git a/src/robot_interfaces/srv/SetMode.srv b/src/robot_interfaces/srv/SetMode.srv
deleted file mode 100644
index 279ee0b..0000000
--- a/src/robot_interfaces/srv/SetMode.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-# Switch who drives the robot: "auto" (navigation) or "remote" (teleop).
-string mode
----
-bool success
-string message
diff --git a/src/robot_localization/README.md b/src/robot_localization/README.md
deleted file mode 100644
index e6077c0..0000000
--- a/src/robot_localization/README.md
+++ /dev/null
@@ -1,23 +0,0 @@
-# robot_localization
-
-`localization_node` bridges FAST-LIO into the standard tree and keeps `map → odom` correct.
-
-```
-map → odom → camera_init → body → base_link
-```
-
-`odom → camera_init` and `body → base_link` come from the URDF (`base_link → livox_imu_frame`) and
-are static. `map → odom` is a planar (x, y, yaw) correction set from an AprilTag, an RViz 2D Pose
-Estimate or, with `assume_start_origin`, the start pose; it is published on `/tf` at `map_odom_hz`
-(REP-105), so every correction reaches the other nodes at once.
-
-| Interface | Type | |
-|---|---|---|
-| `localization/start` | `std_srvs/Trigger` | localize from a visible tag |
-| `localization/record_tags` | `std_srvs/Trigger` | average and save the map pose of visible tags |
-| `localization/status` | `std_msgs/String`, latched | active source, `none` until localized |
-| `localization/tag_markers` | `visualization_msgs/MarkerArray`, latched | known tags |
-| `/initialpose` | `geometry_msgs/PoseWithCovarianceStamped` | RViz pose estimate |
-
-Optional periodic drift correction from any visible tag is gated by `max_correction_m/deg`.
-Parameters: [config/localization_params.yaml](config/localization_params.yaml).
diff --git a/src/robot_localization/launch/localization.launch.py b/src/robot_localization/launch/localization.launch.py
deleted file mode 100644
index fe41038..0000000
--- a/src/robot_localization/launch/localization.launch.py
+++ /dev/null
@@ -1,36 +0,0 @@
-#!/usr/bin/env python3
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    """Launch the localization node (frame bridge + AprilTag map->odom)."""
-    default_params = os.path.join(
-        get_package_share_directory('robot_localization'), 'config', 'localization_params.yaml'
-    )
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument('params_file', default_value=default_params),
-            DeclareLaunchArgument(
-                'tags_config_file',
-                default_value='',
-                description='JSON file with tag poses in the map frame',
-            ),
-            Node(
-                package='robot_localization',
-                executable='localization_node',
-                name='localization_node',
-                output='screen',
-                parameters=[
-                    LaunchConfiguration('params_file'),
-                    {'tags_config_file': LaunchConfiguration('tags_config_file')},
-                ],
-            ),
-        ]
-    )
diff --git a/src/robot_localization/package.xml b/src/robot_localization/package.xml
deleted file mode 100644
index 82626dc..0000000
--- a/src/robot_localization/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>robot_localization</name>
-  <version>0.2.0</version>
-  <description>Bridges FAST-LIO frames (camera_init/body) into map->odom->base_link and sets map->odom from AprilTags or RViz.</description>
-  <maintainer email="darmawandeveloper@gmail.com">Agus Darmawan</maintainer>
-  <license>Apache-2.0</license>
-
-
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>tf_transformations</exec_depend>
-  <exec_depend>python3-transforms3d</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>std_srvs</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>python3-numpy</exec_depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/src/robot_localization/robot_localization/localization_node.py b/src/robot_localization/robot_localization/localization_node.py
deleted file mode 100644
index a4ae039..0000000
--- a/src/robot_localization/robot_localization/localization_node.py
+++ /dev/null
@@ -1,441 +0,0 @@
-#!/usr/bin/env python3
-import json
-import math
-import os
-import time
-
-from geometry_msgs.msg import Point, PoseWithCovarianceStamped
-import numpy as np
-import rclpy
-from rclpy.duration import Duration
-from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSProfile
-from rclpy.time import Time
-from robot_localization.utils.pose_localization import (
-    map_base_from_tag,
-    map_odom_from_map_base,
-    matrix_to_transform,
-    planar_delta,
-    pose_msg_to_matrix,
-    pose_values_to_matrix,
-    transform_to_matrix,
-)
-from robot_localization.utils.tag_recording import average_poses, describe_pose, merge_into_file
-from std_msgs.msg import Empty, String
-from std_srvs.srv import Trigger
-from tf2_ros import (
-    Buffer,
-    StaticTransformBroadcaster,
-    TransformBroadcaster,
-    TransformException,
-    TransformListener,
-)
-import tf_transformations as tft
-from visualization_msgs.msg import Marker, MarkerArray
-
-
-class LocalizationNode(Node):
-    """Bridges FAST-LIO frames into map->odom->base_link and corrects map->odom from AprilTags."""
-
-    def __init__(self):
-        """Declare params, load tag poses, and start the frame bridge."""
-        super().__init__('localization_node')
-        self.declare_parameter('tags_config_file', '')
-        self.declare_parameter('map_frame', 'map')
-        self.declare_parameter('odom_frame', 'odom')
-        self.declare_parameter('base_frame', 'base_link')
-        self.declare_parameter('lio_odom_frame', 'camera_init')
-        self.declare_parameter('lio_body_frame', 'body')
-        self.declare_parameter('imu_frame', 'livox_imu_frame')
-        self.declare_parameter('camera_frame', 'camera_color_optical_frame')
-        self.declare_parameter('tag_max_age_sec', 0.5)
-        self.declare_parameter('auto_localize_on_start', True)
-        self.declare_parameter('assume_start_origin', False)
-        self.declare_parameter('tag_frames', ['base_map', 'charging_dock'])
-        self.declare_parameter('tag_record_duration_sec', 2.0)
-        self.declare_parameter('tags_record_file', '')
-        self.declare_parameter('tag_marker_size', 0.15)
-        self.declare_parameter('auto_correct_period_sec', 0.0)
-        self.declare_parameter('max_correction_m', 0.5)
-        self.declare_parameter('max_correction_deg', 15.0)
-        self.declare_parameter('map_odom_hz', 20.0)
-
-        p = self.get_parameter
-        self._map = p('map_frame').value
-        self._odom = p('odom_frame').value
-        self._base = p('base_frame').value
-        self._lio_odom = p('lio_odom_frame').value
-        self._lio_body = p('lio_body_frame').value
-        self._imu = p('imu_frame').value
-        self._camera = p('camera_frame').value
-        self._tag_max_age = Duration(seconds=float(p('tag_max_age_sec').value))
-        self._max_corr_m = float(p('max_correction_m').value)
-        self._max_corr_rad = math.radians(float(p('max_correction_deg').value))
-
-        self._tag_poses = self.load_tag_poses(p('tags_config_file').value)
-        self._static_broadcaster = StaticTransformBroadcaster(self)
-        self._tf_broadcaster = TransformBroadcaster(self)
-        self._tf_buffer = Buffer()
-        self._tf_listener = TransformListener(self._tf_buffer, self)
-
-        self._m_base_imu = None
-        self._m_map_odom = np.eye(4)
-        self._localized = False
-        self._active_source = 'none'
-
-        latched = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
-        self._status_pub = self.create_publisher(String, 'localization/status', latched)
-        self._assume_start_origin = bool(p('assume_start_origin').value)
-        self.publish_status()
-
-        self._tags_file = p('tags_config_file').value
-        self._tag_markers_pub = self.create_publisher(
-            MarkerArray, 'localization/tag_markers', latched
-        )
-        self.publish_tag_markers()
-
-        self.create_service(Trigger, 'localization/start', self.localization_callback)
-        self.create_service(Trigger, 'localization/record_tags', self.record_tags_callback)
-        self.create_subscription(
-            PoseWithCovarianceStamped, '/initialpose', self.pose_estimate_callback, 10
-        )
-        self.create_subscription(Empty, 'odometry/restarted', self.odometry_restarted_callback, 10)
-        self._auto_timer = None
-
-        self._bridge_timer = self.create_timer(0.5, self.try_start_bridge)
-        self.create_timer(1.0 / float(p('map_odom_hz').value), self.publish_map_odom)
-        self._auto_start = bool(p('auto_localize_on_start').value)
-        period = float(p('auto_correct_period_sec').value)
-        if period > 0.0:
-            self.create_timer(period, self.auto_correct)
-
-        self.get_logger().info(f'localization_node ready with {len(self._tag_poses)} tag(s)')
-
-    def load_tag_poses(self, filepath: str) -> dict:
-        """
-        Load a {tag_frame: {x,y,z,qx,qy,qz,qw}} JSON file into tag_frame -> 4x4 matrix.
-
-        A missing file is normal before the first teach (localization/record_tags creates it);
-        a broken file or entry is reported and skipped instead of crashing the node.
-        """
-        if not filepath:
-            self.get_logger().warn('tags_config_file not set, AprilTag localization disabled')
-            return {}
-        try:
-            with open(filepath, 'r') as f:
-                raw = json.load(f)
-        except FileNotFoundError:
-            self.get_logger().warn(
-                f'{filepath} does not exist yet, record tags with localization/record_tags'
-            )
-            return {}
-        except (OSError, ValueError) as error:
-            self.get_logger().error(f'cannot read tag poses from {filepath}: {error}')
-            return {}
-        if not isinstance(raw, dict):
-            self.get_logger().error(f'{filepath} must be a JSON object of tag poses')
-            return {}
-        poses = {}
-        for frame, v in raw.items():
-            try:
-                poses[frame] = pose_values_to_matrix(
-                    [float(v[k]) for k in ('x', 'y', 'z', 'qx', 'qy', 'qz', 'qw')]
-                )
-            except (KeyError, TypeError, ValueError) as error:
-                self.get_logger().error(f'{filepath}: tag "{frame}" skipped ({error})')
-        return poses
-
-    def try_start_bridge(self):
-        """Wait for base_link->imu from the URDF, then publish the static frame bridge once."""
-        try:
-            tf = self._tf_buffer.lookup_transform(self._base, self._imu, Time())
-        except TransformException as error:
-            self.get_logger().warn(
-                f'waiting for URDF TF {self._base}->{self._imu}: {error}',
-                throttle_duration_sec=5.0,
-            )
-            return
-        self._m_base_imu = transform_to_matrix(tf)
-        self._bridge_timer.cancel()
-        self.broadcast_static()
-        self.get_logger().info('frame bridge up: map->odom->camera_init, body->base_link')
-        if self._assume_start_origin:
-            self._localized = True
-            self._active_source = 'start position'
-            self.publish_status()
-        if self._auto_start and self._tag_poses:
-            self.start_auto_localize()
-
-    def start_auto_localize(self):
-        """(Re)start retrying tag localization every second."""
-        if self._auto_timer is not None:
-            self._auto_timer.cancel()
-        self._auto_timer = self.create_timer(1.0, self.auto_start_tick)
-
-    def odometry_restarted_callback(self, _msg: Empty):
-        """FAST-LIO restarted at a new origin: the old map->odom no longer holds."""
-        if not self._localized:
-            return
-        self._localized = False
-        self.publish_status()
-        self.get_logger().error(
-            'odometry restarted, map->odom invalid: show a tag or set a 2D Pose Estimate'
-        )
-        if self._tag_poses:
-            self.start_auto_localize()
-
-    def publish_status(self):
-        """Publish the active map->odom source ('none' until localized), latched."""
-        self._status_pub.publish(String(data=self._active_source if self._localized else 'none'))
-
-    def broadcast_static(self):
-        """Publish the fixed bridge: odom->camera_init (= base->imu), body->base_link (inverse)."""
-        stamp = self.get_clock().now().to_msg()
-        self._static_broadcaster.sendTransform(
-            [
-                matrix_to_transform(self._m_base_imu, self._odom, self._lio_odom, stamp),
-                matrix_to_transform(
-                    np.linalg.inv(self._m_base_imu), self._lio_body, self._base, stamp
-                ),
-            ]
-        )
-        self.publish_map_odom()
-
-    def publish_map_odom(self):
-        """
-        Publish map->odom on /tf (REP-105), at map_odom_hz and right after every change.
-
-        It used to be a static transform, but it changes on every (re)localization and
-        StaticTransformBroadcaster keeps the first transform per child frame on newer ROS 2,
-        so corrections never reached the other nodes there.
-        """
-        if self._m_base_imu is None:
-            return
-        stamp = self.get_clock().now().to_msg()
-        self._tf_broadcaster.sendTransform(
-            matrix_to_transform(self._m_map_odom, self._map, self._odom, stamp)
-        )
-
-    def set_map_odom(self, m_map_odom: np.ndarray, source: str):
-        """Store and broadcast a new map->odom."""
-        self._m_map_odom = m_map_odom
-        self._localized = True
-        self._active_source = source
-        self.broadcast_static()
-        self.publish_status()
-        t = m_map_odom[:3, 3]
-        yaw = math.degrees(math.atan2(m_map_odom[1, 0], m_map_odom[0, 0]))
-        self.get_logger().info(f'map->odom from {source}: x={t[0]:.3f} y={t[1]:.3f} yaw={yaw:.1f}')
-
-    def fresh_tag(self):
-        """Return (tag_frame, cam->tag TF) for the first tag seen within tag_max_age_sec."""
-        now = self.get_clock().now()
-        for frame in self._tag_poses:
-            try:
-                tf = self._tf_buffer.lookup_transform(self._camera, frame, Time())
-            except TransformException:
-                continue
-            if now - Time.from_msg(tf.header.stamp) <= self._tag_max_age:
-                return frame, tf
-        return None, None
-
-    def map_odom_from_visible_tag(self):
-        """Compute map->odom from a fresh tag: (matrix, tag_frame) or (None, reason)."""
-        if self._m_base_imu is None:
-            return None, 'frame bridge not ready'
-        if not self._tag_poses:
-            return None, 'no tags configured'
-        frame, cam_tag = self.fresh_tag()
-        if frame is None:
-            return None, f'no fresh detection of {list(self._tag_poses)}'
-        stamp = Time.from_msg(cam_tag.header.stamp)
-        try:
-            base_cam = self._tf_buffer.lookup_transform(self._base, self._camera, Time())
-            odom_base = self._tf_buffer.lookup_transform(
-                self._odom, self._base, stamp, Duration(seconds=0.2)
-            )
-        except TransformException as error:
-            return None, f'TF lookup failed: {error}'
-        m_map_base = map_base_from_tag(
-            self._tag_poses[frame], transform_to_matrix(base_cam), transform_to_matrix(cam_tag)
-        )
-        return map_odom_from_map_base(m_map_base, transform_to_matrix(odom_base)), frame
-
-    def localization_callback(self, request, response):
-        """Service: set map->odom from whichever configured tag is currently visible."""
-        m, info = self.map_odom_from_visible_tag()
-        if m is None:
-            response.success = False
-            response.message = info
-            self.get_logger().error(info)
-            return response
-        self.set_map_odom(m, f'apriltag "{info}"')
-        response.success = True
-        response.message = f'map->odom set from tag "{info}"'
-        return response
-
-    def auto_start_tick(self):
-        """Retry tag localization every second until the first success."""
-        m, info = self.map_odom_from_visible_tag()
-        if m is None:
-            self.get_logger().info(f'auto localize waiting: {info}', throttle_duration_sec=10.0)
-            return
-        self.set_map_odom(m, f'apriltag "{info}" (auto start)')
-        self._auto_timer.cancel()
-        self._auto_timer = None
-
-    def auto_correct(self):
-        """Periodically correct drift from any visible tag, rejecting implausible jumps."""
-        if not self._localized:
-            return
-        m, info = self.map_odom_from_visible_tag()
-        if m is None:
-            return
-        dist, dyaw = planar_delta(m, self._m_map_odom)
-        if dist > self._max_corr_m or dyaw > self._max_corr_rad:
-            self.get_logger().warn(
-                f'rejected correction from "{info}": {dist:.2f} m / {math.degrees(dyaw):.1f} deg'
-            )
-            return
-        self.set_map_odom(m, f'apriltag "{info}" (auto correct)')
-
-    def record_tags_callback(self, request, response):
-        """Average the map pose of every visible tag in tag_frames and save it to the tag file."""
-        duration = float(self.get_parameter('tag_record_duration_sec').value)
-        frames = [f for f in self.get_parameter('tag_frames').value if f]
-        samples = {frame: [] for frame in frames}
-        end = time.monotonic() + duration
-        while time.monotonic() < end:
-            for frame in frames:
-                m = self.fresh_map_tag(frame)
-                if m is not None:
-                    samples[frame].append(m)
-            time.sleep(0.1)
-
-        poses, lines = {}, []
-        for frame, values in samples.items():
-            if len(values) < 3:
-                lines.append(f'{frame}: not seen')
-                continue
-            mean, pos_std, yaw_std = average_poses(values)
-            poses[frame] = mean
-            lines.append(
-                f'{frame}: {describe_pose(mean)} n={len(values)} '
-                f'std={pos_std * 100:.1f}cm/{yaw_std:.1f}deg'
-            )
-        if not poses:
-            response.success = False
-            response.message = 'no tag seen: ' + '; '.join(lines)
-            return response
-
-        path = self.get_parameter('tags_record_file').value or self._tags_file
-        path = path or os.path.join(os.getcwd(), 'tag_config.json')
-        merged = merge_into_file(path, poses)
-        self._tag_poses = {
-            f: pose_values_to_matrix([v['x'], v['y'], v['z'], v['qx'], v['qy'], v['qz'], v['qw']])
-            for f, v in merged.items()
-        }
-        self.publish_tag_markers()
-        response.success = True
-        response.message = f'saved {len(poses)} tag(s) to {path}\n' + '\n'.join(lines)
-        self.get_logger().info(response.message)
-        return response
-
-    def fresh_map_tag(self, frame: str):
-        """Return map->tag as a 4x4 matrix if the tag was detected within tag_max_age_sec."""
-        try:
-            cam_tag = self._tf_buffer.lookup_transform(self._camera, frame, Time())
-            if self.get_clock().now() - Time.from_msg(cam_tag.header.stamp) > self._tag_max_age:
-                return None
-            return transform_to_matrix(self._tf_buffer.lookup_transform(self._map, frame, Time()))
-        except TransformException:
-            return None
-
-    def publish_tag_markers(self):
-        """Publish known tags as plates with x/y/z axes, a label and a ground line (latched)."""
-        markers = MarkerArray()
-        clear = Marker()
-        clear.action = Marker.DELETEALL
-        markers.markers.append(clear)
-        size = float(self.get_parameter('tag_marker_size').value)
-        axes = (
-            (np.eye(4), (1.0, 0.1, 0.1)),
-            (tft.euler_matrix(0.0, 0.0, math.pi / 2), (0.1, 1.0, 0.1)),
-            (tft.euler_matrix(0.0, -math.pi / 2, 0.0), (0.2, 0.4, 1.0)),
-        )
-        for i, (frame, m) in enumerate(sorted(self._tag_poses.items())):
-            mid = 10 * i
-            plate = self.tag_marker(Marker.CUBE, mid, m, (1.0, 1.0, 1.0, 0.9))
-            plate.scale.x, plate.scale.y, plate.scale.z = size, size, 0.005
-            markers.markers.append(plate)
-            for k, (rot, rgb) in enumerate(axes):
-                axis = self.tag_marker(Marker.ARROW, mid + 1 + k, m @ rot, (*rgb, 1.0))
-                axis.scale.x, axis.scale.y, axis.scale.z = 0.25, 0.02, 0.02
-                markers.markers.append(axis)
-            text = self.tag_marker(Marker.TEXT_VIEW_FACING, mid + 4, m, (1.0, 0.3, 0.9, 1.0))
-            text.pose.position.z += 0.25
-            text.scale.z = 0.15
-            text.text = frame
-            markers.markers.append(text)
-            drop = self.tag_marker(Marker.LINE_LIST, mid + 5, np.eye(4), (1.0, 0.3, 0.9, 0.6))
-            drop.scale.x = 0.01
-            x, y, z = (float(v) for v in m[:3, 3])
-            drop.points = [Point(x=x, y=y, z=z), Point(x=x, y=y, z=0.0)]
-            markers.markers.append(drop)
-        self._tag_markers_pub.publish(markers)
-
-    def tag_marker(self, kind: int, mid: int, m: np.ndarray, rgba: tuple) -> Marker:
-        """Return a map-frame marker posed at the 4x4 matrix m."""
-        mk = Marker()
-        mk.header.frame_id = self._map
-        mk.ns = 'tags'
-        mk.id = mid
-        mk.type = kind
-        mk.pose.position.x, mk.pose.position.y, mk.pose.position.z = (float(v) for v in m[:3, 3])
-        q = tft.quaternion_from_matrix(m)
-        mk.pose.orientation.x, mk.pose.orientation.y = float(q[0]), float(q[1])
-        mk.pose.orientation.z, mk.pose.orientation.w = float(q[2]), float(q[3])
-        mk.color.r, mk.color.g, mk.color.b, mk.color.a = rgba
-        return mk
-
-    def pose_estimate_callback(self, msg: PoseWithCovarianceStamped):
-        """Set map->odom so base_link lands on an RViz 2D Pose Estimate."""
-        if self._m_base_imu is None:
-            self.get_logger().error('2D Pose Estimate ignored, frame bridge not ready')
-            return
-        try:
-            odom_base = self._tf_buffer.lookup_transform(self._odom, self._base, Time())
-        except TransformException as error:
-            self.get_logger().error(f'2D Pose Estimate ignored, TF not ready: {error}')
-            return
-        m = map_odom_from_map_base(
-            pose_msg_to_matrix(msg.pose.pose), transform_to_matrix(odom_base)
-        )
-        self.set_map_odom(m, 'RViz 2D Pose Estimate')
-
-
-def main(args=None):
-    """Spin the localization node; a second thread keeps TF flowing during blocking lookups."""
-    rclpy.init(args=args)
-    node = LocalizationNode()
-    executor = MultiThreadedExecutor(num_threads=2)
-    executor.add_node(node)
-    try:
-        executor.spin()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_localization/robot_localization/utils/__init__.py b/src/robot_localization/robot_localization/utils/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_localization/robot_localization/utils/pose_localization.py b/src/robot_localization/robot_localization/utils/pose_localization.py
deleted file mode 100644
index ae41196..0000000
--- a/src/robot_localization/robot_localization/utils/pose_localization.py
+++ /dev/null
@@ -1,76 +0,0 @@
-import math
-
-from geometry_msgs.msg import TransformStamped
-import numpy as np
-import tf_transformations as tft
-
-
-def transform_to_matrix(t: TransformStamped) -> np.ndarray:
-    """Convert a TransformStamped into a 4x4 homogeneous matrix."""
-    trans = t.transform.translation
-    rot = t.transform.rotation
-    m = tft.quaternion_matrix([rot.x, rot.y, rot.z, rot.w])
-    m[:3, 3] = [trans.x, trans.y, trans.z]
-    return m
-
-
-def matrix_to_transform(
-    m: np.ndarray, parent_frame: str, child_frame: str, stamp
-) -> TransformStamped:
-    """Convert a 4x4 homogeneous matrix into a TransformStamped."""
-    t = TransformStamped()
-    t.header.stamp = stamp
-    t.header.frame_id = parent_frame
-    t.child_frame_id = child_frame
-    t.transform.translation.x = float(m[0, 3])
-    t.transform.translation.y = float(m[1, 3])
-    t.transform.translation.z = float(m[2, 3])
-    q = tft.quaternion_from_matrix(m)
-    t.transform.rotation.x = float(q[0])
-    t.transform.rotation.y = float(q[1])
-    t.transform.rotation.z = float(q[2])
-    t.transform.rotation.w = float(q[3])
-    return t
-
-
-def pose_values_to_matrix(values) -> np.ndarray:
-    """Convert a [x, y, z, qx, qy, qz, qw] list into a 4x4 homogeneous matrix."""
-    x, y, z, qx, qy, qz, qw = values
-    m = tft.quaternion_matrix([qx, qy, qz, qw])
-    m[:3, 3] = [x, y, z]
-    return m
-
-
-def flatten_to_yaw(m: np.ndarray) -> np.ndarray:
-    """Project a pose onto the ground plane, keeping only x, y and yaw."""
-    yaw = math.atan2(m[1, 0], m[0, 0])
-    flat = tft.euler_matrix(0.0, 0.0, yaw)
-    flat[0, 3] = m[0, 3]
-    flat[1, 3] = m[1, 3]
-    return flat
-
-
-def map_odom_from_map_base(m_map_base: np.ndarray, m_odom_base: np.ndarray) -> np.ndarray:
-    """2D map->odom so that base_link lands on m_map_base given the current odom->base_link."""
-    return flatten_to_yaw(m_map_base) @ tft.inverse_matrix(flatten_to_yaw(m_odom_base))
-
-
-def map_base_from_tag(
-    m_map_tag: np.ndarray, m_base_cam: np.ndarray, m_cam_tag: np.ndarray
-) -> np.ndarray:
-    """map->base_link implied by seeing a tag whose map pose is known."""
-    return m_map_tag @ tft.inverse_matrix(m_base_cam @ m_cam_tag)
-
-
-def pose_msg_to_matrix(pose) -> np.ndarray:
-    """Convert a geometry_msgs/Pose into a 4x4 homogeneous matrix."""
-    p, q = pose.position, pose.orientation
-    return pose_values_to_matrix([p.x, p.y, p.z, q.x, q.y, q.z, q.w])
-
-
-def planar_delta(a: np.ndarray, b: np.ndarray) -> tuple[float, float]:
-    """Return the translation (m) and absolute yaw (rad) difference between two poses."""
-    dist = math.hypot(a[0, 3] - b[0, 3], a[1, 3] - b[1, 3])
-    dyaw = math.atan2(a[1, 0], a[0, 0]) - math.atan2(b[1, 0], b[0, 0])
-    dyaw = abs(math.atan2(math.sin(dyaw), math.cos(dyaw)))
-    return dist, dyaw
diff --git a/src/robot_localization/robot_localization/utils/tag_recording.py b/src/robot_localization/robot_localization/utils/tag_recording.py
deleted file mode 100644
index efc7ab0..0000000
--- a/src/robot_localization/robot_localization/utils/tag_recording.py
+++ /dev/null
@@ -1,53 +0,0 @@
-import json
-import math
-import os
-import shutil
-
-import numpy as np
-import tf_transformations as tft
-
-
-def average_poses(samples: list[np.ndarray]) -> tuple[np.ndarray, float, float]:
-    """Average 4x4 poses; return (mean pose, max position std in m, yaw std in deg)."""
-    positions = np.array([m[:3, 3] for m in samples])
-    quats = np.array([tft.quaternion_from_matrix(m) for m in samples])
-    quats[np.dot(quats, quats[0]) < 0.0] *= -1.0
-    q = quats.mean(axis=0)
-    q /= np.linalg.norm(q)
-    mean = tft.quaternion_matrix(q)
-    mean[:3, 3] = positions.mean(axis=0)
-    yaws = np.array([math.atan2(m[1, 0], m[0, 0]) for m in samples])
-    yaw_mean = math.atan2(np.sin(yaws).mean(), np.cos(yaws).mean())
-    yaw_dev = np.arctan2(np.sin(yaws - yaw_mean), np.cos(yaws - yaw_mean))
-    return mean, float(positions.std(axis=0).max()), float(np.degrees(yaw_dev.std()))
-
-
-def pose_to_dict(m: np.ndarray) -> dict:
-    """Serialise a 4x4 pose in the tag_config.json format."""
-    qx, qy, qz, qw = tft.quaternion_from_matrix(m)
-    x, y, z = m[:3, 3]
-    return {
-        k: round(float(v), 4)
-        for k, v in zip(('x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'), (x, y, z, qx, qy, qz, qw))
-    }
-
-
-def describe_pose(m: np.ndarray) -> str:
-    """Return 'x y z | roll pitch yaw' with metres and degrees."""
-    roll, pitch, yaw = (math.degrees(a) for a in tft.euler_from_matrix(m))
-    x, y, z = m[:3, 3]
-    return f'xyz=({x:.3f}, {y:.3f}, {z:.3f}) rpy=({roll:.1f}, {pitch:.1f}, {yaw:.1f})'
-
-
-def merge_into_file(path: str, poses: dict[str, np.ndarray]) -> dict:
-    """Merge poses into a tag_config.json (keeping other tags), back up the old file, write it."""
-    data = {}
-    if os.path.isfile(path):
-        with open(path, 'r') as f:
-            data = json.load(f)
-        shutil.copyfile(path, path + '.bak')
-    data.update({frame: pose_to_dict(m) for frame, m in poses.items()})
-    os.makedirs(os.path.dirname(os.path.abspath(path)), exist_ok=True)
-    with open(path, 'w') as f:
-        json.dump(data, f, indent=2)
-    return data
diff --git a/src/robot_localization/setup.cfg b/src/robot_localization/setup.cfg
deleted file mode 100644
index 02b2799..0000000
--- a/src/robot_localization/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/robot_localization
-[install]
-install_scripts=$base/lib/robot_localization
diff --git a/src/robot_navigation/README.md b/src/robot_navigation/README.md
deleted file mode 100644
index 053f3b4..0000000
--- a/src/robot_navigation/README.md
+++ /dev/null
@@ -1,78 +0,0 @@
-# robot_navigation
-
-Route following for patrols.
-
-| Node | Role |
-|---|---|
-| `path_loader_node` | loads a route CSV, resamples and smooths it between stop points, publishes it latched |
-| `path_follower_node` | follows the route, speed profile, final x/y/yaw approach, inspection at stop points, loop patrol, safety supervisor |
-| `obstacle_avoidance_node` | reactive avoidance on `SectorScan` (`braitenberg`, `vfh`, `potential_field`, `follow_gap`), and the front-cone emergency stop in every mode |
-
-Every parameter applies at runtime (Tuning panel or `ros2 param set`) except `control_rate` and the
-service names.
-
-## Following on a legged robot: `controller: holonomic` (default)
-
-A robot dog can walk sideways and backwards, so the follower uses all three axes, x, y and yaw:
-
-- **Translation** heads for a carrot `holonomic.lookahead_distance` further along the route, in any
-  direction of the body. A cross-track error closes sideways, smoothly and without overshoot,
-  while the heading stays on the route. The robot cannot weave around the route the way
-  steering-only trackers do, because the heading does not depend on the cross-track error.
-- **Heading** follows the route direction `holonomic.heading_lookahead` ahead, with a proportional,
-  rate-limited yaw command.
-- **When the route is beside or behind the robot**, it moves that way, e.g. after a stop point
-  where it faced a shop, after avoidance, or when navigation starts off the route. It strafes at up
-  to `holonomic.max_lateral_velocity` and backs up at up to `holonomic.max_reverse_velocity`, while
-  it turns to face the route. Translation slows down while the heading is far off
-  (`holonomic.min_alignment_scale`), so it mostly walks forward.
-
-With `holonomic: false` it never strafes: it turns toward the carrot and walks forward. The
-steering-only controllers `pure_pursuit`, `pid`, `mppi`, `lqr` and `stanley` are still available.
-
-## Obstacles: `avoidance_mode: path_shift` (default with the holonomic controller)
-
-Instead of steering away, the robot slides sideways onto a line parallel to the route and keeps
-its heading. It uses the 360° `perception/safety_scan` and the robot footprint (`footprint.*`).
-
-- It takes the offset nearest the route whose corridor is free, from the rear edge to
-  `path_shift.look_ahead` beyond the front edge, with `path_shift.clearance` to spare.
-- It never leaves the route by more than `path_shift.max_offset`. Without a gap, it slows down and
-  stops `path_shift.stop_margin` before the obstacle, and reports `BLOCKED` after
-  `blocked_timeout_sec`.
-- It keeps its side while passing (`path_shift.switch_cost`, `path_shift.switch_margin`). It
-  returns to the route only once the route corridor is free along its whole body for
-  `path_shift.return_delay_sec`.
-- The speed is limited so it can always stop before the first obstacle, both where it is and
-  where it is sliding to.
-
-The obstacle avoidance node still provides the front-cone emergency stop. With
-`avoidance_mode: reactive`, or any other controller, its steering is blended in instead. A
-holonomic robot then turns its direction of travel, not its heading.
-
-## Starting a patrol
-
-`navigation/start_nav` starts from wherever the robot is on the route, not only at its start:
-
-- A paused patrol resumes where it was while the robot is still within `off_path_slow_distance`
-  of its progress; otherwise (route finished, robot moved) the robot is located on the route again.
-- A robot further away than that walks slowly onto the nearest route point (`JOINING`, final
-  approach gains) from up to `join_max_distance`, then follows. Further away the start is refused.
-- Loading a route again, even the same file, starts it over; a redelivered route is ignored.
-
-## Interfaces
-
-| Interface | Type | |
-|---|---|---|
-| `navigation/start_nav`, `navigation/pause` | `std_srvs/SetBool` | run / hold |
-| `navigation/load_path` | `robot_interfaces/LoadPath` | load a route |
-| `navigation/status`, `navigation/mission_status` | status messages | state, lap, next stop, path offset |
-| `navigation/stop_point_event` | `robot_interfaces/StopPointEvent` | arrived / inspected / departed / skipped |
-| `navigation/markers`, `navigation/route_markers`, `navigation/avoidance_markers` | `MarkerArray` | debug drawing |
-| `perception/safety_scan` | `robot_interfaces/SectorScan` | input of path shift |
-| `cmd_vel` | `geometry_msgs/Twist` | output: `linear.x`, `linear.y`, `angular.z` (route it through `robot_bridge`) |
-
-Parameters: [config/navigation_params.yaml](config/navigation_params.yaml).
-`colcon test --packages-select robot_navigation` runs the controller, route and path-shift
-simulations. The simulator's end-to-end test ([robot_sim](../robot_sim)) drives the whole
-stack past a person on the route and stops it for a crowd.
diff --git a/src/robot_navigation/launch/navigation.launch.py b/src/robot_navigation/launch/navigation.launch.py
deleted file mode 100644
index 75633ef..0000000
--- a/src/robot_navigation/launch/navigation.launch.py
+++ /dev/null
@@ -1,60 +0,0 @@
-#!/usr/bin/env python3
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    default_params = os.path.join(
-        get_package_share_directory('robot_navigation'), 'config', 'navigation_params.yaml'
-    )
-    default_waypoints = '/home/robot/dev/Pashupati/map/example/example_waypoint.csv'
-
-    params_file = LaunchConfiguration('params_file')
-    waypoints_file = LaunchConfiguration('waypoints_file')
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument('params_file', default_value=default_params),
-            DeclareLaunchArgument('waypoints_file', default_value=default_waypoints),
-            DeclareLaunchArgument(
-                'cmd_vel_topic',
-                default_value='cmd_vel',
-                description="Robot velocity topic, e.g. '/robot/cmd_vel'",
-            ),
-            DeclareLaunchArgument(
-                'battery_topic',
-                default_value='battery',
-                description="sensor_msgs/BatteryState topic, e.g. '/robot/battery'",
-            ),
-            Node(
-                package='robot_navigation',
-                executable='path_loader_node',
-                name='path_loader_node',
-                output='screen',
-                parameters=[params_file, {'waypoints_file': waypoints_file}],
-            ),
-            Node(
-                package='robot_navigation',
-                executable='obstacle_avoidance_node',
-                name='obstacle_avoidance_node',
-                output='screen',
-                parameters=[params_file],
-            ),
-            Node(
-                package='robot_navigation',
-                executable='path_follower_node',
-                name='path_follower_node',
-                output='screen',
-                parameters=[params_file],
-                remappings=[
-                    ('cmd_vel', LaunchConfiguration('cmd_vel_topic')),
-                    ('battery', LaunchConfiguration('battery_topic')),
-                ],
-            ),
-        ]
-    )
diff --git a/src/robot_navigation/launch/path_follower.launch.py b/src/robot_navigation/launch/path_follower.launch.py
deleted file mode 100644
index e8a3035..0000000
--- a/src/robot_navigation/launch/path_follower.launch.py
+++ /dev/null
@@ -1,53 +0,0 @@
-#!/usr/bin/env python3
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    default_params = os.path.join(
-        get_package_share_directory('robot_navigation'), 'config', 'navigation_params.yaml'
-    )
-    default_waypoints = '/home/robot/dev/Pashupati/map/example/example_waypoint.csv'
-
-    params_file = LaunchConfiguration('params_file')
-    waypoints_file = LaunchConfiguration('waypoints_file')
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument('params_file', default_value=default_params),
-            DeclareLaunchArgument('waypoints_file', default_value=default_waypoints),
-            DeclareLaunchArgument(
-                'cmd_vel_topic',
-                default_value='cmd_vel',
-                description="Robot velocity topic, e.g. '/robot/cmd_vel'",
-            ),
-            DeclareLaunchArgument(
-                'battery_topic',
-                default_value='battery',
-                description="sensor_msgs/BatteryState topic, e.g. '/robot/battery'",
-            ),
-            Node(
-                package='robot_navigation',
-                executable='path_loader_node',
-                name='path_loader_node',
-                output='screen',
-                parameters=[params_file, {'waypoints_file': waypoints_file}],
-            ),
-            Node(
-                package='robot_navigation',
-                executable='path_follower_node',
-                name='path_follower_node',
-                output='screen',
-                parameters=[params_file],
-                remappings=[
-                    ('cmd_vel', LaunchConfiguration('cmd_vel_topic')),
-                    ('battery', LaunchConfiguration('battery_topic')),
-                ],
-            ),
-        ]
-    )
diff --git a/src/robot_navigation/package.xml b/src/robot_navigation/package.xml
deleted file mode 100644
index df162b4..0000000
--- a/src/robot_navigation/package.xml
+++ /dev/null
@@ -1,29 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>robot_navigation</name>
-  <version>0.5.0</version>
-  <description>Selectable path-following controllers (pure pursuit, PID, MPPI) and selectable reactive obstacle avoidance (Braitenberg, VFH) for the robot, with inspection stop-point support (align + dwell) and RViz debug visualization.</description>
-  <maintainer email="darmawandeveloper@gmail.com">Agus Darmawan</maintainer>
-  <license>Apache-2.0</license>
-
-  <depend>rclpy</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>std_srvs</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>robot_interfaces</depend>
-  <depend>python3-numpy</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/src/robot_navigation/resource/robot_navigation b/src/robot_navigation/resource/robot_navigation
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_navigation/robot_navigation/__init__.py b/src/robot_navigation/robot_navigation/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_navigation/robot_navigation/obstacle_avoidance_node.py b/src/robot_navigation/robot_navigation/obstacle_avoidance_node.py
deleted file mode 100644
index 650ea8b..0000000
--- a/src/robot_navigation/robot_navigation/obstacle_avoidance_node.py
+++ /dev/null
@@ -1,215 +0,0 @@
-#!/usr/bin/env python3
-import math
-
-from rcl_interfaces.msg import SetParametersResult
-import rclpy
-from rclpy.duration import Duration
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from robot_interfaces.msg import AvoidanceCommand, SectorScan
-from robot_navigation.utils import visualization as viz
-from robot_navigation.utils.avoidance.registry import create_avoidance
-from visualization_msgs.msg import MarkerArray
-
-ALGORITHM_PARAMS = {
-    'braitenberg': {
-        'steering_gain': 1.5,
-        'velocity_gain': 1.0,
-        'steering_zone_center_deg': 45.0,
-        'steering_zone_width_deg': 60.0,
-        'velocity_zone_center_deg': 60.0,
-        'velocity_zone_width_deg': 90.0,
-        'following_zone_width_deg': 30.0,
-        'smoothing': 0.85,
-    },
-    'vfh': {
-        'min_gap_width_deg': 20.0,
-        'steering_gain': 1.0,
-        'velocity_gain': 1.0,
-        'smoothing': 0.85,
-    },
-    'potential_field': {
-        'repulsion_gain': 0.5,
-        'steering_gain': 0.8,
-        'lateral_gain': 0.3,
-        'max_lateral': 0.2,
-        'velocity_gain': 1.0,
-        'smoothing': 0.8,
-    },
-    'follow_gap': {
-        'bubble_radius': 0.45,
-        'steering_gain': 1.0,
-        'velocity_gain': 1.0,
-        'smoothing': 0.85,
-    },
-}
-
-
-class ObstacleAvoidanceNode(Node):
-    """Runs a selectable reactive avoidance algorithm on SectorScan."""
-
-    def __init__(self):
-        """Declare params, build the initial algorithm, and wire the scan subscription/output."""
-        super().__init__('obstacle_avoidance_node')
-        self.declare_parameter('algorithm', 'vfh')
-        self.declare_parameter('safe_distance', 1.0)
-        self.declare_parameter('emergency_distance', 0.75)
-        self.declare_parameter('emergency_cone_deg', 60.0)
-        for algorithm, params in ALGORITHM_PARAMS.items():
-            for name, value in params.items():
-                self.declare_parameter(f'{algorithm}.{name}', value)
-
-        self._pending = False
-        self._algorithm_name = self.get_parameter('algorithm').value
-        self._avoidance = create_avoidance(
-            self._algorithm_name, self.build_params(self._algorithm_name)
-        )
-
-        self.create_subscription(SectorScan, 'perception/sector_scan', self.scan_callback, 10)
-        self._avoidance_pub = self.create_publisher(AvoidanceCommand, 'navigation/avoidance', 10)
-        self._markers_pub = self.create_publisher(MarkerArray, 'navigation/avoidance_markers', 10)
-        self.add_on_set_parameters_callback(self.on_parameters_changed)
-
-    def build_params(self, name: str) -> dict:
-        """Build constructor kwargs: shared distances plus the algorithm's namespaced params."""
-        if name not in ALGORITHM_PARAMS:
-            raise ValueError(
-                f'unknown avoidance algorithm {name!r}, options are {list(ALGORITHM_PARAMS)}'
-            )
-        params = {
-            key: float(self.get_parameter(key).value)
-            for key in ('safe_distance', 'emergency_distance', 'emergency_cone_deg')
-        }
-        for key in ALGORITHM_PARAMS[name]:
-            params[key] = float(self.get_parameter(f'{name}.{key}').value)
-        return params
-
-    def on_parameters_changed(self, params):
-        """Validate a parameter change; the algorithm is rebuilt on the next scan."""
-        for param in params:
-            if param.name == 'algorithm' and param.value not in ALGORITHM_PARAMS:
-                return SetParametersResult(
-                    successful=False,
-                    reason=f'unknown algorithm {param.value!r}, use {list(ALGORITHM_PARAMS)}',
-                )
-        self._pending = True
-        return SetParametersResult(successful=True)
-
-    def rebuild(self):
-        """Recreate the avoidance algorithm from the current parameters."""
-        name = self.get_parameter('algorithm').value
-        self._avoidance = create_avoidance(name, self.build_params(name))
-        if name != self._algorithm_name:
-            self.get_logger().info(f'switched avoidance algorithm to {name}')
-        else:
-            self.get_logger().info(f'applied {name} parameters')
-        self._algorithm_name = name
-
-    def scan_callback(self, msg: SectorScan):
-        """Run one avoidance update from the sector scan and publish the result."""
-        if self._pending:
-            self._pending = False
-            self.rebuild()
-        result = self._avoidance.update(
-            list(msg.ranges), msg.angle_min, msg.angle_increment, msg.range_max
-        )
-        out = AvoidanceCommand()
-        out.header = msg.header
-        out.steering_bias = float(result.steering_bias)
-        out.lateral_bias = float(result.lateral_bias)
-        out.velocity_scale = float(result.velocity_scale)
-        out.emergency = bool(result.emergency)
-        self._avoidance_pub.publish(out)
-        self.publish_markers(msg, result)
-
-    def publish_markers(self, scan: SectorScan, result):
-        """Draw the emergency cone, safe range and avoidance push in the robot frame."""
-        frame, stamp, ns = scan.header.frame_id or 'base_link', scan.header.stamp, 'avoidance'
-        half_cone = math.radians(float(self.get_parameter('emergency_cone_deg').value)) / 2.0
-        emergency = float(self.get_parameter('emergency_distance').value)
-        safe = float(self.get_parameter('safe_distance').value)
-        markers = MarkerArray()
-        markers.markers.append(
-            viz.cone(
-                frame,
-                stamp,
-                ns,
-                0,
-                half_cone,
-                emergency,
-                viz.ALARM,
-                alpha=0.6 if result.emergency else 0.15,
-            )
-        )
-        markers.markers.append(
-            viz.circle(
-                frame,
-                stamp,
-                ns,
-                1,
-                0.0,
-                0.0,
-                safe,
-                (1.0, 0.6, 0.1),
-                width=0.015,
-                alpha=0.5,
-                start=-math.pi / 2,
-                sweep=math.pi,
-            )
-        )
-        if abs(result.steering_bias) > 0.02:
-            markers.markers.append(
-                viz.arrow(
-                    frame,
-                    stamp,
-                    ns,
-                    2,
-                    0.0,
-                    0.0,
-                    max(-1.2, min(1.2, result.steering_bias)),
-                    0.3 + 0.5 * result.velocity_scale,
-                    (1.0, 0.6, 0.1),
-                )
-            )
-        if abs(result.lateral_bias) > 0.01:
-            markers.markers.append(
-                viz.arrow(
-                    frame,
-                    stamp,
-                    ns,
-                    3,
-                    0.0,
-                    0.0,
-                    math.copysign(math.pi / 2, result.lateral_bias),
-                    3.0 * abs(result.lateral_bias),
-                    (0.8, 0.3, 1.0),
-                )
-            )
-        lifetime = Duration(seconds=0.3).to_msg()
-        for m in markers.markers:
-            m.lifetime = lifetime
-        self._markers_pub.publish(markers)
-
-
-def main(args=None):
-    """Spin the obstacle avoidance node."""
-    rclpy.init(args=args)
-    node = ObstacleAvoidanceNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_navigation/robot_navigation/path_follower_node.py b/src/robot_navigation/robot_navigation/path_follower_node.py
deleted file mode 100644
index 9443be9..0000000
--- a/src/robot_navigation/robot_navigation/path_follower_node.py
+++ /dev/null
@@ -1,1428 +0,0 @@
-#!/usr/bin/env python3
-import math
-import threading
-
-from geometry_msgs.msg import Twist
-import numpy as np
-from rcl_interfaces.msg import SetParametersResult
-import rclpy
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
-from rclpy.duration import Duration
-from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
-from rclpy.time import Time
-from robot_interfaces.msg import (
-    AvoidanceCommand,
-    MissionStatus,
-    NavigationStatus,
-    SectorScan,
-    StopPointEvent,
-    WaypointPath,
-)
-from robot_navigation.utils import visualization
-from robot_navigation.utils.command_filter import LowPassFilter, SlewLimiter
-from robot_navigation.utils.controllers.holonomic_controller import HolonomicController
-from robot_navigation.utils.controllers.registry import create_controller
-from robot_navigation.utils.final_approach import FinalApproach
-from robot_navigation.utils.geometry import clamp, yaw_from_quaternion
-from robot_navigation.utils.path_processing import velocity_profile
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.path_shift import PathShift, PathShiftConfig
-from robot_navigation.utils.pose2d import Pose2D
-from robot_navigation.utils.safety import SafetySupervisor
-from robot_navigation.utils.speed_regulator import SpeedRegulator
-from sensor_msgs.msg import BatteryState
-from std_msgs.msg import String
-from std_srvs.srv import SetBool, Trigger
-from tf2_ros import Buffer, TransformException, TransformListener
-from visualization_msgs.msg import MarkerArray
-
-DEFAULT_PARAMS = {
-    'control_rate': 20.0,
-    'target_linear_velocity': 0.4,
-    'min_linear_velocity': 0.1,
-    'max_lateral_accel': 0.3,
-    'decel_limit': 0.3,
-    'approach_speed': 0.15,
-    'max_linear_velocity': 0.6,
-    'max_angular_velocity': 1.0,
-    'linear_accel_limit': 0.5,
-    'angular_accel_limit': 1.5,
-    'command_smoothing': 0.4,
-    'arc_visualization_length': 2.0,
-    'marker_lifetime_sec': 0.5,
-    'avoidance_enabled': True,
-    'avoidance_timeout_sec': 0.5,
-    'estop_release_sec': 1.0,
-    'blocked_timeout_sec': 30.0,
-    'off_path_slow_distance': 0.6,
-    'off_path_stop_distance': 1.5,
-    'tf_timeout_sec': 0.5,
-    'loop_route': True,
-    'loop_close_distance': 1.0,
-    'low_battery_percentage': 0.2,
-    'holonomic': True,
-    'avoidance_lateral_enabled': True,
-    'approach_radius': 0.5,
-    'approach_timeout_sec': 20.0,
-    'join_max_distance': 3.0,
-    'join_timeout_sec': 60.0,
-    'approach.kp_xy': 1.2,
-    'approach.kp_yaw': 1.5,
-    'approach.max_speed': 0.2,
-    'approach.max_yaw_rate': 0.6,
-    'approach.min_speed': 0.04,
-    'approach.position_tolerance': 0.05,
-    'approach.yaw_tolerance_deg': 3.0,
-    'stop_point_skip_margin': 5,
-    'auto_mode_on_service': '',
-    'auto_mode_off_service': '',
-    'inspection_services': [''],
-    'inspection_timeout_sec': 10.0,
-    'require_localization': True,
-    'controller': 'holonomic',
-    'avoidance_mode': 'path_shift',
-    'footprint.front': 0.40,
-    'footprint.rear': 0.40,
-    'footprint.half_width': 0.28,
-    'path_shift.max_offset': 0.8,
-    'path_shift.look_ahead': 2.0,
-    'path_shift.clearance': 0.10,
-    'path_shift.offset_rate': 0.3,
-    'path_shift.switch_cost': 0.5,
-    'path_shift.switch_margin': 0.05,
-    'path_shift.return_delay_sec': 0.5,
-    'path_shift.decel': 0.5,
-    'path_shift.stop_margin': 0.40,
-    'speed_regulator_enabled': False,
-    'speed_regulator.kp': 0.3,
-    'speed_regulator.ki': 0.0,
-    'speed_regulator.kd': 0.0,
-    'speed_regulator.min_scale': 0.3,
-}
-
-AVOIDANCE_MODES = ('path_shift', 'reactive')
-
-CONTROLLER_PARAMS = {
-    'holonomic': {
-        'lookahead_distance': 0.6,
-        'heading_lookahead': 0.6,
-        'goal_tolerance': 0.3,
-        'kp_yaw': 1.5,
-        'max_lateral_velocity': 0.25,
-        'max_reverse_velocity': 0.15,
-        'min_alignment_scale': 0.25,
-    },
-    'pure_pursuit': {
-        'lookahead_distance': 1.0,
-        'goal_tolerance': 0.3,
-    },
-    'pid': {
-        'lookahead_distance': 1.0,
-        'goal_tolerance': 0.3,
-        'kp': 1.5,
-        'ki': 0.0,
-        'kd': 0.2,
-    },
-    'mppi': {
-        'goal_tolerance': 0.3,
-        'horizon_steps': 15,
-        'dt': 0.1,
-        'num_samples': 200,
-        'angular_std': 1.0,
-        'temperature': 0.05,
-        'window_points': 60,
-    },
-    'lqr': {
-        'goal_tolerance': 0.3,
-        'q_lateral': 1.0,
-        'q_heading': 0.5,
-        'r_angular': 0.5,
-    },
-    'stanley': {
-        'goal_tolerance': 0.3,
-        'k_cross_track': 1.0,
-        'k_soft': 0.2,
-        'k_heading': 1.5,
-    },
-}
-
-STARTUP_ONLY_PARAMS = {
-    'control_rate',
-    'auto_mode_on_service',
-    'auto_mode_off_service',
-    'inspection_services',
-}
-
-
-class PathFollowerNode(Node):
-    """
-    Patrol a taught route with inspection stop points.
-
-    Follows the route with a selectable controller and at each stop point converges onto x/y/yaw,
-    triggers the inspection services and dwells. With the holonomic controller (legged robots) it
-    walks forward, sideways or backwards as the route needs and, in path_shift avoidance, slides
-    sideways around obstacles within path_shift.max_offset instead of steering away; other
-    controllers blend the reactive avoidance node's steering.
-    """
-
-    _WARN_STATES = {
-        'NO_PATH',
-        'TF_UNAVAILABLE',
-        'EMERGENCY_STOP',
-        'AVOIDANCE_STALE',
-        'BLOCKED',
-        'OFF_PATH',
-    }
-
-    def __init__(self):
-        """Declare params, set up TF, build the controller, and wire all interfaces."""
-        super().__init__('path_follower_node')
-        self.declare_params()
-        p = self.get_parameter
-
-        self._control_rate = float(p('control_rate').value)
-        self._controller = None
-        self._controller_name = ''
-        self._approach = None
-        self._linear_limiter = self._angular_limiter = self._lateral_limiter = None
-        self._angular_filter = self._lateral_filter = None
-        self._safety = None
-        self._speed_regulator = None
-        self._pending_params: set[str] = set()
-        self._pending_lock = threading.Lock()
-
-        self._avoidance = AvoidanceCommand(velocity_scale=1.0)
-        self._avoidance_time = None
-        self._scan_points = np.empty((0, 2))   # safety scan obstacles in base_link
-        self._scan_time = None
-        self._path_offset = 0.0                # applied lateral offset from the route (+left)
-        self._shift_target = 0.0
-        self._shift_free = math.inf
-        self._return_since = None              # when a smaller offset was first proposed
-        self._nav_active = False
-        self._paused = False
-        self._battery_low = False
-        self._waypoints: list[tuple[float, float, float, float]] = []
-        self._speed_profile: list[float] = []
-        self._next_stop_index = None
-        self._follower_state = 'FOLLOWING'
-        self._approach_start_time = None
-        self._approach_index = 0
-        self._approach_is_goal = False
-        self._join_index = 0
-        self._join_start_time = None
-        self._waypoints_stamp = None
-        self._dwell_duration = 0.0
-        self._dwell_start_time = None
-        self._inspection_pending = 0
-        self._inspection_results: list[str] = []
-        self._inspection_index = None
-        self._inspection_round = 0
-        self._lap = 0
-        self._last_status = 'NAV_INACTIVE'
-
-        self.load_tunables()
-
-        self._tf_buffer = Buffer()
-        self._tf_listener = TransformListener(self._tf_buffer, self)
-
-        self._srv_group = ReentrantCallbackGroup()
-        self._loop_group = MutuallyExclusiveCallbackGroup()
-        latched = QoSProfile(
-            reliability=QoSReliabilityPolicy.RELIABLE,
-            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
-            history=QoSHistoryPolicy.KEEP_LAST,
-            depth=1,
-        )
-        self.create_subscription(
-            WaypointPath,
-            'navigation/waypoints',
-            self.waypoints_callback,
-            latched,
-            callback_group=self._loop_group,
-        )
-        self.create_subscription(
-            AvoidanceCommand,
-            'navigation/avoidance',
-            self.avoidance_callback,
-            10,
-            callback_group=self._loop_group,
-        )
-        self.create_subscription(
-            SectorScan,
-            'perception/safety_scan',
-            self.safety_scan_callback,
-            10,
-            callback_group=self._loop_group,
-        )
-        self._cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
-        self._markers_pub = self.create_publisher(MarkerArray, 'navigation/markers', 10)
-        self._route_markers_pub = self.create_publisher(
-            MarkerArray, 'navigation/route_markers', latched
-        )
-        self._route_markers_key = None
-        self._status_pub = self.create_publisher(NavigationStatus, 'navigation/status', 10)
-        self._stop_event_pub = self.create_publisher(
-            StopPointEvent, 'navigation/stop_point_event', 10
-        )
-
-        self._auto_on_client = self.optional_client(p('auto_mode_on_service').value)
-        self._auto_off_client = self.optional_client(p('auto_mode_off_service').value)
-        self._inspection_clients = [
-            self.create_client(Trigger, name, callback_group=self._srv_group)
-            for name in p('inspection_services').value
-            if name
-        ]
-        self.create_service(
-            SetBool,
-            'navigation/start_nav',
-            self.start_nav_callback,
-            callback_group=self._srv_group,
-        )
-        self.create_service(
-            SetBool, 'navigation/pause', self.pause_callback, callback_group=self._srv_group
-        )
-        self.create_subscription(
-            BatteryState, 'battery', self.battery_callback, 10, callback_group=self._loop_group
-        )
-        self._mission_pub = self.create_publisher(MissionStatus, 'navigation/mission_status', 10)
-        self._localization_source = 'none'
-        self.create_subscription(
-            String,
-            'localization/status',
-            self.localization_status_callback,
-            latched,
-            callback_group=self._loop_group,
-        )
-        self.create_timer(1.0, self.publish_mission_status, callback_group=self._loop_group)
-        self.add_on_set_parameters_callback(self.on_parameters_changed)
-
-        self._last_time = self.get_clock().now()
-        self.create_timer(
-            1.0 / self._control_rate, self.control_loop, callback_group=self._loop_group
-        )
-
-    def declare_params(self):
-        """Declare the general parameters and every controller's namespaced parameters."""
-        for name, value in DEFAULT_PARAMS.items():
-            self.declare_parameter(name, value)
-        for controller, params in CONTROLLER_PARAMS.items():
-            for name, value in params.items():
-                self.declare_parameter(f'{controller}.{name}', value)
-
-    def optional_client(self, name: str):
-        """Create a Trigger client, or None when the service name is empty."""
-        return self.create_client(Trigger, name, callback_group=self._srv_group) if name else None
-
-    def build_controller_params(self, name: str) -> dict:
-        """Build constructor kwargs for a controller from its namespaced parameters."""
-        if name not in CONTROLLER_PARAMS:
-            raise ValueError(f'unknown controller {name!r}, options are {list(CONTROLLER_PARAMS)}')
-        params = {
-            key: type(default)(self.get_parameter(f'{name}.{key}').value)
-            for key, default in CONTROLLER_PARAMS[name].items()
-        }
-        if name in ('holonomic', 'mppi', 'lqr', 'stanley'):
-            params['max_angular_velocity'] = self._max_angular_velocity
-        if name == 'holonomic' and not self._holonomic:
-            params['max_lateral_velocity'] = 0.0  # the robot cannot strafe
-        if name == 'lqr':
-            params['dt'] = 1.0 / self._control_rate
-        return params
-
-    def load_tunables(self, changed: set | None = None):
-        """(Re)read runtime-tunable parameters; rebuild only the parts whose parameters changed."""
-        p = self.get_parameter
-        everything = changed is None
-        changed = changed or set()
-
-        def touched(*prefixes):
-            return everything or any(n.startswith(prefixes) for n in changed)
-
-        self._target_linear_velocity = float(p('target_linear_velocity').value)
-        self._min_linear_velocity = float(p('min_linear_velocity').value)
-        self._max_lateral_accel = float(p('max_lateral_accel').value)
-        self._decel_limit = float(p('decel_limit').value)
-        self._approach_speed = float(p('approach_speed').value)
-        self._max_linear_velocity = float(p('max_linear_velocity').value)
-        self._max_angular_velocity = float(p('max_angular_velocity').value)
-        self._arc_length = float(p('arc_visualization_length').value)
-        self._marker_lifetime = Duration(seconds=float(p('marker_lifetime_sec').value)).to_msg()
-        self._avoidance_enabled = bool(p('avoidance_enabled').value)
-        self._avoidance_lateral = bool(p('avoidance_lateral_enabled').value)
-        self._speed_regulator_enabled = bool(p('speed_regulator_enabled').value)
-        self._avoidance_timeout = Duration(seconds=float(p('avoidance_timeout_sec').value))
-        self._tf_timeout = Duration(seconds=float(p('tf_timeout_sec').value))
-        self._loop_route = bool(p('loop_route').value)
-        self._loop_close_distance = float(p('loop_close_distance').value)
-        self._low_battery = float(p('low_battery_percentage').value)
-        self._approach_radius = float(p('approach_radius').value)
-        self._approach_timeout = float(p('approach_timeout_sec').value)
-        self._approach_tolerance = float(p('approach.position_tolerance').value)
-        self._holonomic = bool(p('holonomic').value)
-        self._stop_point_skip_margin = int(p('stop_point_skip_margin').value)
-        self._inspection_timeout = float(p('inspection_timeout_sec').value)
-        self._require_localization = bool(p('require_localization').value)
-        self._avoidance_mode = str(p('avoidance_mode').value)
-        self._offset_rate = float(p('path_shift.offset_rate').value)
-        self._return_delay = float(p('path_shift.return_delay_sec').value)
-
-        if touched('approach.', 'holonomic'):
-            self._approach = FinalApproach(
-                holonomic=self._holonomic,
-                kp_xy=float(p('approach.kp_xy').value),
-                kp_yaw=float(p('approach.kp_yaw').value),
-                max_speed=float(p('approach.max_speed').value),
-                max_yaw_rate=float(p('approach.max_yaw_rate').value),
-                min_speed=float(p('approach.min_speed').value),
-                position_tolerance=float(p('approach.position_tolerance').value),
-                yaw_tolerance=math.radians(float(p('approach.yaw_tolerance_deg').value)),
-            )
-        if touched('linear_accel_limit', 'angular_accel_limit'):
-            self._linear_limiter = self.carry(
-                self._linear_limiter, SlewLimiter(float(p('linear_accel_limit').value))
-            )
-            self._lateral_limiter = self.carry(
-                self._lateral_limiter, SlewLimiter(float(p('linear_accel_limit').value))
-            )
-            self._angular_limiter = self.carry(
-                self._angular_limiter, SlewLimiter(float(p('angular_accel_limit').value))
-            )
-        if touched('command_smoothing'):
-            alpha = float(p('command_smoothing').value)
-            self._angular_filter = self.carry(self._angular_filter, LowPassFilter(alpha))
-            self._lateral_filter = self.carry(self._lateral_filter, LowPassFilter(alpha))
-        if touched('estop_release_sec', 'blocked_timeout_sec', 'off_path_'):
-            self._safety = SafetySupervisor(
-                estop_release_sec=float(p('estop_release_sec').value),
-                blocked_timeout_sec=float(p('blocked_timeout_sec').value),
-                off_path_slow_distance=float(p('off_path_slow_distance').value),
-                off_path_stop_distance=float(p('off_path_stop_distance').value),
-            )
-        if touched('path_shift.', 'footprint.'):
-            self._path_shift = PathShift(
-                PathShiftConfig(
-                    max_offset=float(p('path_shift.max_offset').value),
-                    look_ahead=float(p('path_shift.look_ahead').value),
-                    front=float(p('footprint.front').value),
-                    rear=float(p('footprint.rear').value),
-                    half_width=float(p('footprint.half_width').value),
-                    clearance=float(p('path_shift.clearance').value),
-                    switch_cost=float(p('path_shift.switch_cost').value),
-                    switch_margin=float(p('path_shift.switch_margin').value),
-                    decel=float(p('path_shift.decel').value),
-                    stop_margin=float(p('path_shift.stop_margin').value),
-                )
-            )
-        if touched('speed_regulator.'):
-            self._speed_regulator = SpeedRegulator(
-                kp=float(p('speed_regulator.kp').value),
-                ki=float(p('speed_regulator.ki').value),
-                kd=float(p('speed_regulator.kd').value),
-                min_scale=float(p('speed_regulator.min_scale').value),
-            )
-        name = p('controller').value
-        if touched('controller', 'max_angular_velocity', 'holonomic', f'{name}.'):
-            self.build_controller(name)
-        if touched(
-            'target_linear_velocity',
-            'min_linear_velocity',
-            'max_lateral_accel',
-            'decel_limit',
-            'approach_speed',
-        ):
-            self.compute_speed_profile()
-        if not everything:
-            self.get_logger().info(f'applied parameters: {", ".join(sorted(changed))}')
-
-    @staticmethod
-    def carry(old, new):
-        """Return new, starting from the old filter/limiter output so commands do not jump."""
-        if old is not None:
-            new.reset(old.value)
-        return new
-
-    def build_controller(self, name: str):
-        """Instantiate a controller, keeping the current route and progress."""
-        progress = self._controller.progress_index() if self._controller else None
-        self._controller = create_controller(name, self.build_controller_params(name))
-        if self._waypoints:
-            self._controller.set_path([(x, y) for x, y, _, _ in self._waypoints], progress)
-        if name != self._controller_name and self._controller_name:
-            self.get_logger().info(f'switched controller to {name}')
-        self._controller_name = name
-        self.reset_path_shift()
-
-    def compute_speed_profile(self):
-        """Recompute the per-point speed limits for the current route."""
-        self._speed_profile = velocity_profile(
-            [(w[0], w[1]) for w in self._waypoints],
-            [i for i, w in enumerate(self._waypoints) if w[3] > 0.0],
-            cruise=self._target_linear_velocity,
-            min_speed=self._min_linear_velocity,
-            max_lateral_accel=self._max_lateral_accel,
-            decel=self._decel_limit,
-            approach_speed=self._approach_speed,
-        )
-
-    def on_parameters_changed(self, params):
-        """Validate a parameter change; it is applied on the next control tick."""
-        for param in params:
-            if param.name in STARTUP_ONLY_PARAMS:
-                return SetParametersResult(
-                    successful=False, reason=f'{param.name} is read at startup, restart the node'
-                )
-            if param.name == 'controller' and param.value not in CONTROLLER_PARAMS:
-                return SetParametersResult(
-                    successful=False,
-                    reason=f'unknown controller {param.value!r}, use {list(CONTROLLER_PARAMS)}',
-                )
-            if param.name == 'avoidance_mode' and param.value not in AVOIDANCE_MODES:
-                return SetParametersResult(
-                    successful=False,
-                    reason=f'unknown avoidance_mode {param.value!r}, use {list(AVOIDANCE_MODES)}',
-                )
-        with self._pending_lock:
-            self._pending_params.update(param.name for param in params)
-        return SetParametersResult(successful=True)
-
-    def find_next_stop_index(self, from_index: int):
-        """Index of the next waypoint with dwell_sec > 0 at or after from_index, or None."""
-        for i in range(from_index, len(self._waypoints)):
-            if self._waypoints[i][3] > 0.0:
-                return i
-        return None
-
-    def restart_route(self, from_start: bool = False):
-        """Reset controller progress and the stop-point state machine to the start of the route."""
-        self._controller.set_path(
-            [(x, y) for x, y, _, _ in self._waypoints], 0 if from_start else None
-        )
-        self._speed_regulator.reset()
-        self.reset_path_shift()
-        self._follower_state = 'FOLLOWING'
-        self._next_stop_index = self.find_next_stop_index(0)
-
-    def find_robot_on_route(self) -> tuple[bool, str]:
-        """
-        On start: keep the progress if the robot is still near it, else search the whole route.
-
-        Progress survives a stop so a paused patrol resumes where it was; after the route end or
-        once the robot was moved elsewhere the old progress would report the route as finished or
-        the robot as far off it, so the robot is located on the route again. A robot up to
-        join_max_distance away first walks onto the route (JOINING); further away the start is
-        refused. Returns (start allowed, note).
-        """
-        if not self._waypoints:
-            return True, ''
-        pose = self.current_pose()
-        if pose is None:
-            return True, ''
-        held = self._follower_state == 'GOAL_HOLD'
-        near = float(self.get_parameter('off_path_slow_distance').value)
-        if not held and self.cross_track_error(pose) <= near:
-            return True, ''
-        path = [(x, y) for x, y, _, _ in self._waypoints]
-        locator = PathProgress()
-        locator.set_path(path)
-        index = locator.update(pose)
-        distance = math.hypot(path[index][0] - pose.x, path[index][1] - pose.y)
-        join_max = float(self.get_parameter('join_max_distance').value)
-        stop = float(self.get_parameter('off_path_stop_distance').value)
-        if distance > max(join_max, stop):
-            return False, (
-                f'robot is {distance:.1f} m from the route (join_max_distance '
-                f'{join_max:.1f} m): drive it closer or load the route it is on'
-            )
-        self.restart_route()
-        self._controller.set_path(path, index)
-        self._next_stop_index = self.find_next_stop_index(index)
-        self.get_logger().info(f'robot on the route at point {index}, {distance:.2f} m away')
-        if distance > near and join_max > 0.0:
-            self.start_join(index)
-            return True, f'joining the route at point {index}, {distance:.1f} m away'
-        return True, f'progress found again on the route at point {index}'
-
-    def waypoints_callback(self, msg: WaypointPath):
-        """
-        Load a new route; a redelivered one (same stamp) is ignored so progress is kept.
-
-        Loading the same file again publishes it with a new stamp and starts it over.
-        """
-        waypoints = [(w.x, w.y, w.yaw, w.dwell_sec) for w in msg.waypoints]
-        stamp = (msg.header.stamp.sec, msg.header.stamp.nanosec)
-        if waypoints == self._waypoints and stamp == self._waypoints_stamp:
-            return
-        self._waypoints = waypoints
-        self._waypoints_stamp = stamp
-        self.compute_speed_profile()
-        self._lap = 0
-        self.restart_route()
-        stops = sum(1 for w in waypoints if w[3] > 0.0)
-        self.get_logger().info(f'route loaded: {len(waypoints)} waypoints, {stops} stop points')
-
-    def avoidance_callback(self, msg: AvoidanceCommand):
-        """Cache the latest avoidance signal and its arrival time."""
-        self._avoidance = msg
-        self._avoidance_time = self.get_clock().now()
-
-    def safety_scan_callback(self, msg: SectorScan):
-        """
-        Keep the obstacles of the 360 deg safety scan as points in base_link.
-
-        A sector only says something lies within its angle at that range, so it becomes three
-        points (centre and both edges): an obstacle is never thinner than its sectors.
-        """
-        ranges = np.asarray(msg.ranges, dtype=float)
-        start = msg.angle_min + np.arange(ranges.size) * msg.angle_increment
-        hit = np.isfinite(ranges) & (ranges < msg.range_max - 1e-3)
-        r = np.repeat(ranges[hit], 3)
-        a = (start[hit][:, None] + msg.angle_increment * np.array([0.0, 0.5, 1.0])).ravel()
-        self._scan_points = np.column_stack([r * np.cos(a), r * np.sin(a)])
-        self._scan_time = self.get_clock().now()
-
-    def reset_path_shift(self):
-        """Back onto the route itself (new route, lap, stop point or controller)."""
-        self._path_offset = self._shift_target = 0.0
-        self._shift_free = math.inf
-        self._return_since = None
-        if isinstance(self._controller, HolonomicController):
-            self._controller.set_offset(0.0)
-
-    def path_shifting(self) -> bool:
-        """Return True when obstacles are passed by sliding sideways (holonomic controller)."""
-        return (
-            self._avoidance_enabled
-            and self._avoidance_mode == 'path_shift'
-            and isinstance(self._controller, HolonomicController)
-        )
-
-    def update_path_shift(self, pose: Pose2D, dt: float):
-        """
-        Slide toward the offset that passes obstacles; return the speed limit, or None if stale.
-
-        The offset changes at most path_shift.offset_rate per second. The speed is limited so the
-        robot can stop before the first obstacle both where it is and where it is going.
-        """
-        if (
-            self._scan_time is None
-            or self.get_clock().now() - self._scan_time > self._avoidance_timeout
-        ):
-            return None
-        progress = self._controller.path_progress()
-        s_robot = progress.projection(pose)
-        c, s = math.cos(pose.yaw), math.sin(pose.yaw)
-        local = self._scan_points
-        points = np.empty((0, 2))
-        if local.size:
-            points = np.column_stack(
-                [
-                    pose.x + c * local[:, 0] - s * local[:, 1],
-                    pose.y + s * local[:, 0] + c * local[:, 1],
-                ]
-            )
-        shift = self._path_shift
-        s_pts, d_pts = shift.path_coordinates(progress.path, progress.arc_lengths, s_robot, points)
-        self.set_shift_target(
-            shift.choose(s_pts, d_pts, s_robot, self._shift_target).offset, pose
-        )
-        target = self._shift_target
-        self._path_offset += clamp(target - self._path_offset, self._offset_rate * dt)
-        self._controller.set_offset(self._path_offset)
-        self._shift_free = min(
-            shift.free_at(s_pts, d_pts, s_robot, target),
-            shift.free_at(s_pts, d_pts, s_robot, self._path_offset),
-        )
-        return shift.speed_limit(self._shift_free)
-
-    def call_trigger(self, client, timeout: float = 2.0):
-        """Call a Trigger service and wait for it (requires the multi-threaded executor)."""
-        if client is None:
-            return True, 'skipped'
-        if not client.wait_for_service(timeout_sec=timeout):
-            return False, f'{client.srv_name} not available'
-        done = threading.Event()
-        future = client.call_async(Trigger.Request())
-        future.add_done_callback(lambda _: done.set())
-        if not done.wait(timeout) or future.result() is None:
-            return False, f'{client.srv_name} timed out'
-        return future.result().success, future.result().message
-
-    def start_nav_callback(self, request, response):
-        """Enable or disable navigation, switching the robot's control mode along with it."""
-        if request.data:
-            if self._require_localization and self._localization_source == 'none':
-                response.success = False
-                response.message = (
-                    "not localized: call 'localization/start' or set an RViz 2D Pose Estimate"
-                )
-                self.get_logger().error(response.message)
-                return response
-            located, note = self.find_robot_on_route()
-            if not located:
-                response.success = False
-                response.message = note
-                self.get_logger().error(note)
-                return response
-            ok, message = self.call_trigger(self._auto_on_client)
-            if not ok:
-                response.success = False
-                response.message = f'failed to enable auto mode: {message}'
-                self.get_logger().error(response.message)
-                return response
-            self._safety.reset()
-            self._paused = False
-            self._nav_active = True
-            response.success = True
-            response.message = 'navigation started' + (f' ({note})' if note else '')
-            self.get_logger().info(response.message)
-            return response
-
-        self._nav_active = False
-        self.publish_stop(hard=True)
-        ok, message = self.call_trigger(self._auto_off_client)
-        if not ok:
-            self.get_logger().error(f'failed to switch back to manual mode: {message}')
-        response.success = True
-        response.message = 'navigation stopped'
-        self.get_logger().info(response.message)
-        return response
-
-    def localization_status_callback(self, msg: String):
-        """Track which source set map->odom ('none' until localized)."""
-        if msg.data != self._localization_source:
-            self.get_logger().info(f'localization source: {msg.data}')
-        self._localization_source = msg.data
-        lost = msg.data == 'none' and self._require_localization
-        if lost and self._nav_active and not self._paused:
-            # e.g. FAST-LIO restarted: the route is now in a different frame than the robot
-            self._paused = True
-            self.publish_stop(hard=True)
-            self.get_logger().error('localization lost, navigation paused until relocalized')
-
-    def pause_callback(self, request, response):
-        """Pause (hold position, keep progress) or resume navigation."""
-        self._paused = bool(request.data)
-        if self._paused:
-            self.publish_stop(hard=True)
-        response.success = True
-        response.message = 'paused' if self._paused else 'resumed'
-        self.get_logger().info(f'navigation {response.message}')
-        return response
-
-    def battery_callback(self, msg: BatteryState):
-        """
-        Flag low battery so the patrol ends after this lap; clear it again once charged.
-
-        Unknown readings (NaN, outside 0..1) are ignored. The flag clears only 5 % above the
-        threshold so a sagging battery does not toggle it.
-        """
-        pct = msg.percentage
-        if not 0.0 <= pct <= 1.0:
-            return
-        if pct < self._low_battery and not self._battery_low:
-            self._battery_low = True
-            self.get_logger().warn(f'battery {pct * 100:.0f}%, finishing this lap and stopping')
-        elif self._battery_low and pct >= self._low_battery + 0.05:
-            self._battery_low = False
-            self.get_logger().info(f'battery {pct * 100:.0f}%, patrol may loop again')
-
-    def publish_mission_status(self):
-        """Publish a 1 Hz mission summary for dashboards."""
-        msg = MissionStatus()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.active = self._nav_active and not self._paused
-        msg.root_status = self._last_status
-        stops = sum(1 for w in self._waypoints if w[3] > 0.0)
-        flags = ' | battery low' if self._battery_low else ''
-        msg.active_behavior = (
-            f'lap {self._lap} | next stop {self._next_stop_index} of {stops} | '
-            f'{self._follower_state}{flags}'
-        )
-        try:
-            self._mission_pub.publish(msg)
-        except Exception:  # noqa: B902  shutdown took the context away
-            if rclpy.ok():
-                raise
-
-    def current_pose(self):
-        """Look up map->base_link; None if unavailable or older than tf_timeout_sec."""
-        try:
-            t = self._tf_buffer.lookup_transform('map', 'base_link', Time())
-        except TransformException as error:
-            self.get_logger().warn(f'TF map->base_link failed: {error}', throttle_duration_sec=2.0)
-            return None
-        stamp = Time.from_msg(t.header.stamp)
-        if stamp.nanoseconds > 0 and self.get_clock().now() - stamp > self._tf_timeout:
-            self.get_logger().warn(
-                'TF map->base_link is stale (odometry stopped?)', throttle_duration_sec=2.0
-            )
-            return None
-        p = t.transform.translation
-        return Pose2D(p.x, p.y, yaw_from_quaternion(t.transform.rotation))
-
-    def avoidance_signal(self):
-        """Return (steering, lateral, velocity_scale, emergency, stale) from avoidance."""
-        if not self._avoidance_enabled:
-            return 0.0, 0.0, 1.0, False, False
-        stale = (
-            self._avoidance_time is None
-            or self.get_clock().now() - self._avoidance_time > self._avoidance_timeout
-        )
-        a = self._avoidance
-        lateral = a.lateral_bias if self._holonomic and self._avoidance_lateral else 0.0
-        return a.steering_bias, lateral, a.velocity_scale, a.emergency, stale
-
-    def publish_status(self, stamp, status, message):
-        """Publish NavigationStatus, logging once per state transition."""
-        if status != self._last_status:
-            # one call site per severity: rclpy raises if a call site changes its severity
-            if status in self._WARN_STATES:
-                self.get_logger().warn(f'[{status}] {message}')
-            else:
-                self.get_logger().info(f'[{status}] {message}')
-        self._last_status = status
-        msg = NavigationStatus()
-        msg.header.stamp = stamp
-        msg.header.frame_id = 'map'
-        msg.state = status
-        msg.message = message
-        self._status_pub.publish(msg)
-
-    def publish_markers(self, stamp, status, message, pose_x, pose_y, debug=None):
-        """Publish the status and the per-tick debug drawing (no status text)."""
-        self.publish_status(stamp, status, message)
-        debug = debug or {}
-        v = visualization
-        ns, items = 'follower', []
-        color = v.state_color(status)
-        if status not in ('NAV_INACTIVE', 'NO_PATH', 'TF_UNAVAILABLE'):
-            items.append(v.disc('map', stamp, ns, 0, pose_x, pose_y, 0.35, color, alpha=0.25))
-            items.append(v.circle('map', stamp, ns, 1, pose_x, pose_y, 0.35, color, width=0.03))
-        lookahead = debug.get('lookahead_xy')
-        if lookahead is not None:
-            lx, ly = lookahead
-            items.append(
-                v.line('map', stamp, ns, 2, [(pose_x, pose_y), (lx, ly)], (0.1, 0.9, 1.0))
-            )
-            items.append(v.sphere('map', stamp, ns, 3, lx, ly, 0.16, (0.1, 0.9, 1.0)))
-            if debug.get('lookahead_distance'):
-                items.append(
-                    v.circle(
-                        'map',
-                        stamp,
-                        ns,
-                        4,
-                        pose_x,
-                        pose_y,
-                        debug['lookahead_distance'],
-                        (0.1, 0.9, 1.0),
-                        width=0.01,
-                        alpha=0.35,
-                    )
-                )
-        nearest = debug.get('nearest_xy')
-        if nearest is not None:
-            nx, ny = nearest
-            items.append(v.sphere('map', stamp, ns, 5, nx, ny, 0.1, (1.0, 0.6, 0.1)))
-            items.append(
-                v.line(
-                    'map',
-                    stamp,
-                    ns,
-                    6,
-                    [(pose_x, pose_y), (nx, ny)],
-                    visualization.ALARM,
-                    width=0.015,
-                    alpha=0.7,
-                )
-            )
-        if debug.get('curvature') is not None:
-            items.append(
-                v.arc_from_curvature(
-                    'base_link',
-                    stamp,
-                    ns,
-                    7,
-                    debug['curvature'],
-                    self._arc_length,
-                    (0.3, 0.6, 1.0),
-                )
-            )
-        if debug.get('rollout_xy') is not None:
-            items.append(
-                v.line(
-                    'map',
-                    stamp,
-                    ns,
-                    8,
-                    debug['rollout_xy'],
-                    (1.0, 0.4, 0.9),
-                    width=0.02,
-                    alpha=0.8,
-                )
-            )
-        target = debug.get('approach')
-        if target is not None:
-            tx, ty, tyaw, tolerance, progress = target
-            items.append(
-                v.disc(
-                    'map', stamp, ns, 9, tx, ty, max(tolerance, 0.03), (1.0, 0.85, 0.1), alpha=0.5
-                )
-            )
-            items.append(v.arrow('map', stamp, ns, 10, tx, ty, tyaw, 0.4, (1.0, 0.85, 0.1)))
-            items.append(
-                v.line(
-                    'map',
-                    stamp,
-                    ns,
-                    11,
-                    [(pose_x, pose_y), (tx, ty)],
-                    (1.0, 0.85, 0.1),
-                    width=0.015,
-                    alpha=0.6,
-                )
-            )
-            if progress > 0.0:
-                items.append(
-                    v.circle(
-                        'map',
-                        stamp,
-                        ns,
-                        12,
-                        tx,
-                        ty,
-                        0.3,
-                        (0.2, 0.6, 1.0),
-                        width=0.05,
-                        start=math.pi / 2,
-                        sweep=-2 * math.pi * min(progress, 1.0),
-                    )
-                )
-        markers = MarkerArray()
-        for m in items:
-            m.lifetime = self._marker_lifetime
-            markers.markers.append(m)
-        self._markers_pub.publish(markers)
-
-    def publish_route_markers(self):
-        """Publish the latched route drawing: speed-coloured path, start/end and stop points."""
-        key = (
-            len(self._waypoints),
-            self._next_stop_index,
-            self._lap,
-            self._target_linear_velocity,
-        )
-        if key == self._route_markers_key:
-            return
-        self._route_markers_key = key
-        v, stamp, ns = visualization, self.get_clock().now().to_msg(), 'route'
-        markers = MarkerArray()
-        markers.markers.append(v.delete_all('map', stamp))
-        if self._waypoints:
-            path = v.base('map', stamp, ns, 0, v.Marker.LINE_STRIP)
-            path.scale.x = 0.06
-            vmax = max(self._speed_profile) if self._speed_profile else 1.0
-            for i, w in enumerate(self._waypoints):
-                path.points.append(v.pt(w[0], w[1], 0.01))
-                speed = self._speed_profile[i] if i < len(self._speed_profile) else vmax
-                path.colors.append(v.speed_color(speed, vmax))
-            markers.markers.append(path)
-            sx, sy = self._waypoints[0][:2]
-            ex, ey = self._waypoints[-1][:2]
-            markers.markers.append(v.sphere('map', stamp, ns, 1, sx, sy, 0.25, (0.2, 0.9, 0.3)))
-            markers.markers.append(v.sphere('map', stamp, ns, 2, ex, ey, 0.2, (0.9, 0.2, 0.2)))
-            stops = [i for i, w in enumerate(self._waypoints) if w[3] > 0.0]
-            for n, i in enumerate(stops):
-                x, y, yaw_deg, dwell = self._waypoints[i]
-                nxt = self._next_stop_index
-                rgb = (
-                    (0.5, 0.5, 0.5)
-                    if nxt is not None and i < nxt
-                    else (1.0, 0.85, 0.1) if i == nxt else (0.3, 0.6, 1.0)
-                )
-                markers.markers.append(v.disc('map', stamp, ns, 10 + 3 * n, x, y, 0.25, rgb, 0.6))
-                markers.markers.append(
-                    v.arrow('map', stamp, ns, 11 + 3 * n, x, y, math.radians(yaw_deg), 0.45, rgb)
-                )
-                markers.markers.append(
-                    v.label('map', stamp, ns, 12 + 3 * n, x, y, f'S{n + 1} {dwell:.0f}s', rgb)
-                )
-        self._route_markers_pub.publish(markers)
-
-    def publish_stop_event(self, event: str, message: str = '', index=None):
-        """Publish a StopPointEvent for the current (or the given) stop point."""
-        index = self._next_stop_index if index is None else index
-        if index is None or index >= len(self._waypoints):
-            return
-        x, y, yaw_deg, dwell = self._waypoints[index]
-        msg = StopPointEvent()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.header.frame_id = 'map'
-        msg.index = index
-        msg.x, msg.y, msg.yaw_deg, msg.dwell_sec = float(x), float(y), float(yaw_deg), float(dwell)
-        msg.event = event
-        msg.message = message
-        self._stop_event_pub.publish(msg)
-
-    def start_inspection(self):
-        """Fire every inspection Trigger service asynchronously for the current stop."""
-        self._inspection_round += 1
-        self._inspection_index = self._next_stop_index
-        self._inspection_results = []
-        self._inspection_pending = 0
-        for client in self._inspection_clients:
-            if not client.service_is_ready():
-                self._inspection_results.append(f'{client.srv_name}: not available')
-                continue
-            self._inspection_pending += 1
-            future = client.call_async(Trigger.Request())
-            future.add_done_callback(
-                lambda f, name=client.srv_name, r=self._inspection_round: self.inspection_done(
-                    name, f, r
-                )
-            )
-        if self._inspection_results and not self._inspection_pending:
-            # every inspector is down: report that now, the stop is not silently uninspected
-            self.publish_stop_event(
-                'inspected', ' | '.join(self._inspection_results), self._inspection_index
-            )
-
-    def inspection_done(self, name: str, future, round_id: int):
-        """Collect one inspection result; answers for an earlier stop are dropped."""
-        if round_id != self._inspection_round:
-            self.get_logger().warn(f'{name} answered after the robot left the stop, ignored')
-            return
-        try:
-            result = future.result()
-        except Exception as error:  # noqa: B902  a failed service is a failed inspection
-            result, error_text = None, str(error)
-        else:
-            error_text = ''
-        verdict = 'ok' if result and result.success else 'fail'
-        detail = result.message if result else error_text
-        self._inspection_results.append(f'{name}: {verdict} {detail}'.strip())
-        self._inspection_pending = max(0, self._inspection_pending - 1)
-        if self._inspection_pending == 0:
-            self.publish_stop_event(
-                'inspected', ' | '.join(self._inspection_results), self._inspection_index
-            )
-
-    def run_approaching(self, pose: Pose2D, stamp):
-        """Converge onto the target x, y, yaw, then dwell (stop point) or hold (route end)."""
-        x, y, yaw_deg, _ = self._waypoints[self._approach_index]
-        cmd = self._approach.update(pose, x, y, math.radians(yaw_deg))
-        elapsed = (self.get_clock().now() - self._approach_start_time).nanoseconds / 1e9
-        if cmd.done or elapsed > self._approach_timeout:
-            self.publish_stop(hard=True)
-            note = '' if cmd.done else f'approach timeout, error {cmd.position_error:.2f} m'
-            if note:
-                self.get_logger().warn(note)
-            if self._approach_is_goal:
-                self._follower_state = 'GOAL_HOLD'
-                return
-            self._follower_state = 'DWELLING'
-            self._dwell_start_time = self.get_clock().now()
-            self.publish_stop_event('arrived', note)
-            self.start_inspection()
-            self.publish_markers(
-                stamp, 'DWELLING', f'stop {self._next_stop_index}, inspecting', pose.x, pose.y
-            )
-            return
-
-        if self.stopped_for_obstacle(pose, stamp, 'final approach'):
-            return
-
-        target = 'goal' if self._approach_is_goal else f'stop {self._approach_index}'
-        self.publish_approach(cmd)
-        self.publish_markers(
-            stamp,
-            'APPROACHING',
-            f'{target}: {cmd.position_error * 100:.1f} cm, '
-            f'{math.degrees(cmd.yaw_error):.1f} deg',
-            pose.x,
-            pose.y,
-            {'approach': (x, y, math.radians(yaw_deg), self._approach_tolerance, 0.0)},
-        )
-
-    def stopped_for_obstacle(self, pose: Pose2D, stamp, during: str) -> bool:
-        """Stop on an emergency or a stale avoidance signal; True when stopped."""
-        _, _, _, emergency, stale = self.avoidance_signal()
-        if not (emergency or stale):
-            return False
-        self.publish_stop(hard=True)
-        state, reason = (
-            ('AVOIDANCE_STALE', f'no fresh navigation/avoidance during {during}')
-            if stale
-            else ('EMERGENCY_STOP', f'obstacle during {during}')
-        )
-        self.publish_markers(stamp, state, reason, pose.x, pose.y)
-        return True
-
-    def publish_approach(self, cmd):
-        """Publish an approach command; the limiters start from it when following resumes."""
-        out = Twist()
-        out.linear.x = cmd.vx
-        out.linear.y = cmd.vy
-        out.angular.z = cmd.wz
-        self._linear_limiter.reset(cmd.vx)
-        self._lateral_limiter.reset(cmd.vy)
-        self._angular_limiter.reset(cmd.wz)
-        self._cmd_vel_pub.publish(out)
-
-    def start_join(self, index: int):
-        """Walk onto the route at waypoint index before following it."""
-        self._follower_state = 'JOINING'
-        self._join_index = index
-        self._approach.reset()
-        self._join_start_time = self.get_clock().now()
-
-    def run_joining(self, pose: Pose2D, stamp):
-        """Walk onto the route at the located point (slowly, facing along it), then follow."""
-        x, y, yaw_deg, _ = self._waypoints[self._join_index]
-        yaw = math.radians(yaw_deg)
-        cmd = self._approach.update(pose, x, y, yaw)
-        on_route = cmd.position_error < 0.15 and abs(cmd.yaw_error) < math.radians(20.0)
-        elapsed = (self.get_clock().now() - self._join_start_time).nanoseconds / 1e9
-        if cmd.done or on_route or elapsed > float(self.get_parameter('join_timeout_sec').value):
-            if not (cmd.done or on_route):
-                self.get_logger().warn(
-                    f'join timeout, {cmd.position_error:.2f} m from the route: following anyway'
-                )
-            self.publish_stop(hard=True)
-            self._follower_state = 'FOLLOWING'
-            self.get_logger().info(f'on the route at point {self._join_index}')
-            return
-        if self.stopped_for_obstacle(pose, stamp, 'joining the route'):
-            return
-        self.publish_approach(cmd)
-        self.publish_markers(
-            stamp,
-            'JOINING',
-            f'route point {self._join_index}: {cmd.position_error * 100:.0f} cm, '
-            f'{math.degrees(cmd.yaw_error):.0f} deg',
-            pose.x,
-            pose.y,
-            {'approach': (x, y, yaw, self._approach_tolerance, 0.0)},
-        )
-
-    def run_dwelling(self, pose: Pose2D, stamp):
-        """Hold until dwell time passed and inspections finished (or timed out), then move on."""
-        self.publish_stop(hard=True)
-        elapsed = (self.get_clock().now() - self._dwell_start_time).nanoseconds / 1e9
-        waiting = (
-            self._inspection_pending > 0
-            and elapsed < self._dwell_duration + self._inspection_timeout
-        )
-        if elapsed < self._dwell_duration or waiting:
-            remaining = max(0.0, self._dwell_duration - elapsed)
-            note = (
-                f', {self._inspection_pending} inspection(s) pending'
-                if self._inspection_pending
-                else ''
-            )
-            x, y, yaw_deg, _ = self._waypoints[self._next_stop_index]
-            progress = elapsed / self._dwell_duration if self._dwell_duration > 0 else 1.0
-            self.publish_markers(
-                stamp,
-                'DWELLING',
-                f'resuming in {remaining:.1f}s{note}',
-                pose.x,
-                pose.y,
-                {'approach': (x, y, math.radians(yaw_deg), self._approach_tolerance, progress)},
-            )
-            return
-
-        if self._inspection_pending:
-            self.publish_stop_event(
-                'inspected', 'timeout: ' + ' | '.join(self._inspection_results)
-            )
-            self._inspection_pending = 0
-            self._inspection_round += 1  # late answers belong to this stop, drop them
-        self.publish_stop_event('departed')
-        self._follower_state = 'FOLLOWING'
-        self._next_stop_index = self.find_next_stop_index(self._next_stop_index + 1)
-
-    def check_stop_point(self, pose: Pose2D, stamp) -> bool:
-        """Enter APPROACHING near the next stop point, or skip it if progress already passed it."""
-        if self._next_stop_index is None:
-            return False
-        sx, sy, _, sdwell = self._waypoints[self._next_stop_index]
-        if math.hypot(sx - pose.x, sy - pose.y) < self._approach_radius:
-            self._dwell_duration = sdwell
-            self.start_approach(self._next_stop_index, is_goal=False)
-            self.publish_markers(
-                stamp, 'APPROACHING', f'stop point {self._next_stop_index}', pose.x, pose.y
-            )
-            return True
-        if (
-            self._controller.progress_index()
-            > self._next_stop_index + self._stop_point_skip_margin
-        ):
-            self.get_logger().warn(f'stop point {self._next_stop_index} missed, skipping')
-            self.publish_stop_event('skipped', 'passed without reaching tolerance')
-            self._next_stop_index = self.find_next_stop_index(self._next_stop_index + 1)
-        return False
-
-    def set_shift_target(self, offset: float, pose: Pose2D = None):
-        """
-        Accept a new path-shift offset.
-
-        Moving away from an obstacle happens at once; back toward the route only after that was
-        proposed for return_delay_sec, so the robot does not hop back and forth.
-        """
-        target = self._shift_target
-        if offset == target:
-            self._return_since = None
-            return
-        if abs(offset) < abs(target) and offset * target >= 0.0:
-            now = self.get_clock().now()
-            if self._return_since is None:
-                self._return_since = now
-            if (now - self._return_since).nanoseconds / 1e9 < self._return_delay:
-                return
-        self._return_since = None
-        where = f' at ({pose.x:.1f}, {pose.y:.1f})' if pose is not None else ''
-        if offset == 0.0:
-            self.get_logger().info(f'path shift: obstacle passed, back onto the route{where}')
-        elif target == 0.0 or offset * target < 0.0:
-            side = 'left' if offset > 0.0 else 'right'
-            self.get_logger().info(
-                f'path shift: passing an obstacle {abs(offset):.1f} m {side}{where}'
-            )
-        self._shift_target = offset
-
-    def start_approach(self, index: int, is_goal: bool):
-        """Switch to the final approach toward waypoint index."""
-        self._follower_state = 'APPROACHING'
-        self._approach_index = index
-        self._approach_is_goal = is_goal
-        self._approach.reset()
-        self.reset_path_shift()
-        self._approach_start_time = self.get_clock().now()
-
-    def handle_route_end(self):
-        """Start the next lap on a closed loop, else approach the last point and hold."""
-        first_x, first_y = self._waypoints[0][0], self._waypoints[0][1]
-        last_x, last_y = self._waypoints[-1][0], self._waypoints[-1][1]
-        closed = math.hypot(first_x - last_x, first_y - last_y) < self._loop_close_distance
-        if self._loop_route and closed and not self._battery_low:
-            self._lap += 1
-            self.get_logger().info(f'lap {self._lap} complete, restarting route')
-            self.restart_route(from_start=True)
-            return
-        self.start_approach(len(self._waypoints) - 1, is_goal=True)
-
-    def control_loop(self):
-        """Run one control tick; a tick cut short by shutdown (context gone) ends quietly."""
-        try:
-            self.control_tick()
-        except Exception:  # noqa: B902
-            if rclpy.ok():
-                raise
-
-    def control_tick(self):
-        """Run one control tick: safety checks, stop-point state machine, then path following."""
-        with self._pending_lock:
-            changed, self._pending_params = self._pending_params, set()
-        if changed:
-            self.load_tunables(changed)
-        self.publish_route_markers()
-        now = self.get_clock().now()
-        dt = (now - self._last_time).nanoseconds / 1e9
-        self._last_time = now
-        if dt <= 0.0 or dt > 1.0:
-            dt = 1.0 / self._control_rate
-        stamp = now.to_msg()
-
-        if not self._nav_active:
-            self.publish_markers(
-                stamp, 'NAV_INACTIVE', "call 'navigation/start_nav' to begin", 0.0, 0.0
-            )
-            return
-        if self._paused:
-            self.publish_stop(hard=True)
-            self.publish_markers(
-                stamp, 'PAUSED', "call 'navigation/pause' false to resume", 0.0, 0.0
-            )
-            return
-        if not self._waypoints:
-            self.publish_stop(hard=True)
-            self.publish_markers(stamp, 'NO_PATH', 'waiting for navigation/waypoints', 0.0, 0.0)
-            return
-
-        pose = self.current_pose()
-        if pose is None:
-            self.publish_stop(hard=True)
-            self.publish_markers(
-                stamp, 'TF_UNAVAILABLE', 'waiting for fresh map->base_link', 0.0, 0.0
-            )
-            return
-
-        if self._follower_state == 'GOAL_HOLD':
-            self.publish_stop(hard=True)
-            self.publish_markers(
-                stamp, 'GOAL_REACHED', 'holding position at the route end', pose.x, pose.y
-            )
-            return
-        if self._follower_state == 'JOINING':
-            self.run_joining(pose, stamp)
-            return
-        if self._follower_state == 'APPROACHING':
-            self.run_approaching(pose, stamp)
-            return
-        if self._follower_state == 'DWELLING':
-            self.run_dwelling(pose, stamp)
-            return
-
-        steering_bias, lateral_bias, velocity_scale, emergency, stale = self.avoidance_signal()
-        if stale:
-            self.publish_stop(hard=True)
-            self.publish_markers(
-                stamp, 'AVOIDANCE_STALE', 'no fresh navigation/avoidance, stopped', pose.x, pose.y
-            )
-            return
-
-        if self.check_stop_point(pose, stamp):
-            return
-        if self._controller.is_finished(pose):
-            self.handle_route_end()
-            return
-
-        now_sec = now.nanoseconds / 1e9
-        shifting = self.path_shifting()
-        # sliding around an obstacle is not being off the route
-        cross_track = max(0.0, self.cross_track_error(pose) - abs(self._path_offset))
-        slow, off_path = self._safety.off_path(cross_track)
-        if off_path:
-            self.publish_stop(hard=True)
-            self.publish_markers(
-                stamp,
-                'OFF_PATH',
-                f'{cross_track:.2f} m from the route, check the robot '
-                "and call 'navigation/start_nav' again",
-                pose.x,
-                pose.y,
-            )
-            return
-
-        index = min(self._controller.progress_index(), len(self._speed_profile) - 1)
-        target_speed = (
-            self._speed_profile[index] if self._speed_profile else self._target_linear_velocity
-        )
-        if slow:
-            target_speed = min(target_speed, self._min_linear_velocity)
-        if shifting:
-            limit = self.update_path_shift(pose, dt)
-            if limit is None:
-                self.publish_stop(hard=True)
-                self.publish_markers(
-                    stamp, 'AVOIDANCE_STALE', 'no fresh perception/safety_scan, stopped',
-                    pose.x, pose.y,
-                )
-                return
-            velocity_scale = min(1.0, limit / target_speed) if target_speed > 1e-3 else 1.0
-            target_speed = min(target_speed, limit)
-        output = self._controller.update(pose, dt, target_speed)
-        target_linear, target_lateral, target_angular = self.blend(
-            output, steering_bias, lateral_bias, velocity_scale, shifting, dt
-        )
-
-        estop = self._safety.estop(emergency, now_sec)
-        stalled = estop or (target_speed < 0.05 if shifting else velocity_scale < 0.05)
-        blocked = self._safety.blocked(stalled, now_sec)
-        if estop:
-            self.publish_stop(hard=True)
-        else:
-            cmd = Twist()
-            cmd.linear.x = self._linear_limiter.step(
-                clamp(target_linear, self._max_linear_velocity), dt
-            )
-            lateral = self._lateral_filter.step(clamp(target_lateral, self._max_linear_velocity))
-            angular = self._angular_filter.step(clamp(target_angular, self._max_angular_velocity))
-            cmd.linear.y = self._lateral_limiter.step(lateral, dt)
-            cmd.angular.z = self._angular_limiter.step(angular, dt)
-            self._cmd_vel_pub.publish(cmd)
-
-        status = 'BLOCKED' if blocked else 'EMERGENCY_STOP' if estop else 'FOLLOWING'
-        shift_note = f'offset {self._path_offset:+.2f} m | ' if shifting else ''
-        message = (
-            f'lap {self._lap} | {self._controller_name} | {shift_note}v_scale {velocity_scale:.2f}'
-            f' | cte {cross_track:.2f} m | next stop {self._next_stop_index}'
-        )
-        self.publish_markers(stamp, status, message, pose.x, pose.y, self._controller.debug_info())
-
-    def blend(self, output, steering_bias, lateral_bias, velocity_scale, shifting, dt):
-        """
-        Combine the controller output with avoidance into (forward, sideways, yaw rate) targets.
-
-        Path shift already moved the route the holonomic controller follows, so its output is
-        used as is. With reactive avoidance a holonomic robot turns its direction of travel by the
-        steering bias (keeping its heading), any other controller steers by it.
-        """
-        if shifting:
-            return output.linear, output.lateral, output.angular
-        if isinstance(self._controller, HolonomicController):
-            c, s = math.cos(steering_bias), math.sin(steering_bias)
-            vx = (c * output.linear - s * output.lateral) * velocity_scale
-            vy = (s * output.linear + c * output.lateral) * velocity_scale
-            return vx, vy + lateral_bias, output.angular
-        target_angular = output.angular + steering_bias
-        target_linear = output.linear * velocity_scale
-        if self._speed_regulator_enabled:
-            target_linear *= self._speed_regulator.scale_for(target_angular, dt)
-        return target_linear, lateral_bias, target_angular
-
-    def cross_track_error(self, pose: Pose2D) -> float:
-        """Distance from the robot to the route around the current progress index."""
-        i = self._controller.progress_index()
-        window = self._waypoints[max(0, i - 20):i + 20]
-        return min(math.hypot(w[0] - pose.x, w[1] - pose.y) for w in window) if window else 0.0
-
-    def publish_stop(self, hard: bool = False):
-        """Publish zero velocity; hard skips the slew ramp entirely."""
-        if hard:
-            for limiter in (
-                self._linear_limiter,
-                self._lateral_limiter,
-                self._angular_limiter,
-                self._angular_filter,
-                self._lateral_filter,
-            ):
-                limiter.reset()
-            self._cmd_vel_pub.publish(Twist())
-            return
-        dt = 1.0 / self._control_rate
-        cmd = Twist()
-        cmd.linear.x = self._linear_limiter.step(0.0, dt)
-        cmd.linear.y = self._lateral_limiter.step(0.0, dt)
-        cmd.angular.z = self._angular_limiter.step(0.0, dt)
-        self._cmd_vel_pub.publish(cmd)
-
-
-def main(args=None):
-    """Spin the path follower node on a multi-threaded executor."""
-    rclpy.init(args=args)
-    node = PathFollowerNode()
-    executor = MultiThreadedExecutor(num_threads=3)
-    executor.add_node(node)
-    try:
-        executor.spin()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_navigation/robot_navigation/path_loader_node.py b/src/robot_navigation/robot_navigation/path_loader_node.py
deleted file mode 100644
index bdcffc0..0000000
--- a/src/robot_navigation/robot_navigation/path_loader_node.py
+++ /dev/null
@@ -1,183 +0,0 @@
-#!/usr/bin/env python3
-import csv
-import math
-import os
-
-from geometry_msgs.msg import PoseStamped
-from nav_msgs.msg import Path
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
-from robot_interfaces.msg import Waypoint, WaypointPath
-from robot_interfaces.srv import LoadPath
-from robot_navigation.utils.path_processing import parse_route_rows, process_route
-
-
-class PathLoaderNode(Node):
-    """Loads a recorded route CSV, smooths it and publishes it latched."""
-
-    def __init__(self):
-        """Declare params, load and process the route, and publish it."""
-        super().__init__('path_loader_node')
-        self.declare_parameter('waypoints_file', '')
-        self.declare_parameter('route_state_file', '~/.pashupati/last_route')
-        self.declare_parameter('republish_period', 2.0)
-        self.declare_parameter('smoothing_enabled', True)
-        self.declare_parameter('resample_spacing', 0.1)
-        self.declare_parameter('smooth_weight_data', 0.05)
-        self.declare_parameter('smooth_weight_smooth', 0.4)
-        self.declare_parameter('max_smoothing_deviation', 0.3)
-
-        p = self.get_parameter
-        self._smoothing = bool(p('smoothing_enabled').value)
-        self._spacing = float(p('resample_spacing').value)
-        self._weight_data = float(p('smooth_weight_data').value)
-        self._weight_smooth = float(p('smooth_weight_smooth').value)
-        self._max_deviation = float(p('max_smoothing_deviation').value)
-
-        latched = QoSProfile(
-            reliability=QoSReliabilityPolicy.RELIABLE,
-            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
-            history=QoSHistoryPolicy.KEEP_LAST,
-            depth=1,
-        )
-        self._path_pub = self.create_publisher(Path, 'navigation/path', latched)
-        self._raw_path_pub = self.create_publisher(Path, 'navigation/path_raw', latched)
-        self._waypoints_pub = self.create_publisher(WaypointPath, 'navigation/waypoints', latched)
-
-        self._raw: list = []
-        self._route: list = []
-        self._state_file = os.path.expanduser(p('route_state_file').value)
-        if not self.load(p('waypoints_file').value or self.remembered_route()):
-            self.get_logger().info('no route loaded yet, call navigation/load_path')
-
-        period = float(p('republish_period').value)
-        if period > 0.0:
-            self.create_timer(period, self.publish_viz)
-        self.create_service(LoadPath, 'navigation/load_path', self.load_path_callback)
-
-    def read_csv(self, filepath: str) -> list:
-        """Read x,y,yaw_deg[,dwell_sec] rows; bad rows are skipped and logged."""
-        if not filepath or not os.path.isfile(filepath):
-            self.get_logger().warn(f'waypoints_file not found: "{filepath}"')
-            return []
-        try:
-            with open(filepath, 'r', newline='') as f:
-                waypoints, problems = parse_route_rows(csv.reader(f))
-        except (OSError, UnicodeDecodeError, csv.Error) as error:
-            self.get_logger().error(f'cannot read {filepath}: {error}')
-            return []
-        for problem in problems:
-            self.get_logger().warn(f'{filepath}: {problem}')
-        return waypoints
-
-    def load(self, filepath: str) -> bool:
-        """Load, process and publish a route; returns False when nothing was loaded."""
-        raw = self.read_csv(filepath)
-        if not raw:
-            return False
-        route, deviation = (
-            process_route(raw, self._spacing, self._weight_data, self._weight_smooth)
-            if self._smoothing
-            else (raw, 0.0)
-        )
-        if deviation > self._max_deviation:
-            self.get_logger().warn(
-                f'smoothing moved the route up to {deviation:.2f} m '
-                f'(> {self._max_deviation:.2f} m), '
-                'lower smooth_weight_smooth near walls'
-            )
-        self._raw, self._route = raw, route
-        stops = sum(1 for w in route if w[3] > 0.0)
-        self.get_logger().info(
-            f'loaded {filepath}: {len(raw)} -> {len(route)} points, {stops} stop points, '
-            f'max deviation {deviation:.3f} m'
-        )
-        self.publish_viz()
-        self.publish_waypoints()
-        return True
-
-    def load_path_callback(self, request, response):
-        """Service: load another route file without relaunching."""
-        response.success = self.load(request.waypoints_file)
-        if response.success:
-            self.remember_route(request.waypoints_file)
-        response.message = (
-            f'loaded {len(self._route)} points from {request.waypoints_file}'
-            if response.success
-            else f'no waypoints loaded from "{request.waypoints_file}"'
-        )
-        return response
-
-    def remembered_route(self) -> str:
-        """Return the last route loaded through the service (survives restarts), or ''."""
-        try:
-            with open(self._state_file) as f:
-                return f.read().strip()
-        except OSError:
-            return ''
-
-    def remember_route(self, filepath: str):
-        """Store the route path so a restarted robot comes back with the same route."""
-        try:
-            os.makedirs(os.path.dirname(self._state_file), exist_ok=True)
-            with open(self._state_file, 'w') as f:
-                f.write(os.path.abspath(filepath))
-        except OSError as error:
-            self.get_logger().warn(f'cannot remember route: {error}')
-
-    def to_path(self, waypoints: list) -> Path:
-        """Build a nav_msgs/Path in the map frame."""
-        msg = Path()
-        msg.header.frame_id = 'map'
-        msg.header.stamp = self.get_clock().now().to_msg()
-        for x, y, yaw_deg, _ in waypoints:
-            pose = PoseStamped()
-            pose.header = msg.header
-            pose.pose.position.x = x
-            pose.pose.position.y = y
-            pose.pose.orientation.z = math.sin(math.radians(yaw_deg) / 2.0)
-            pose.pose.orientation.w = math.cos(math.radians(yaw_deg) / 2.0)
-            msg.poses.append(pose)
-        return msg
-
-    def publish_viz(self):
-        """Publish the processed and raw routes for RViz."""
-        self._path_pub.publish(self.to_path(self._route))
-        self._raw_path_pub.publish(self.to_path(self._raw))
-
-    def publish_waypoints(self):
-        """Publish the latched WaypointPath consumed by path_follower_node."""
-        msg = WaypointPath()
-        msg.header.frame_id = 'map'
-        msg.header.stamp = self.get_clock().now().to_msg()
-        for x, y, yaw_deg, dwell_sec in self._route:
-            msg.waypoints.append(
-                Waypoint(x=float(x), y=float(y), yaw=float(yaw_deg), dwell_sec=float(dwell_sec))
-            )
-        self._waypoints_pub.publish(msg)
-
-
-def main(args=None):
-    """Spin the path loader node."""
-    rclpy.init(args=args)
-    node = PathLoaderNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_navigation/robot_navigation/utils/__init__.py b/src/robot_navigation/robot_navigation/utils/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/__init__.py b/src/robot_navigation/robot_navigation/utils/avoidance/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/base_avoidance.py b/src/robot_navigation/robot_navigation/utils/avoidance/base_avoidance.py
deleted file mode 100644
index 69b2da7..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/base_avoidance.py
+++ /dev/null
@@ -1,44 +0,0 @@
-from dataclasses import dataclass
-import math
-
-
-@dataclass
-class AvoidanceResult:
-    """One avoidance update: steering bias, speed scale, emergency flag and sideways push."""
-
-    steering_bias: float = 0.0
-    velocity_scale: float = 1.0
-    emergency: bool = False
-    lateral_bias: float = 0.0
-
-
-def clean_ranges(ranges: list[float], range_max: float) -> list[float]:
-    """Replace NaN/inf/None sector ranges with range_max."""
-    return [range_max if r is None or math.isnan(r) or math.isinf(r) else r for r in ranges]
-
-
-def sector_angle(index: int, angle_min: float, angle_increment: float) -> float:
-    """Centre angle of a sector."""
-    return angle_min + (index + 0.5) * angle_increment
-
-
-def smooth_result(
-    previous: AvoidanceResult, raw: AvoidanceResult, alpha: float
-) -> AvoidanceResult:
-    """Exponential smoothing of the continuous outputs; the emergency flag is never delayed."""
-    return AvoidanceResult(
-        steering_bias=alpha * previous.steering_bias + (1 - alpha) * raw.steering_bias,
-        velocity_scale=alpha * previous.velocity_scale + (1 - alpha) * raw.velocity_scale,
-        emergency=raw.emergency,
-        lateral_bias=alpha * previous.lateral_bias + (1 - alpha) * raw.lateral_bias,
-    )
-
-
-class AvoidanceAlgorithm:
-    """Common interface every reactive obstacle-avoidance algorithm implements."""
-
-    def update(
-        self, ranges: list[float], angle_min: float, angle_increment: float, range_max: float
-    ) -> AvoidanceResult:
-        """Compute a new avoidance result from one sector scan."""
-        raise NotImplementedError
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/braitenberg_avoidance.py b/src/robot_navigation/robot_navigation/utils/avoidance/braitenberg_avoidance.py
deleted file mode 100644
index c68d4d6..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/braitenberg_avoidance.py
+++ /dev/null
@@ -1,114 +0,0 @@
-import math
-
-from robot_navigation.utils.avoidance.base_avoidance import AvoidanceAlgorithm, AvoidanceResult
-
-
-def angle_diff(a: float, b: float) -> float:
-    """Return the signed difference a-b wrapped into (-pi, pi]."""
-    d = a - b
-    while d > math.pi:
-        d -= 2 * math.pi
-    while d < -math.pi:
-        d += 2 * math.pi
-    return d
-
-
-def tent_weight(theta: float, center: float, half_width: float) -> float:
-    """Return a 1-at-center, 0-at-edge triangular weight for theta within a zone."""
-    if half_width <= 0.0:
-        return 0.0
-    d = abs(angle_diff(theta, center))
-    return max(0.0, 1.0 - d / half_width)
-
-
-class BraitenbergAvoidance(AvoidanceAlgorithm):
-    """Discrete-sector Braitenberg-style reactive avoidance: obstacles bias steering and speed."""
-
-    def __init__(
-        self,
-        safe_distance: float = 3.0,
-        emergency_distance: float = 0.4,
-        steering_gain: float = 1.5,
-        velocity_gain: float = 1.0,
-        steering_zone_center_deg: float = 45.0,
-        steering_zone_width_deg: float = 60.0,
-        velocity_zone_center_deg: float = 60.0,
-        velocity_zone_width_deg: float = 90.0,
-        following_zone_width_deg: float = 30.0,
-        emergency_cone_deg: float = 60.0,
-        smoothing: float = 0.85,
-    ):
-        self._safe_distance = safe_distance
-        self._emergency_distance = emergency_distance
-        self._steering_gain = steering_gain
-        self._velocity_gain = velocity_gain
-        self._steering_zone_center = math.radians(steering_zone_center_deg)
-        self._steering_zone_half = math.radians(steering_zone_width_deg) / 2.0
-        self._velocity_zone_center = math.radians(velocity_zone_center_deg)
-        self._velocity_zone_half = math.radians(velocity_zone_width_deg) / 2.0
-        self._following_half_width = math.radians(following_zone_width_deg) / 2.0
-        self._emergency_half_cone = math.radians(emergency_cone_deg) / 2.0
-        self._smoothing = smoothing
-        self._filtered = AvoidanceResult()
-
-    def update(
-        self, ranges: list[float], angle_min: float, angle_increment: float, range_max: float
-    ) -> AvoidanceResult:
-        """Compute a new avoidance result from one sector scan and blend it with the last one."""
-        if len(ranges) == 0:
-            return self._filtered
-
-        num_l = den_l = num_r = den_r = 0.0
-        num_vl = den_vl = num_vr = den_vr = 0.0
-        num_f = den_f = 0.0
-        emergency = False
-
-        for i, r in enumerate(ranges):
-            if r is None or math.isnan(r) or math.isinf(r):
-                r = range_max
-            theta = angle_min + (i + 0.5) * angle_increment
-            danger = max(0.0, 1.0 - min(r, self._safe_distance) / self._safe_distance)
-
-            if (
-                r < self._emergency_distance
-                and abs(angle_diff(theta, 0.0)) < self._emergency_half_cone
-            ):
-                emergency = True
-
-            w_l = tent_weight(theta, self._steering_zone_center, self._steering_zone_half)
-            w_r = tent_weight(theta, -self._steering_zone_center, self._steering_zone_half)
-            w_vl = tent_weight(theta, self._velocity_zone_center, self._velocity_zone_half)
-            w_vr = tent_weight(theta, -self._velocity_zone_center, self._velocity_zone_half)
-            w_f = tent_weight(theta, 0.0, self._following_half_width)
-
-            num_l += w_l * danger
-            den_l += w_l
-            num_r += w_r * danger
-            den_r += w_r
-            num_vl += w_vl * danger
-            den_vl += w_vl
-            num_vr += w_vr * danger
-            den_vr += w_vr
-            num_f += w_f * danger
-            den_f += w_f
-
-        danger_left = num_l / den_l if den_l > 1e-6 else 0.0
-        danger_right = num_r / den_r if den_r > 1e-6 else 0.0
-        danger_vel_left = num_vl / den_vl if den_vl > 1e-6 else 0.0
-        danger_vel_right = num_vr / den_vr if den_vr > 1e-6 else 0.0
-        danger_following = num_f / den_f if den_f > 1e-6 else 0.0
-
-        steering_bias = self._steering_gain * (danger_right - danger_left)
-        lateral_danger = max(danger_vel_left, danger_vel_right)
-        velocity_scale = max(
-            0.0, (1.0 - self._velocity_gain * lateral_danger) * (1.0 - danger_following)
-        )
-
-        raw = AvoidanceResult(steering_bias, velocity_scale, emergency)
-        a = self._smoothing
-        self._filtered = AvoidanceResult(
-            steering_bias=a * self._filtered.steering_bias + (1 - a) * raw.steering_bias,
-            velocity_scale=a * self._filtered.velocity_scale + (1 - a) * raw.velocity_scale,
-            emergency=raw.emergency,
-        )
-        return self._filtered
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/follow_gap_avoidance.py b/src/robot_navigation/robot_navigation/utils/avoidance/follow_gap_avoidance.py
deleted file mode 100644
index 07348bc..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/follow_gap_avoidance.py
+++ /dev/null
@@ -1,91 +0,0 @@
-import math
-
-from robot_navigation.utils.avoidance.base_avoidance import (
-    AvoidanceAlgorithm,
-    AvoidanceResult,
-    clean_ranges,
-    sector_angle,
-    smooth_result,
-)
-
-
-class FollowGapAvoidance(AvoidanceAlgorithm):
-    """
-    Follow-the-gap avoidance.
-
-    Inflates the closest obstacle by a safety bubble and steers to the centre of the widest
-    remaining gap, only while the frontal cone is blocked.
-    """
-
-    def __init__(
-        self,
-        safe_distance: float = 1.5,
-        emergency_distance: float = 0.75,
-        emergency_cone_deg: float = 60.0,
-        bubble_radius: float = 0.45,
-        steering_gain: float = 1.0,
-        velocity_gain: float = 1.0,
-        smoothing: float = 0.85,
-    ):
-        self._safe_distance = safe_distance
-        self._emergency_distance = emergency_distance
-        self._emergency_half_cone = math.radians(emergency_cone_deg) / 2.0
-        self._bubble_radius = bubble_radius
-        self._steering_gain = steering_gain
-        self._velocity_gain = velocity_gain
-        self._smoothing = smoothing
-        self._filtered = AvoidanceResult()
-
-    def update(self, ranges, angle_min, angle_increment, range_max) -> AvoidanceResult:
-        """Bubble the nearest obstacle, find the widest free gap and steer to its centre."""
-        if not ranges:
-            return self._filtered
-        r = clean_ranges(ranges, range_max)
-        n = len(r)
-        cone = [
-            i
-            for i in range(n)
-            if abs(sector_angle(i, angle_min, angle_increment)) < self._emergency_half_cone
-        ]
-        emergency = any(r[i] < self._emergency_distance for i in cone)
-        frontal_min = min((r[i] for i in cone), default=range_max)
-        velocity_scale = max(
-            0.0, 1.0 - self._velocity_gain * max(0.0, 1.0 - frontal_min / self._safe_distance)
-        )
-
-        steering = 0.0
-        if frontal_min < self._safe_distance:
-            nearest = min(range(n), key=lambda i: r[i])
-            half = math.atan2(self._bubble_radius, max(r[nearest], 1e-3))
-            free = [
-                r[i] >= self._safe_distance
-                and abs(
-                    sector_angle(i, angle_min, angle_increment)
-                    - sector_angle(nearest, angle_min, angle_increment)
-                )
-                > half
-                for i in range(n)
-            ]
-            gap = self.widest_gap(free)
-            if gap is None:
-                velocity_scale = 0.0
-            else:
-                centre = sector_angle((gap[0] + gap[1]) / 2.0, angle_min, angle_increment)
-                steering = self._steering_gain * centre
-
-        raw = AvoidanceResult(steering, velocity_scale, emergency, 0.0)
-        self._filtered = smooth_result(self._filtered, raw, self._smoothing)
-        return self._filtered
-
-    @staticmethod
-    def widest_gap(free: list[bool]):
-        """(start, end) of the longest run of free sectors, or None."""
-        best, start = None, None
-        for i, is_free in enumerate(free + [False]):
-            if is_free and start is None:
-                start = i
-            elif not is_free and start is not None:
-                if best is None or (i - 1 - start) > (best[1] - best[0]):
-                    best = (start, i - 1)
-                start = None
-        return best
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/potential_field_avoidance.py b/src/robot_navigation/robot_navigation/utils/avoidance/potential_field_avoidance.py
deleted file mode 100644
index 6e884e1..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/potential_field_avoidance.py
+++ /dev/null
@@ -1,67 +0,0 @@
-import math
-
-from robot_navigation.utils.avoidance.base_avoidance import (
-    AvoidanceAlgorithm,
-    AvoidanceResult,
-    clean_ranges,
-    sector_angle,
-    smooth_result,
-)
-
-
-class PotentialFieldAvoidance(AvoidanceAlgorithm):
-    """
-    Artificial potential field: every obstacle inside safe_distance pushes the robot away.
-
-    The repulsion yields a steering bias, a sideways push for holonomic robots and a speed scale.
-    """
-
-    def __init__(
-        self,
-        safe_distance: float = 1.5,
-        emergency_distance: float = 0.75,
-        emergency_cone_deg: float = 60.0,
-        repulsion_gain: float = 0.5,
-        steering_gain: float = 0.8,
-        lateral_gain: float = 0.3,
-        max_lateral: float = 0.2,
-        velocity_gain: float = 1.0,
-        smoothing: float = 0.8,
-    ):
-        self._safe_distance = safe_distance
-        self._emergency_distance = emergency_distance
-        self._emergency_half_cone = math.radians(emergency_cone_deg) / 2.0
-        self._repulsion_gain = repulsion_gain
-        self._steering_gain = steering_gain
-        self._lateral_gain = lateral_gain
-        self._max_lateral = max_lateral
-        self._velocity_gain = velocity_gain
-        self._smoothing = smoothing
-        self._filtered = AvoidanceResult()
-
-    def update(self, ranges, angle_min, angle_increment, range_max) -> AvoidanceResult:
-        """Sum the repulsive forces of all sectors and map them to the avoidance outputs."""
-        if not ranges:
-            return self._filtered
-        fx = fy = 0.0
-        emergency = False
-        frontal_danger = 0.0
-        for i, r in enumerate(clean_ranges(ranges, range_max)):
-            theta = sector_angle(i, angle_min, angle_increment)
-            in_cone = abs(theta) < self._emergency_half_cone
-            if in_cone and r < self._emergency_distance:
-                emergency = True
-            if r >= self._safe_distance or r <= 1e-3:
-                continue
-            magnitude = self._repulsion_gain * (1.0 / r - 1.0 / self._safe_distance) / (r * r)
-            fx -= magnitude * math.cos(theta)
-            fy -= magnitude * math.sin(theta)
-            if in_cone:
-                frontal_danger = max(frontal_danger, 1.0 - r / self._safe_distance)
-
-        steering = self._steering_gain * math.atan(fy)
-        lateral = max(-self._max_lateral, min(self._max_lateral, self._lateral_gain * fy))
-        velocity_scale = max(0.0, 1.0 - self._velocity_gain * frontal_danger)
-        raw = AvoidanceResult(steering, velocity_scale, emergency, lateral)
-        self._filtered = smooth_result(self._filtered, raw, self._smoothing)
-        return self._filtered
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/registry.py b/src/robot_navigation/robot_navigation/utils/avoidance/registry.py
deleted file mode 100644
index 7f2f1e7..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/registry.py
+++ /dev/null
@@ -1,20 +0,0 @@
-from robot_navigation.utils.avoidance.braitenberg_avoidance import BraitenbergAvoidance
-from robot_navigation.utils.avoidance.follow_gap_avoidance import FollowGapAvoidance
-from robot_navigation.utils.avoidance.potential_field_avoidance import PotentialFieldAvoidance
-from robot_navigation.utils.avoidance.vfh_avoidance import VfhAvoidance
-
-AVOIDANCE_ALGORITHMS = {
-    'braitenberg': BraitenbergAvoidance,
-    'vfh': VfhAvoidance,
-    'potential_field': PotentialFieldAvoidance,
-    'follow_gap': FollowGapAvoidance,
-}
-
-
-def create_avoidance(name: str, params: dict):
-    """Build an avoidance algorithm instance by name using the given constructor kwargs."""
-    if name not in AVOIDANCE_ALGORITHMS:
-        raise ValueError(
-            f'unknown avoidance algorithm: {name!r}, options are {list(AVOIDANCE_ALGORITHMS)}'
-        )
-    return AVOIDANCE_ALGORITHMS[name](**params)
diff --git a/src/robot_navigation/robot_navigation/utils/avoidance/vfh_avoidance.py b/src/robot_navigation/robot_navigation/utils/avoidance/vfh_avoidance.py
deleted file mode 100644
index 1d1fc36..0000000
--- a/src/robot_navigation/robot_navigation/utils/avoidance/vfh_avoidance.py
+++ /dev/null
@@ -1,114 +0,0 @@
-import math
-
-from robot_navigation.utils.avoidance.base_avoidance import AvoidanceAlgorithm, AvoidanceResult
-
-
-class VfhAvoidance(AvoidanceAlgorithm):
-    """
-    Simplified Vector Field Histogram.
-
-    Only when the frontal cone is blocked, steers toward the passable gap edge closest to
-    straight ahead; with no passable gap it slows to a stop.
-    """
-
-    def __init__(
-        self,
-        safe_distance: float = 3.0,
-        emergency_distance: float = 0.4,
-        emergency_cone_deg: float = 60.0,
-        min_gap_width_deg: float = 20.0,
-        steering_gain: float = 1.0,
-        velocity_gain: float = 1.0,
-        smoothing: float = 0.85,
-    ):
-        self._safe_distance = safe_distance
-        self._emergency_distance = emergency_distance
-        self._emergency_half_cone = math.radians(emergency_cone_deg) / 2.0
-        self._min_gap_width = math.radians(min_gap_width_deg)
-        self._steering_gain = steering_gain
-        self._velocity_gain = velocity_gain
-        self._smoothing = smoothing
-        self._filtered = AvoidanceResult()
-
-    def sector_angle(self, index: int, angle_min: float, angle_increment: float) -> float:
-        """Return the center angle of a given sector index."""
-        return angle_min + (index + 0.5) * angle_increment
-
-    def find_gaps(self, blocked: list[bool]) -> list[tuple[int, int]]:
-        """Return (start, end) index ranges of contiguous free (not blocked) sectors."""
-        gaps = []
-        start = None
-        for i, is_blocked in enumerate(blocked):
-            if not is_blocked and start is None:
-                start = i
-            elif is_blocked and start is not None:
-                gaps.append((start, i - 1))
-                start = None
-        if start is not None:
-            gaps.append((start, len(blocked) - 1))
-        return gaps
-
-    def gap_heading(self, gap, angle_min, angle_increment, min_gap_sectors, signed=False) -> float:
-        """Return the heading of the gap edge closest to straight ahead, inset by half a gap."""
-        start, end = gap
-        lo = self.sector_angle(start + min_gap_sectors // 2, angle_min, angle_increment)
-        hi = self.sector_angle(end - min_gap_sectors // 2, angle_min, angle_increment)
-        heading = 0.0 if lo <= 0.0 <= hi else (lo if abs(lo) < abs(hi) else hi)
-        return heading if signed else abs(heading)
-
-    def update(
-        self, ranges: list[float], angle_min: float, angle_increment: float, range_max: float
-    ) -> AvoidanceResult:
-        """Build the obstacle histogram, pick the best gap, and steer toward its center."""
-        n = len(ranges)
-        if n == 0:
-            return self._filtered
-
-        blocked = []
-        emergency = False
-        frontal_danger_max = 0.0
-
-        for i, r in enumerate(ranges):
-            if r is None or math.isnan(r) or math.isinf(r):
-                r = range_max
-            theta = self.sector_angle(i, angle_min, angle_increment)
-            blocked.append(r < self._safe_distance)
-
-            if r < self._emergency_distance and abs(theta) < self._emergency_half_cone:
-                emergency = True
-            if abs(theta) < self._emergency_half_cone:
-                danger = max(0.0, 1.0 - min(r, self._safe_distance) / self._safe_distance)
-                frontal_danger_max = max(frontal_danger_max, danger)
-
-        gaps = self.find_gaps(blocked)
-        min_gap_sectors = max(1, int(self._min_gap_width / angle_increment))
-        gaps = [g for g in gaps if (g[1] - g[0] + 1) >= min_gap_sectors]
-
-        front_blocked = any(
-            blocked[i]
-            for i in range(n)
-            if abs(self.sector_angle(i, angle_min, angle_increment)) < self._emergency_half_cone
-        )
-
-        steering_bias = 0.0
-        velocity_scale = max(0.0, 1.0 - self._velocity_gain * frontal_danger_max)
-        if front_blocked:
-            if gaps:
-                best_gap = min(
-                    gaps,
-                    key=lambda g: self.gap_heading(g, angle_min, angle_increment, min_gap_sectors),
-                )
-                steering_bias = self._steering_gain * self.gap_heading(
-                    best_gap, angle_min, angle_increment, min_gap_sectors, signed=True
-                )
-            else:
-                velocity_scale = 0.0
-        raw = AvoidanceResult(steering_bias, velocity_scale, emergency)
-
-        a = self._smoothing
-        self._filtered = AvoidanceResult(
-            steering_bias=a * self._filtered.steering_bias + (1 - a) * raw.steering_bias,
-            velocity_scale=a * self._filtered.velocity_scale + (1 - a) * raw.velocity_scale,
-            emergency=raw.emergency,
-        )
-        return self._filtered
diff --git a/src/robot_navigation/robot_navigation/utils/command_filter.py b/src/robot_navigation/robot_navigation/utils/command_filter.py
deleted file mode 100644
index c79571b..0000000
--- a/src/robot_navigation/robot_navigation/utils/command_filter.py
+++ /dev/null
@@ -1,43 +0,0 @@
-class SlewLimiter:
-    """Caps how fast a value can change per second."""
-
-    def __init__(self, max_rate: float):
-        self._max_rate = max_rate
-        self._value = 0.0
-
-    @property
-    def value(self) -> float:
-        """Return the current output value."""
-        return self._value
-
-    def step(self, target: float, dt: float) -> float:
-        """Move toward target by at most max_rate * dt and return the new value."""
-        max_delta = self._max_rate * dt
-        self._value += max(-max_delta, min(max_delta, target - self._value))
-        return self._value
-
-    def reset(self, value: float = 0.0):
-        """Jump straight to value, bypassing the rate limit."""
-        self._value = value
-
-
-class LowPassFilter:
-    """First-order low-pass: alpha = 1 passes the input through, smaller values smooth more."""
-
-    def __init__(self, alpha: float):
-        self._alpha = max(0.0, min(1.0, alpha))
-        self._value = 0.0
-
-    @property
-    def value(self) -> float:
-        """Return the current filtered value."""
-        return self._value
-
-    def step(self, target: float) -> float:
-        """Blend target into the filtered value and return it."""
-        self._value += self._alpha * (target - self._value)
-        return self._value
-
-    def reset(self, value: float = 0.0):
-        """Set the filter state directly."""
-        self._value = value
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/__init__.py b/src/robot_navigation/robot_navigation/utils/controllers/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/base_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/base_controller.py
deleted file mode 100644
index 96978db..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/base_controller.py
+++ /dev/null
@@ -1,36 +0,0 @@
-from dataclasses import dataclass
-
-from robot_navigation.utils.pose2d import Pose2D
-
-
-@dataclass
-class ControllerOutput:
-    """Nominal (pre-avoidance) body velocity command from a path controller."""
-
-    linear: float = 0.0   # m/s forward
-    angular: float = 0.0  # rad/s, +left
-    lateral: float = 0.0  # m/s, +left; only holonomic controllers set it
-
-
-class PathController:
-    """Common interface every path-following controller implements."""
-
-    def set_path(self, path_xy: list[tuple[float, float]], start_index=None):
-        """Replace the tracked path; start_index pins progress instead of a global search."""
-        raise NotImplementedError
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Compute the nominal command for the current pose at the requested forward speed."""
-        raise NotImplementedError
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end of the path."""
-        raise NotImplementedError
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        raise NotImplementedError
-
-    def debug_info(self) -> dict:
-        """Return controller-specific debug values for visualization (all keys optional)."""
-        return {}
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/holonomic_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/holonomic_controller.py
deleted file mode 100644
index d61140e..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/holonomic_controller.py
+++ /dev/null
@@ -1,136 +0,0 @@
-import math
-
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.geometry import clamp, to_local, wrap_angle
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class HolonomicController(PathController):
-    """
-    Path following with x, y and yaw for legged and omnidirectional robots.
-
-    Translation and heading are controlled separately:
-
-    - The robot moves toward a carrot `lookahead_distance` further along the path (measured from
-      its projection on the path), in any direction of its body. A cross-track error therefore
-      closes sideways along a smooth first-order curve without changing the heading. When the
-      path is behind the robot it backs up, and far from the path it heads straight back to it.
-    - The heading turns toward the path direction a short `heading_lookahead` ahead, with a
-      proportional, rate-limited yaw command. It does not depend on the cross-track error, so
-      it cannot oscillate around the path the way steering-only trackers do.
-
-    Sideways and backward speeds are capped (`max_lateral_velocity`, `max_reverse_velocity`)
-    while keeping the direction of travel, and translation slows while the heading is far off,
-    so a legged robot mostly walks forward and only creeps sideways or backwards.
-
-    `set_offset()` makes the robot follow a parallel line `offset` metres to the left (+) or
-    right (-) of the path, used to slide around obstacles without leaving the route heading.
-
-    With `max_lateral_velocity` 0 (a robot that cannot strafe) it degrades to turn-toward-the-
-    carrot and drive forward.
-    """
-
-    def __init__(
-        self,
-        lookahead_distance: float = 0.6,
-        heading_lookahead: float = 0.6,
-        goal_tolerance: float = 0.3,
-        kp_yaw: float = 1.5,
-        max_angular_velocity: float = 1.0,
-        max_lateral_velocity: float = 0.25,
-        max_reverse_velocity: float = 0.15,
-        min_alignment_scale: float = 0.25,
-    ):
-        self._lookahead = max(0.1, lookahead_distance)
-        self._heading_lookahead = max(0.1, heading_lookahead)
-        self._goal_tolerance = goal_tolerance
-        self._kp_yaw = kp_yaw
-        self._max_wz = max_angular_velocity
-        self._max_vy = max(0.0, max_lateral_velocity)
-        self._max_reverse = max(0.0, max_reverse_velocity)
-        self._min_alignment = max(0.0, min(1.0, min_alignment_scale))
-        self._progress = PathProgress()
-        self._offset = 0.0
-        self._debug = {}
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path, optionally pinning progress to start_index."""
-        self._progress.set_path(path_xy, start_index)
-        self._debug = {}
-
-    def set_offset(self, offset: float):
-        """Follow a line parallel to the path, offset metres to the left (+) or right (-)."""
-        self._offset = offset
-
-    def offset(self) -> float:
-        """Return the current lateral offset from the path."""
-        return self._offset
-
-    def path_progress(self) -> PathProgress:
-        """Return the path tracker (route, arc lengths, projection) for path-shift avoidance."""
-        return self._progress
-
-    def shifted_point(self, s: float) -> tuple[float, float]:
-        """Point at arc length s, moved sideways by the offset (left normal of the path)."""
-        x, y = self._progress.point_at(s)
-        if self._offset == 0.0:
-            return x, y
-        heading = self._progress.heading_between(s - 0.1, s + 0.1)
-        return x - math.sin(heading) * self._offset, y + math.cos(heading) * self._offset
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Command (vx, vy, wz) toward the carrot, with the heading along the path."""
-        progress = self._progress
-        if len(progress.path) < 2:
-            return ControllerOutput()
-        progress.update(pose)
-        s = progress.projection(pose)
-        cx, cy = self.shifted_point(s + self._lookahead)
-        dx, dy = cx - pose.x, cy - pose.y
-        distance = math.hypot(dx, dy)
-
-        path_heading = progress.heading_between(s, s + self._heading_lookahead)
-        if self._max_vy <= 0.0 and distance > 1e-3:
-            path_heading = math.atan2(dy, dx)  # cannot strafe: face the carrot instead
-        heading_error = wrap_angle(path_heading - pose.yaw)
-        wz = clamp(self._kp_yaw * heading_error, self._max_wz)
-
-        vx = vy = 0.0
-        if distance > 1e-3 and target_speed > 0.0:
-            # never faster than needed to reach the carrot within about a second
-            speed = min(target_speed, distance)
-            vx, vy = to_local(speed * dx / distance, speed * dy / distance, pose.yaw)
-            vx, vy = self.limit(vx, vy, heading_error)
-
-        self._debug = {
-            'lookahead_xy': (cx, cy),
-            'nearest_xy': self.shifted_point(s),
-            'lookahead_distance': self._lookahead,
-            'heading_error': heading_error,
-        }
-        return ControllerOutput(linear=vx, angular=wz, lateral=vy)
-
-    def limit(self, vx: float, vy: float, heading_error: float) -> tuple[float, float]:
-        """Slow down while misaligned, then cap reverse and sideways speed (keeping direction)."""
-        alignment = math.cos(heading_error) if abs(heading_error) < math.pi / 2 else 0.0
-        scale = max(self._min_alignment, alignment)
-        if self._max_vy <= 0.0:  # cannot strafe: forward only, while turning toward the carrot
-            return max(0.0, vx) * scale, 0.0
-        if vx < 0.0:
-            scale = min(scale, self._max_reverse / -vx) if self._max_reverse > 0.0 else 0.0
-        if abs(vy) > 1e-9:
-            scale = min(scale, self._max_vy / abs(vy))
-        return vx * scale, vy * scale
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end of the path and the robot is near it."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def debug_info(self) -> dict:
-        """Carrot, nearest point and lookahead for visualization."""
-        return dict(self._debug)
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/lqr_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/lqr_controller.py
deleted file mode 100644
index 6a39781..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/lqr_controller.py
+++ /dev/null
@@ -1,73 +0,0 @@
-import numpy as np
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class LqrController(PathController):
-    """Discrete LQR on the path error [lateral, heading] with curvature feedforward."""
-
-    def __init__(
-        self,
-        goal_tolerance=0.3,
-        q_lateral=1.0,
-        q_heading=0.5,
-        r_angular=0.5,
-        dt=0.05,
-        max_angular_velocity=1.0,
-    ):
-        self._goal_tolerance = goal_tolerance
-        self._Q = np.diag([q_lateral, q_heading])
-        self._R = np.array([[r_angular]])
-        self._dt = dt
-        self._max_angular_velocity = max_angular_velocity
-        self._progress = PathProgress()
-        self._gains: dict[float, np.ndarray] = {}
-        self._last_reference = None
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path."""
-        self._progress.set_path(path_xy, start_index)
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end and the robot is within goal_tolerance."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def gain(self, speed: float) -> np.ndarray:
-        """LQR gain for a forward speed, cached on a 5 cm/s grid."""
-        key = round(max(speed, 0.05) / 0.05) * 0.05
-        if key not in self._gains:
-            A = np.array([[1.0, key * self._dt], [0.0, 1.0]])
-            B = np.array([[0.0], [self._dt]])
-            P = self._Q.copy()
-            for _ in range(500):
-                BtP = B.T @ P
-                K = np.linalg.solve(self._R + BtP @ B, BtP @ A)
-                P_next = self._Q + A.T @ P @ (A - B @ K)
-                if np.max(np.abs(P_next - P)) < 1e-9:
-                    break
-                P = P_next
-            self._gains[key] = K
-        return self._gains[key]
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Feedback on lateral/heading error plus v * curvature feedforward."""
-        if not self._progress.path:
-            return ControllerOutput(0.0, 0.0)
-        self._progress.update(pose)
-        ref = self._progress.reference(pose)
-        self._last_reference = ref
-        x = np.array([ref.lateral_error, ref.heading_error])
-        feedback = float(-(self.gain(target_speed) @ x)[0])
-        angular = target_speed * ref.curvature + feedback
-        angular = max(-self._max_angular_velocity, min(self._max_angular_velocity, angular))
-        return ControllerOutput(linear=target_speed, angular=angular)
-
-    def debug_info(self) -> dict:
-        """Expose the nearest path point for visualization."""
-        path = self._progress.path
-        return {'nearest_xy': path[self._progress.index]} if path else {}
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/mppi_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/mppi_controller.py
deleted file mode 100644
index e74a2f0..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/mppi_controller.py
+++ /dev/null
@@ -1,106 +0,0 @@
-import numpy as np
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class MppiController(PathController):
-    """
-    Simplified Model Predictive Path Integral controller.
-
-    Samples angular-velocity sequences at the requested speed, rolls out a unicycle model and
-    returns the cost-weighted first control (local path window + lookahead terminal cost).
-    """
-
-    def __init__(
-        self,
-        goal_tolerance=0.3,
-        horizon_steps=15,
-        dt=0.1,
-        num_samples=200,
-        angular_std=1.0,
-        temperature=1.0,
-        max_angular_velocity=1.0,
-        window_points=60,
-    ):
-        self._goal_tolerance = goal_tolerance
-        self._horizon_steps = horizon_steps
-        self._rollout_dt = dt
-        self._num_samples = num_samples
-        self._angular_std = angular_std
-        self._temperature = temperature
-        self._max_angular_velocity = max_angular_velocity
-        self._progress = PathProgress()
-        self._window_points = window_points
-        self._path = np.zeros((0, 2))
-        self._last_angular = 0.0
-        self._last_rollout = None
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path, optionally pinning progress to start_index."""
-        self._progress.set_path(path_xy, start_index)
-        self._path = np.zeros((0, 2))
-        self._last_angular = 0.0
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end and the robot is within goal_tolerance."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def path_cost(self, xs: np.ndarray, ys: np.ndarray, target) -> np.ndarray:
-        """Return the squared path-window distance plus the terminal lookahead distance."""
-        dx = xs[:, :, None] - self._path[None, None, :, 0]
-        dy = ys[:, :, None] - self._path[None, None, :, 1]
-        tracking = (dx * dx + dy * dy).min(axis=2).mean(axis=1)
-        terminal = (xs[:, -1] - target[0]) ** 2 + (ys[:, -1] - target[1]) ** 2
-        return tracking + terminal
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Sample control sequences, roll them out, and return a cost-weighted-average command."""
-        if not self._progress.path:
-            return ControllerOutput(0.0, 0.0)
-        self._progress.update(pose)
-        self._path = np.array(self._progress.window(self._window_points))
-        speed = max(target_speed, 0.05)
-        horizon_length = speed * self._rollout_dt * self._horizon_steps
-        target = self._progress.lookahead_point(pose, horizon_length)
-
-        angular_samples = np.random.normal(
-            self._last_angular, self._angular_std, size=(self._num_samples, self._horizon_steps)
-        )
-        angular_samples = np.clip(
-            angular_samples, -self._max_angular_velocity, self._max_angular_velocity
-        )
-
-        x = np.full(self._num_samples, pose.x)
-        y = np.full(self._num_samples, pose.y)
-        yaw = np.full(self._num_samples, pose.yaw)
-        xs = np.zeros((self._num_samples, self._horizon_steps))
-        ys = np.zeros((self._num_samples, self._horizon_steps))
-
-        for step in range(self._horizon_steps):
-            yaw = yaw + angular_samples[:, step] * self._rollout_dt
-            x = x + speed * np.cos(yaw) * self._rollout_dt
-            y = y + speed * np.sin(yaw) * self._rollout_dt
-            xs[:, step] = x
-            ys[:, step] = y
-
-        costs = self.path_cost(xs, ys, target) + 0.01 * np.mean(angular_samples**2, axis=1)
-        weights = np.exp(-(costs - costs.min()) / self._temperature)
-        weights /= np.sum(weights) + 1e-9
-
-        best_angular_sequence = np.sum(weights[:, None] * angular_samples, axis=0)
-        self._last_angular = float(best_angular_sequence[0])
-        self._last_rollout = (xs[np.argmax(weights)].tolist(), ys[np.argmax(weights)].tolist())
-
-        return ControllerOutput(linear=target_speed, angular=self._last_angular)
-
-    def debug_info(self) -> dict:
-        """Expose the best-scoring rollout for visualization, once one has been computed."""
-        if self._last_rollout is None:
-            return {}
-        xs, ys = self._last_rollout
-        return {'rollout_xy': list(zip(xs, ys))}
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/pid_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/pid_controller.py
deleted file mode 100644
index c3bb5b5..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/pid_controller.py
+++ /dev/null
@@ -1,60 +0,0 @@
-import math
-
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.geometry import angle_diff
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class PidController(PathController):
-    """Heading-error PID path follower that steers toward a lookahead point."""
-
-    def __init__(self, lookahead_distance=1.0, goal_tolerance=0.3, kp=1.5, ki=0.0, kd=0.2):
-        self._lookahead_distance = lookahead_distance
-        self._goal_tolerance = goal_tolerance
-        self._kp = kp
-        self._ki = ki
-        self._kd = kd
-        self._progress = PathProgress()
-        self._integral = 0.0
-        self._last_error = 0.0
-        self._last_lookahead = (0.0, 0.0)
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path and reset the integral/derivative state."""
-        self._progress.set_path(path_xy, start_index)
-        self._integral = 0.0
-        self._last_error = 0.0
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end and the robot is within goal_tolerance."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Run one PID step on the heading error toward the lookahead point."""
-        if not self._progress.path:
-            return ControllerOutput(0.0, 0.0)
-
-        self._progress.update(pose)
-        gx, gy = self._progress.lookahead_point(pose, self._lookahead_distance)
-        self._last_lookahead = (gx, gy)
-
-        error = angle_diff(math.atan2(gy - pose.y, gx - pose.x), pose.yaw)
-        safe_dt = max(dt, 1e-3)
-        self._integral += error * safe_dt
-        derivative = (error - self._last_error) / safe_dt
-        self._last_error = error
-
-        angular = self._kp * error + self._ki * self._integral + self._kd * derivative
-        return ControllerOutput(linear=target_speed, angular=angular)
-
-    def debug_info(self) -> dict:
-        """Expose the lookahead point for visualization."""
-        return {
-            'lookahead_xy': self._last_lookahead,
-            'lookahead_distance': self._lookahead_distance,
-        }
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/pure_pursuit_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/pure_pursuit_controller.py
deleted file mode 100644
index 5000784..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/pure_pursuit_controller.py
+++ /dev/null
@@ -1,40 +0,0 @@
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.pose2d import Pose2D
-from robot_navigation.utils.pure_pursuit import PurePursuit
-
-
-class PurePursuitController(PathController):
-    """Pure pursuit adapter: curvature-based steering at a constant cruise speed."""
-
-    def __init__(self, lookahead_distance=1.0, goal_tolerance=0.3):
-        self._pure_pursuit = PurePursuit(
-            lookahead_distance=lookahead_distance, goal_tolerance=goal_tolerance
-        )
-        self._last_curvature = 0.0
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path, optionally pinning progress to start_index."""
-        self._pure_pursuit.set_path(path_xy, start_index)
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Compute the pure pursuit curvature and scale it by the requested speed."""
-        curvature = self._pure_pursuit.update(pose)
-        self._last_curvature = curvature
-        return ControllerOutput(linear=target_speed, angular=target_speed * curvature)
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Delegate to the underlying pure pursuit tracker."""
-        return self._pure_pursuit.is_finished(pose)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._pure_pursuit.progress_index()
-
-    def debug_info(self) -> dict:
-        """Expose the lookahead point, nearest point, and curvature for visualization."""
-        return {
-            'lookahead_xy': self._pure_pursuit.last_lookahead_point(),
-            'nearest_xy': self._pure_pursuit.nearest_point(),
-            'lookahead_distance': self._pure_pursuit.lookahead_distance(),
-            'curvature': self._last_curvature,
-        }
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/registry.py b/src/robot_navigation/robot_navigation/utils/controllers/registry.py
deleted file mode 100644
index ef647c9..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/registry.py
+++ /dev/null
@@ -1,22 +0,0 @@
-from robot_navigation.utils.controllers.holonomic_controller import HolonomicController
-from robot_navigation.utils.controllers.lqr_controller import LqrController
-from robot_navigation.utils.controllers.mppi_controller import MppiController
-from robot_navigation.utils.controllers.pid_controller import PidController
-from robot_navigation.utils.controllers.pure_pursuit_controller import PurePursuitController
-from robot_navigation.utils.controllers.stanley_controller import StanleyController
-
-CONTROLLERS = {
-    'holonomic': HolonomicController,
-    'pure_pursuit': PurePursuitController,
-    'pid': PidController,
-    'mppi': MppiController,
-    'lqr': LqrController,
-    'stanley': StanleyController,
-}
-
-
-def create_controller(name: str, params: dict):
-    """Build a controller instance by name using the given constructor kwargs."""
-    if name not in CONTROLLERS:
-        raise ValueError(f'unknown controller: {name!r}, options are {list(CONTROLLERS)}')
-    return CONTROLLERS[name](**params)
diff --git a/src/robot_navigation/robot_navigation/utils/controllers/stanley_controller.py b/src/robot_navigation/robot_navigation/utils/controllers/stanley_controller.py
deleted file mode 100644
index 3530830..0000000
--- a/src/robot_navigation/robot_navigation/utils/controllers/stanley_controller.py
+++ /dev/null
@@ -1,55 +0,0 @@
-import math
-
-from robot_navigation.utils.controllers.base_controller import ControllerOutput, PathController
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class StanleyController(PathController):
-    """Stanley steering law adapted to a unicycle, with curvature feedforward."""
-
-    def __init__(
-        self,
-        goal_tolerance=0.3,
-        k_cross_track=1.0,
-        k_soft=0.2,
-        k_heading=1.5,
-        max_angular_velocity=1.0,
-    ):
-        self._goal_tolerance = goal_tolerance
-        self._k_cross_track = k_cross_track
-        self._k_soft = k_soft
-        self._k_heading = k_heading
-        self._max_angular_velocity = max_angular_velocity
-        self._progress = PathProgress()
-
-    def set_path(self, path_xy, start_index=None):
-        """Replace the tracked path."""
-        self._progress.set_path(path_xy, start_index)
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end and the robot is within goal_tolerance."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def update(self, pose: Pose2D, dt: float, target_speed: float) -> ControllerOutput:
-        """Steer by -(heading error + atan(k * e / (v + k_soft))) on top of v * curvature."""
-        if not self._progress.path:
-            return ControllerOutput(0.0, 0.0)
-        self._progress.update(pose)
-        ref = self._progress.reference(pose)
-        steer = -(
-            ref.heading_error
-            + math.atan2(self._k_cross_track * ref.lateral_error, target_speed + self._k_soft)
-        )
-        angular = target_speed * ref.curvature + self._k_heading * steer
-        angular = max(-self._max_angular_velocity, min(self._max_angular_velocity, angular))
-        return ControllerOutput(linear=target_speed, angular=angular)
-
-    def debug_info(self) -> dict:
-        """Expose the nearest path point for visualization."""
-        path = self._progress.path
-        return {'nearest_xy': path[self._progress.index]} if path else {}
diff --git a/src/robot_navigation/robot_navigation/utils/final_approach.py b/src/robot_navigation/robot_navigation/utils/final_approach.py
deleted file mode 100644
index 397967b..0000000
--- a/src/robot_navigation/robot_navigation/utils/final_approach.py
+++ /dev/null
@@ -1,107 +0,0 @@
-from dataclasses import dataclass
-import math
-
-from robot_navigation.utils.geometry import angle_diff, clamp, to_local
-from robot_navigation.utils.pose2d import Pose2D
-
-
-@dataclass
-class ApproachCommand:
-    """Velocity command and convergence state of one final-approach step."""
-
-    vx: float = 0.0
-    vy: float = 0.0
-    wz: float = 0.0
-    position_error: float = 0.0
-    yaw_error: float = 0.0
-    done: bool = False
-
-
-class FinalApproach:
-    """
-    Brings the robot exactly onto a target x, y, yaw.
-
-    Holonomic robots (legged, omni) close x/y/yaw simultaneously in the body frame; differential
-    robots turn toward the point, drive to it, then rotate to the target yaw.
-    """
-
-    def __init__(
-        self,
-        holonomic: bool = True,
-        kp_xy: float = 1.2,
-        kp_yaw: float = 1.5,
-        max_speed: float = 0.2,
-        max_yaw_rate: float = 0.6,
-        min_speed: float = 0.04,
-        position_tolerance: float = 0.05,
-        yaw_tolerance: float = math.radians(3.0),
-    ):
-        self._holonomic = holonomic
-        self._kp_xy = kp_xy
-        self._kp_yaw = kp_yaw
-        self._max_speed = max_speed
-        self._max_yaw_rate = max_yaw_rate
-        self._min_speed = min_speed
-        self._pos_tol = position_tolerance
-        self._yaw_tol = yaw_tolerance
-        self._position_reached = False
-
-    def reset(self):
-        """Forget convergence state before a new target."""
-        self._position_reached = False
-
-    def update(self, pose: Pose2D, x: float, y: float, yaw: float) -> ApproachCommand:
-        """One approach step toward (x, y, yaw)."""
-        ex, ey = to_local(x - pose.x, y - pose.y, pose.yaw)
-        dist = math.hypot(ex, ey)
-        eyaw = angle_diff(yaw, pose.yaw)
-        if dist < self._pos_tol:
-            self._position_reached = True
-        elif dist > 2.0 * self._pos_tol:
-            self._position_reached = False
-
-        cmd = ApproachCommand(position_error=dist, yaw_error=eyaw)
-        if self._position_reached and abs(eyaw) < self._yaw_tol:
-            cmd.done = True
-            return cmd
-
-        if self._holonomic:
-            self.holonomic_step(cmd, ex, ey, dist, eyaw)
-        else:
-            self.differential_step(cmd, ex, ey, dist, eyaw)
-        return cmd
-
-    def holonomic_step(self, cmd: ApproachCommand, ex: float, ey: float, dist: float, eyaw: float):
-        """Close x, y and yaw together in the body frame."""
-        if not self._position_reached:
-            speed = min(self._max_speed, max(self._min_speed, self._kp_xy * dist))
-            cmd.vx, cmd.vy = speed * ex / dist, speed * ey / dist
-        cmd.wz = self.yaw_rate(eyaw)
-
-    def differential_step(
-        self, cmd: ApproachCommand, ex: float, ey: float, dist: float, eyaw: float
-    ):
-        """Turn toward the point, drive onto it, then rotate to the target yaw."""
-        if self._position_reached:
-            cmd.wz = self.yaw_rate(eyaw)
-            return
-        bearing = math.atan2(ey, ex)
-        if abs(bearing) > math.pi / 2.0:
-            bearing = angle_diff(bearing, math.pi)
-            direction = -1.0
-        else:
-            direction = 1.0
-        if abs(bearing) > math.radians(20.0):
-            cmd.wz = self.yaw_rate(bearing)
-            return
-        speed = min(self._max_speed, max(self._min_speed, self._kp_xy * dist))
-        cmd.vx = direction * speed * math.cos(bearing)
-        cmd.wz = self.yaw_rate(bearing)
-
-    def yaw_rate(self, error: float) -> float:
-        """Proportional yaw rate with a small floor so the last degrees still converge."""
-        if abs(error) < self._yaw_tol:
-            return 0.0
-        rate = self._kp_yaw * error
-        floor = 0.1 if abs(rate) < 0.1 else 0.0
-        return clamp(math.copysign(max(abs(rate), floor), error), self._max_yaw_rate)
diff --git a/src/robot_navigation/robot_navigation/utils/geometry.py b/src/robot_navigation/robot_navigation/utils/geometry.py
deleted file mode 100644
index c7d20df..0000000
--- a/src/robot_navigation/robot_navigation/utils/geometry.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import math
-
-
-def wrap_angle(a: float) -> float:
-    """Wrap an angle into (-pi, pi]."""
-    return math.atan2(math.sin(a), math.cos(a))
-
-
-def angle_diff(a: float, b: float) -> float:
-    """Signed difference a - b wrapped into (-pi, pi]."""
-    return wrap_angle(a - b)
-
-
-def clamp(value: float, limit: float) -> float:
-    """Clamp value into [-limit, limit]."""
-    return max(-limit, min(limit, value))
-
-
-def yaw_from_quaternion(q) -> float:
-    """Yaw angle of a geometry_msgs Quaternion."""
-    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 to_local(dx: float, dy: float, yaw: float) -> tuple[float, float]:
-    """Rotate a world-frame vector into a frame with heading yaw."""
-    c, s = math.cos(yaw), math.sin(yaw)
-    return c * dx + s * dy, -s * dx + c * dy
diff --git a/src/robot_navigation/robot_navigation/utils/path_processing.py b/src/robot_navigation/robot_navigation/utils/path_processing.py
deleted file mode 100644
index fa2569c..0000000
--- a/src/robot_navigation/robot_navigation/utils/path_processing.py
+++ /dev/null
@@ -1,200 +0,0 @@
-import math
-
-Waypoint = tuple[float, float, float, float]  # x, y, yaw_deg, dwell_sec
-
-
-def dedupe(points: list[tuple[float, float]], min_spacing: float) -> list[tuple[float, float]]:
-    """Drop points closer than min_spacing to the previously kept one (last point always kept)."""
-    if len(points) < 2:
-        return list(points)
-    out = [points[0]]
-    for p in points[1:-1]:
-        if math.hypot(p[0] - out[-1][0], p[1] - out[-1][1]) >= min_spacing:
-            out.append(p)
-    if math.hypot(points[-1][0] - out[-1][0], points[-1][1] - out[-1][1]) > 1e-6:
-        out.append(points[-1])
-    return out
-
-
-def resample(points: list[tuple[float, float]], spacing: float) -> list[tuple[float, float]]:
-    """Resample a polyline at a uniform arc-length spacing, keeping both endpoints."""
-    if len(points) < 2 or spacing <= 0.0:
-        return list(points)
-    out = [points[0]]
-    carry = 0.0
-    for (x0, y0), (x1, y1) in zip(points, points[1:]):
-        seg = math.hypot(x1 - x0, y1 - y0)
-        s = spacing - carry
-        while s <= seg:
-            t = s / seg
-            out.append((x0 + t * (x1 - x0), y0 + t * (y1 - y0)))
-            s += spacing
-        carry = seg - (s - spacing)
-    if math.hypot(points[-1][0] - out[-1][0], points[-1][1] - out[-1][1]) > spacing * 0.3:
-        out.append(points[-1])
-    else:
-        out[-1] = points[-1]
-    return out
-
-
-def smooth(
-    points: list[tuple[float, float]],
-    weight_data: float,
-    weight_smooth: float,
-    tolerance: float = 1e-5,
-    max_iterations: int = 2000,
-) -> list[tuple[float, float]]:
-    """Gradient-descent smoothing with both endpoints fixed."""
-    if len(points) < 3:
-        return list(points)
-    orig = [list(p) for p in points]
-    new = [list(p) for p in points]
-    for _ in range(max_iterations):
-        change = 0.0
-        for i in range(1, len(new) - 1):
-            for d in (0, 1):
-                prev = new[i][d]
-                new[i][d] += weight_data * (orig[i][d] - new[i][d])
-                new[i][d] += weight_smooth * (new[i - 1][d] + new[i + 1][d] - 2.0 * new[i][d])
-                change += abs(prev - new[i][d])
-        if change < tolerance:
-            break
-    return [(p[0], p[1]) for p in new]
-
-
-def headings(points: list[tuple[float, float]]) -> list[float]:
-    """Tangent heading (rad) at every point using central differences."""
-    n = len(points)
-    if n < 2:
-        return [0.0] * n
-    out = []
-    for i in range(n):
-        a, b = points[max(i - 1, 0)], points[min(i + 1, n - 1)]
-        out.append(math.atan2(b[1] - a[1], b[0] - a[0]))
-    return out
-
-
-def curvatures(points: list[tuple[float, float]], span: int = 3) -> list[float]:
-    """Unsigned Menger curvature (1/m) at every point from neighbours `span` indices away."""
-    n = len(points)
-    out = [0.0] * n
-    for i in range(span, n - span):
-        (ax, ay), (bx, by), (cx, cy) = points[i - span], points[i], points[i + span]
-        ab = math.hypot(bx - ax, by - ay)
-        bc = math.hypot(cx - bx, cy - by)
-        ca = math.hypot(ax - cx, ay - cy)
-        area2 = abs((bx - ax) * (cy - ay) - (by - ay) * (cx - ax))
-        denom = ab * bc * ca
-        out[i] = 2.0 * area2 / denom if denom > 1e-9 else 0.0
-    return out
-
-
-def process_route(
-    waypoints: list[Waypoint], spacing: float, weight_data: float, weight_smooth: float
-) -> tuple[list[Waypoint], float]:
-    """
-    Resample and smooth a recorded route between anchors (ends and stop points).
-
-    Returns the processed route and the maximum deviation from the recording in metres.
-    """
-    if len(waypoints) < 3:
-        return list(waypoints), 0.0
-    anchors = [0] + [
-        i for i, w in enumerate(waypoints) if w[3] > 0.0 and 0 < i < len(waypoints) - 1
-    ]
-    anchors.append(len(waypoints) - 1)
-
-    route: list[Waypoint] = []
-    max_dev = 0.0
-    for a, b in zip(anchors, anchors[1:]):
-        raw = [(w[0], w[1]) for w in waypoints[a:b + 1]]
-        pts = smooth(resample(dedupe(raw, spacing * 0.5), spacing), weight_data, weight_smooth)
-        yaws = headings(pts)
-        max_dev = max(max_dev, max_deviation(pts, raw))
-        for j, (x, y) in enumerate(pts):
-            if j == 0 and route:
-                continue
-            if j == 0:
-                route.append(waypoints[a])
-            elif j == len(pts) - 1:
-                route.append(waypoints[b])
-            else:
-                route.append((x, y, math.degrees(yaws[j]), 0.0))
-    return route, max_dev
-
-
-def point_segment_distance(p, a, b) -> float:
-    """Distance from point p to segment ab."""
-    dx, dy = b[0] - a[0], b[1] - a[1]
-    length_sq = dx * dx + dy * dy
-    t = (
-        0.0
-        if length_sq < 1e-12
-        else max(0.0, min(1.0, ((p[0] - a[0]) * dx + (p[1] - a[1]) * dy) / length_sq))
-    )
-    return math.hypot(p[0] - (a[0] + t * dx), p[1] - (a[1] + t * dy))
-
-
-def max_deviation(path: list[tuple[float, float]], reference: list[tuple[float, float]]) -> float:
-    """Largest distance from any path point to the reference polyline."""
-    if len(reference) < 2:
-        return 0.0
-    segments = list(zip(reference, reference[1:]))
-    return max(min(point_segment_distance(p, a, b) for a, b in segments) for p in path)
-
-
-def velocity_profile(
-    points: list[tuple[float, float]],
-    stop_indices: list[int],
-    cruise: float,
-    min_speed: float,
-    max_lateral_accel: float,
-    decel: float,
-    approach_speed: float,
-) -> list[float]:
-    """Return the speed limit per point: curvature cap, then braking into stops and the end."""
-    n = len(points)
-    if n == 0:
-        return []
-    kappa = curvatures(points)
-    v = [
-        max(min_speed, min(cruise, math.sqrt(max_lateral_accel / k) if k > 1e-6 else cruise))
-        for k in kappa
-    ]
-    for i in stop_indices:
-        v[i] = min(v[i], approach_speed)
-    v[-1] = min(v[-1], approach_speed)
-    for i in range(n - 2, -1, -1):
-        ds = math.hypot(points[i + 1][0] - points[i][0], points[i + 1][1] - points[i][1])
-        v[i] = min(v[i], math.sqrt(v[i + 1] ** 2 + 2.0 * decel * ds))
-    return v
-
-
-def parse_route_rows(rows) -> tuple[list[Waypoint], list[str]]:
-    """
-    Parse route CSV rows (x, y[, yaw_deg[, dwell_sec]]) into waypoints.
-
-    Comments and blank rows are skipped. A row that is not numeric, or holds NaN or infinity, is
-    dropped and reported: it would steer the robot nowhere. A negative dwell is reported and read
-    as 0 (no stop).
-    """
-    waypoints, problems = [], []
-    for line_no, row in enumerate(rows, start=1):
-        if not row or not row[0].strip() or row[0].strip().startswith('#'):
-            continue
-        try:
-            values = [float(v) for v in row[:4]]
-            x, y = values[0], values[1]
-        except (ValueError, IndexError):
-            problems.append(f'line {line_no}: not a waypoint: {row}')
-            continue
-        yaw_deg = values[2] if len(values) > 2 else 0.0
-        dwell_sec = values[3] if len(values) > 3 else 0.0
-        if not all(math.isfinite(v) for v in (x, y, yaw_deg, dwell_sec)):
-            problems.append(f'line {line_no}: not finite: {row}')
-            continue
-        if dwell_sec < 0.0:
-            problems.append(f'line {line_no}: negative dwell {dwell_sec:g} s, not a stop')
-            dwell_sec = 0.0
-        waypoints.append((x, y, yaw_deg, dwell_sec))
-    return waypoints, problems
diff --git a/src/robot_navigation/robot_navigation/utils/path_progress.py b/src/robot_navigation/robot_navigation/utils/path_progress.py
deleted file mode 100644
index c108775..0000000
--- a/src/robot_navigation/robot_navigation/utils/path_progress.py
+++ /dev/null
@@ -1,174 +0,0 @@
-from bisect import bisect_right
-from dataclasses import dataclass
-import math
-
-from robot_navigation.utils.geometry import wrap_angle
-from robot_navigation.utils.pose2d import Pose2D
-
-
-@dataclass
-class PathReference:
-    """Robot pose expressed relative to the path at the current progress index."""
-
-    lateral_error: float
-    heading_error: float
-    curvature: float
-    path_yaw: float
-
-
-class PathProgress:
-    """Monotonic progress along a polyline path, robust to overlapping or looping routes."""
-
-    def __init__(self, search_window: int = 200, heading_weight: float = 1.0):
-        self._search_window = search_window
-        self._heading_weight = heading_weight
-        self._path: list[tuple[float, float]] = []
-        self._index = 0
-        self._initialized = False
-
-    def set_path(self, path_xy: list[tuple[float, float]], start_index=None):
-        """Replace the path; without start_index the next update searches the whole path."""
-        self._path = list(path_xy)
-        self._index = start_index or 0
-        self._initialized = start_index is not None
-        self._curvature = self.signed_curvatures()
-        self._s = [0.0]  # cumulative arc length at every point
-        for (ax, ay), (bx, by) in zip(self._path, self._path[1:]):
-            self._s.append(self._s[-1] + math.hypot(bx - ax, by - ay))
-
-    @property
-    def arc_lengths(self) -> list[float]:
-        """Cumulative arc length at every path point."""
-        return self._s
-
-    @property
-    def length(self) -> float:
-        """Total arc length of the path in metres."""
-        return self._s[-1] if self._path else 0.0
-
-    def projection(self, pose: Pose2D) -> float:
-        """Arc length of the robot's projection onto the segment at the progress index."""
-        if len(self._path) < 2:
-            return 0.0
-        i = min(self._index, len(self._path) - 2)
-        fraction = max(0.0, min(1.0, self.segment_fraction(i, pose)))
-        return self._s[i] + fraction * (self._s[i + 1] - self._s[i])
-
-    def point_at(self, s: float) -> tuple[float, float]:
-        """Point at arc length s (clamped to the path), linearly interpolated."""
-        if not self._path:
-            return 0.0, 0.0
-        if len(self._path) == 1 or s <= 0.0:
-            return self._path[0]
-        if s >= self._s[-1]:
-            return self._path[-1]
-        i = bisect_right(self._s, s) - 1
-        length = self._s[i + 1] - self._s[i]
-        t = (s - self._s[i]) / length if length > 1e-12 else 0.0
-        (ax, ay), (bx, by) = self._path[i], self._path[i + 1]
-        return ax + t * (bx - ax), ay + t * (by - ay)
-
-    def heading_between(self, s0: float, s1: float) -> float:
-        """Heading of the chord from arc length s0 to s1 (a smoothed path tangent)."""
-        ax, ay = self.point_at(s0)
-        bx, by = self.point_at(s1)
-        if math.hypot(bx - ax, by - ay) < 1e-6:
-            i = min(max(bisect_right(self._s, s0) - 1, 0), max(len(self._path) - 2, 0))
-            return self.segment_heading(i)
-        return math.atan2(by - ay, bx - ax)
-
-    @property
-    def path(self) -> list[tuple[float, float]]:
-        """Return the tracked path."""
-        return self._path
-
-    @property
-    def index(self) -> int:
-        """Return the current nearest path index."""
-        return self._index
-
-    def segment_heading(self, i: int) -> float:
-        """Heading of the path segment starting at index i."""
-        j = min(i + 1, len(self._path) - 1)
-        k = max(j - 1, 0)
-        return math.atan2(self._path[j][1] - self._path[k][1], self._path[j][0] - self._path[k][0])
-
-    def score(self, i: int, pose: Pose2D, tracking: bool = False) -> float:
-        """Distance to point i plus a penalty for driving against the segment direction."""
-        d = math.hypot(self._path[i][0] - pose.x, self._path[i][1] - pose.y)
-        weight = self._heading_weight * (0.3 if tracking else 1.0)
-        return d + weight * abs(wrap_angle(self.segment_heading(i) - pose.yaw)) / math.pi
-
-    def update(self, pose: Pose2D) -> int:
-        """Advance the nearest index (global search on first call, forward window afterwards)."""
-        if not self._path:
-            return 0
-        if not self._initialized:
-            self._index = min(range(len(self._path)), key=lambda i: self.score(i, pose))
-            self._initialized = True
-            return self._index
-        end = min(len(self._path), self._index + self._search_window)
-        self._index = min(
-            range(self._index, end), key=lambda i: self.score(i, pose, tracking=True)
-        )
-        while self._index < len(self._path) - 2 and self.segment_fraction(self._index, pose) > 1.0:
-            self._index += 1
-        return self._index
-
-    def segment_fraction(self, i: int, pose: Pose2D) -> float:
-        """Projection of the robot onto segment i -> i+1 (0 at start, 1 at end)."""
-        (ax, ay), (bx, by) = self._path[i], self._path[i + 1]
-        dx, dy = bx - ax, by - ay
-        length_sq = dx * dx + dy * dy
-        return ((pose.x - ax) * dx + (pose.y - ay) * dy) / length_sq if length_sq > 1e-12 else 1.0
-
-    def signed_curvatures(self, span: int = 3) -> list[float]:
-        """Signed curvature (1/m, +left) at every point from heading change over +-span points."""
-        n = len(self._path)
-        out = [0.0] * n
-        for i in range(span, n - span):
-            (ax, ay), (bx, by), (cx, cy) = (
-                self._path[i - span],
-                self._path[i],
-                self._path[i + span],
-            )
-            h1 = math.atan2(by - ay, bx - ax)
-            h2 = math.atan2(cy - by, cx - bx)
-            length = math.hypot(bx - ax, by - ay) + math.hypot(cx - bx, cy - by)
-            if length > 1e-6:
-                out[i] = 2.0 * wrap_angle(h2 - h1) / length
-        return out
-
-    def reference(self, pose: Pose2D) -> PathReference:
-        """Return lateral error (+left), heading error, curvature and path yaw at the index."""
-        i = min(self._index, len(self._path) - 2)
-        (ax, ay), (bx, by) = self._path[i], self._path[i + 1]
-        path_yaw = math.atan2(by - ay, bx - ax)
-        dx, dy = pose.x - ax, pose.y - ay
-        lateral = math.cos(path_yaw) * dy - math.sin(path_yaw) * dx
-        return PathReference(
-            lateral_error=lateral,
-            heading_error=wrap_angle(pose.yaw - path_yaw),
-            curvature=self._curvature[self._index] if self._curvature else 0.0,
-            path_yaw=path_yaw,
-        )
-
-    def lookahead_point(self, pose: Pose2D, distance: float) -> tuple[float, float]:
-        """First path point at least `distance` away, walking forward from the current index."""
-        for i in range(self._index, len(self._path)):
-            if math.hypot(self._path[i][0] - pose.x, self._path[i][1] - pose.y) >= distance:
-                return self._path[i]
-        return self._path[-1]
-
-    def window(self, count: int) -> list[tuple[float, float]]:
-        """Up to `count` path points starting at the current index."""
-        return self._path[self._index:self._index + count]
-
-    def is_finished(self, pose: Pose2D, tolerance: float, end_margin: int = 3) -> bool:
-        """Return True once progress reached the end and the robot is within tolerance."""
-        if not self._path:
-            return True
-        if self._index < len(self._path) - 1 - end_margin:
-            return False
-        gx, gy = self._path[-1]
-        return math.hypot(gx - pose.x, gy - pose.y) < tolerance
diff --git a/src/robot_navigation/robot_navigation/utils/path_shift.py b/src/robot_navigation/robot_navigation/utils/path_shift.py
deleted file mode 100644
index d1f6ec0..0000000
--- a/src/robot_navigation/robot_navigation/utils/path_shift.py
+++ /dev/null
@@ -1,150 +0,0 @@
-"""
-Path-shift avoidance for holonomic robots: slide sideways around obstacles, keep the heading.
-
-Pure numpy, unit-tested without ROS. Obstacle points (map frame) are expressed in path
-coordinates, arc length along the route and signed distance to its left. A lateral offset `o`
-is clear when no point lies in the robot's corridor (|d - o| < half_width + clearance) from its
-rear edge to `look_ahead` beyond its front edge. Points beside the body block an offset outright
-(the robot would slide into them), points ahead limit how far it can drive.
-
-The offset nearest the route that is clear wins, with a cost on changing it, so the robot does not
-flip sides and only returns to the route once the obstacle is beside or behind it and the route
-corridor is free along its whole body. It never leaves the route by more than `max_offset`: with
-no clear offset it slows down and stops short of the obstacle instead of wandering off.
-"""
-
-from dataclasses import dataclass
-import math
-
-import numpy as np
-
-
-@dataclass
-class ShiftResult:
-    """Chosen offset and the free distance ahead of the robot's front edge on it."""
-
-    offset: float          # m, +left of the route
-    free_distance: float   # m from the front edge to the first obstacle on that offset (inf)
-    clear: bool            # False: no offset within max_offset is free, the robot must slow
-
-
-@dataclass
-class PathShiftConfig:
-    """Footprint, search range and stopping model, in metres, seconds and m/s^2."""
-
-    max_offset: float = 0.8       # furthest the robot leaves the route
-    step: float = 0.1             # candidate spacing
-    look_ahead: float = 2.0       # route ahead of the front edge that must be free
-    front: float = 0.40           # base_link to the front edge
-    rear: float = 0.40            # base_link to the rear edge
-    half_width: float = 0.28      # base_link to the side edges
-    clearance: float = 0.10       # gap kept to obstacles beside the corridor
-    switch_cost: float = 0.5      # cost per metre of changing the offset (hysteresis)
-    switch_margin: float = 0.05   # extra clearance a new offset needs, so it does not flicker
-    decel: float = 0.5            # braking used to limit speed toward an obstacle
-    stop_margin: float = 0.40     # stop this far before an obstacle (front edge)
-
-
-class PathShift:
-    """Choose the lateral offset from the route that passes obstacles (see module docstring)."""
-
-    def __init__(self, cfg: PathShiftConfig = PathShiftConfig()):
-        self.cfg = cfg
-        n = int(round(cfg.max_offset / cfg.step))
-        self.candidates = np.array([i * cfg.step for i in range(-n, n + 1)])
-
-    def path_coordinates(self, path_xy, path_s, s_robot: float, points: np.ndarray):
-        """
-        Return (s, d) of every point that is near the route around the robot.
-
-        path_xy / path_s: route points and their cumulative arc length. Only the route from the
-        rear edge to look_ahead beyond the front edge is used.
-        """
-        cfg = self.cfg
-        if points.size == 0 or len(path_xy) < 2:
-            return np.empty(0), np.empty(0)
-        s_lo = s_robot - cfg.rear - 0.5
-        s_hi = s_robot + cfg.front + cfg.look_ahead + 0.5
-        s = np.asarray(path_s)
-        i0 = max(int(np.searchsorted(s, s_lo)) - 1, 0)
-        i1 = min(int(np.searchsorted(s, s_hi)) + 1, len(s) - 1)
-        if i1 <= i0:
-            return np.empty(0), np.empty(0)
-        xy = np.asarray(path_xy[i0:i1 + 1], dtype=float)
-        a, b = xy[:-1], xy[1:]
-        seg = b - a
-        length = np.maximum(np.hypot(seg[:, 0], seg[:, 1]), 1e-9)
-        rel = points[:, None, :] - a[None, :, :]                     # n x k x 2
-        t_raw = (rel * seg[None]).sum(axis=2) / length**2
-        t = np.clip(t_raw, 0.0, 1.0)
-        closest = a[None] + t[..., None] * seg[None]
-        dist = np.hypot(points[:, None, 0] - closest[..., 0], points[:, None, 1] - closest[..., 1])
-        k = dist.argmin(axis=1)
-        rows = np.arange(points.shape[0])
-        cross = seg[k, 0] * rel[rows, k, 1] - seg[k, 1] * rel[rows, k, 0]
-        d = cross / length[k]
-        # before the window's first or after its last segment, measure along the extension, so
-        # a wall behind the start of the route is behind the robot, not beside it
-        along = t[rows, k]
-        first, last = k == 0, k == seg.shape[0] - 1
-        along = np.where(first & (t_raw[rows, k] < 0.0), t_raw[rows, k], along)
-        along = np.where(last & (t_raw[rows, k] > 1.0), t_raw[rows, k], along)
-        s_pts = s[i0:i1][k] + along * length[k]
-        reach = cfg.max_offset + cfg.half_width + cfg.clearance + 0.2
-        near = np.abs(d) < reach
-        if i1 == len(s) - 1:
-            # past the end of the route (the goal, or a loop that restarts): not in the way
-            near &= ~(last & (t_raw[rows, k] > 1.0))
-        return s_pts[near], d[near]
-
-    def free_distances(self, s_pts, d_pts, s_robot: float, offsets, extra=0.0) -> np.ndarray:
-        """Free distance ahead of the front edge for each offset; 0 if something is beside."""
-        cfg = self.cfg
-        offsets = np.atleast_1d(np.asarray(offsets, dtype=float))
-        free = np.full(offsets.shape, math.inf)
-        if s_pts.size == 0:
-            return free
-        width = cfg.half_width + cfg.clearance + np.broadcast_to(extra, offsets.shape)
-        band = np.abs(d_pts[None, :] - offsets[:, None]) < width[:, None]
-        ahead = s_pts - (s_robot + cfg.front)                         # from the front edge
-        beside = (s_pts >= s_robot - cfg.rear) & (ahead <= 0.0)
-        in_range = (ahead > 0.0) & (ahead <= cfg.look_ahead)
-        blocked_beside = (band & beside[None, :]).any(axis=1)
-        for i in range(offsets.size):
-            hits = ahead[band[i] & in_range]
-            if hits.size:
-                free[i] = hits.min()
-        free[blocked_beside] = 0.0
-        return free
-
-    def choose(self, s_pts, d_pts, s_robot: float, current: float) -> ShiftResult:
-        """Pick the offset for this tick from obstacle path coordinates (path_coordinates())."""
-        # the current offset keeps its plain clearance, any other needs switch_margin more
-        extra = np.where(
-            np.abs(self.candidates - current) < 1e-6, 0.0, self.cfg.switch_margin
-        )
-        free = self.free_distances(s_pts, d_pts, s_robot, self.candidates, extra)
-        clear = np.isinf(free)
-        if clear.any():
-            cost = np.abs(self.candidates) + self.cfg.switch_cost * np.abs(
-                self.candidates - current
-            )
-            cost[~clear] = math.inf
-            i = int(cost.argmin())
-            return ShiftResult(float(self.candidates[i]), math.inf, True)
-        # nothing is clear: take the offset that gets furthest, preferring the current one
-        best = free.max()
-        ties = np.flatnonzero(free >= best - 1e-6)
-        i = int(ties[np.abs(self.candidates[ties] - current).argmin()])
-        return ShiftResult(float(self.candidates[i]), float(free[i]), False)
-
-    def free_at(self, s_pts, d_pts, s_robot: float, offset: float) -> float:
-        """Free distance ahead on one offset, e.g. where the robot is now."""
-        return float(self.free_distances(s_pts, d_pts, s_robot, [offset])[0])
-
-    def speed_limit(self, free_distance: float) -> float:
-        """Highest speed that still stops stop_margin before an obstacle free_distance ahead."""
-        if math.isinf(free_distance):
-            return math.inf
-        room = free_distance - self.cfg.stop_margin
-        return math.sqrt(2.0 * self.cfg.decel * room) if room > 0.0 else 0.0
diff --git a/src/robot_navigation/robot_navigation/utils/pose2d.py b/src/robot_navigation/robot_navigation/utils/pose2d.py
deleted file mode 100644
index 3685949..0000000
--- a/src/robot_navigation/robot_navigation/utils/pose2d.py
+++ /dev/null
@@ -1,10 +0,0 @@
-from dataclasses import dataclass
-
-
-@dataclass
-class Pose2D:
-    """A 2D pose: x, y position and yaw heading."""
-
-    x: float = 0.0
-    y: float = 0.0
-    yaw: float = 0.0
diff --git a/src/robot_navigation/robot_navigation/utils/pure_pursuit.py b/src/robot_navigation/robot_navigation/utils/pure_pursuit.py
deleted file mode 100644
index b922658..0000000
--- a/src/robot_navigation/robot_navigation/utils/pure_pursuit.py
+++ /dev/null
@@ -1,61 +0,0 @@
-import math
-
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.pose2d import Pose2D
-
-
-class PurePursuit:
-    """Tracks a path of x,y points and outputs steering curvature toward a lookahead point."""
-
-    def __init__(self, lookahead_distance: float = 1.0, goal_tolerance: float = 0.3):
-        self._lookahead_distance = lookahead_distance
-        self._goal_tolerance = goal_tolerance
-        self._progress = PathProgress()
-        self._last_lookahead = (0.0, 0.0)
-
-    def set_path(self, path_xy: list[tuple[float, float]], start_index=None):
-        """Replace the tracked path, optionally pinning progress to start_index."""
-        self._progress.set_path(path_xy, start_index)
-
-    def set_lookahead_distance(self, distance: float):
-        """Update the lookahead distance, floored to avoid a degenerate zero-length lookahead."""
-        self._lookahead_distance = max(0.05, distance)
-
-    def lookahead_distance(self) -> float:
-        """Return the currently configured lookahead distance."""
-        return self._lookahead_distance
-
-    def last_lookahead_point(self) -> tuple[float, float]:
-        """Return the lookahead point computed by the most recent update() call."""
-        return self._last_lookahead
-
-    def nearest_point(self) -> tuple[float, float]:
-        """Return the path point at the current progress index."""
-        path = self._progress.path
-        return path[self._progress.index] if path else (0.0, 0.0)
-
-    def progress_index(self) -> int:
-        """Return the current progress index along the path."""
-        return self._progress.index
-
-    def is_finished(self, pose: Pose2D) -> bool:
-        """Return True once progress reached the end and the robot is within goal_tolerance."""
-        return self._progress.is_finished(pose, self._goal_tolerance)
-
-    def update(self, pose: Pose2D) -> float:
-        """Advance tracking to the current pose and return the target curvature (1/m, +left)."""
-        if not self._progress.path:
-            return 0.0
-
-        self._progress.update(pose)
-        gx, gy = self._progress.lookahead_point(pose, self._lookahead_distance)
-        self._last_lookahead = (gx, gy)
-
-        dx, dy = gx - pose.x, gy - pose.y
-        local_x = math.cos(-pose.yaw) * dx - math.sin(-pose.yaw) * dy
-        local_y = math.sin(-pose.yaw) * dx + math.cos(-pose.yaw) * dy
-
-        lookahead_sq = local_x * local_x + local_y * local_y
-        if lookahead_sq < 1e-6:
-            return 0.0
-        return 2.0 * local_y / lookahead_sq
diff --git a/src/robot_navigation/robot_navigation/utils/safety.py b/src/robot_navigation/robot_navigation/utils/safety.py
deleted file mode 100644
index 7ff9d0d..0000000
--- a/src/robot_navigation/robot_navigation/utils/safety.py
+++ /dev/null
@@ -1,53 +0,0 @@
-class SafetySupervisor:
-    """
-    Debounces emergency stops and detects blocked and off-path conditions.
-
-    All times are seconds from a monotonic clock supplied by the caller.
-    """
-
-    def __init__(
-        self,
-        estop_release_sec: float = 1.0,
-        blocked_timeout_sec: float = 30.0,
-        off_path_slow_distance: float = 0.6,
-        off_path_stop_distance: float = 1.5,
-    ):
-        self._estop_release = estop_release_sec
-        self._blocked_timeout = blocked_timeout_sec
-        self._off_path_slow = off_path_slow_distance
-        self._off_path_stop = off_path_stop_distance
-        self.reset()
-
-    def reset(self):
-        """Clear all latches, e.g. when navigation is (re)started."""
-        self._estop_latched = False
-        self._estop_clear_since = None
-        self._stalled_since = None
-        self._off_path_latched = False
-
-    def estop(self, emergency: bool, now: float) -> bool:
-        """Latch on any emergency and release only after it stayed clear for estop_release_sec."""
-        if emergency:
-            self._estop_latched = True
-            self._estop_clear_since = None
-        elif self._estop_latched:
-            if self._estop_clear_since is None:
-                self._estop_clear_since = now
-            elif now - self._estop_clear_since >= self._estop_release:
-                self._estop_latched = False
-        return self._estop_latched
-
-    def blocked(self, stalled: bool, now: float) -> bool:
-        """Return True once the robot could not make progress for blocked_timeout_sec."""
-        if not stalled:
-            self._stalled_since = None
-            return False
-        if self._stalled_since is None:
-            self._stalled_since = now
-        return now - self._stalled_since >= self._blocked_timeout
-
-    def off_path(self, cross_track: float) -> tuple[bool, bool]:
-        """(slow, stop) for the current cross-track error; stop latches until reset()."""
-        if cross_track > self._off_path_stop:
-            self._off_path_latched = True
-        return cross_track > self._off_path_slow, self._off_path_latched
diff --git a/src/robot_navigation/robot_navigation/utils/speed_regulator.py b/src/robot_navigation/robot_navigation/utils/speed_regulator.py
deleted file mode 100644
index ecb6def..0000000
--- a/src/robot_navigation/robot_navigation/utils/speed_regulator.py
+++ /dev/null
@@ -1,26 +0,0 @@
-class SpeedRegulator:
-    """Scale the linear velocity down on sharp turns, independent of the steering controller."""
-
-    def __init__(self, kp=0.3, ki=0.0, kd=0.0, min_scale=0.3):
-        self._kp = kp
-        self._ki = ki
-        self._kd = kd
-        self._min_scale = min_scale
-        self._integral = 0.0
-        self._last_error = 0.0
-
-    def reset(self):
-        """Clear the integral and derivative state, e.g. when a new path is loaded."""
-        self._integral = 0.0
-        self._last_error = 0.0
-
-    def scale_for(self, angular: float, dt: float) -> float:
-        """Return a 0..1 velocity multiplier that shrinks as |angular| grows."""
-        error = abs(angular)
-        safe_dt = max(dt, 1e-3)
-        self._integral += error * safe_dt
-        derivative = (error - self._last_error) / safe_dt
-        self._last_error = error
-
-        correction = self._kp * error + self._ki * self._integral + self._kd * derivative
-        return max(self._min_scale, min(1.0, 1.0 - correction))
diff --git a/src/robot_navigation/robot_navigation/utils/visualization.py b/src/robot_navigation/robot_navigation/utils/visualization.py
deleted file mode 100644
index 54754f4..0000000
--- a/src/robot_navigation/robot_navigation/utils/visualization.py
+++ /dev/null
@@ -1,166 +0,0 @@
-import math
-
-from geometry_msgs.msg import Point
-from std_msgs.msg import ColorRGBA
-from visualization_msgs.msg import Marker
-
-STATE_COLORS = {
-    'FOLLOWING': (0.2, 0.9, 0.3),
-    'APPROACHING': (1.0, 0.85, 0.1),
-    'JOINING': (1.0, 0.85, 0.1),
-    'DWELLING': (0.2, 0.6, 1.0),
-    'GOAL_REACHED': (0.4, 0.6, 1.0),
-    'PAUSED': (0.7, 0.7, 0.7),
-    'NAV_INACTIVE': (0.5, 0.5, 0.5),
-    'BLOCKED': (1.0, 0.5, 0.0),
-    'NO_PATH': (1.0, 0.5, 0.0),
-}
-ALARM = (1.0, 0.15, 0.15)
-
-
-def rgba(rgb, a: float = 1.0) -> ColorRGBA:
-    """Return a ColorRGBA from an (r, g, b) tuple."""
-    return ColorRGBA(r=float(rgb[0]), g=float(rgb[1]), b=float(rgb[2]), a=float(a))
-
-
-def state_color(state: str) -> tuple:
-    """RGB for a follower state; unknown states are alarms."""
-    return STATE_COLORS.get(state, ALARM)
-
-
-def speed_color(v: float, v_max: float) -> ColorRGBA:
-    """Red (slow) to green (fast) colour for a speed."""
-    t = max(0.0, min(1.0, v / v_max)) if v_max > 0 else 1.0
-    return ColorRGBA(r=float(1.0 - t), g=float(0.3 + 0.7 * t), b=0.1, a=1.0)
-
-
-def base(frame: str, stamp, ns: str, mid: int, kind: int) -> Marker:
-    """Marker skeleton with identity orientation."""
-    m = Marker()
-    m.header.frame_id = frame
-    m.header.stamp = stamp
-    m.ns = ns
-    m.id = mid
-    m.type = kind
-    m.action = Marker.ADD
-    m.pose.orientation.w = 1.0
-    return m
-
-
-def pt(x: float, y: float, z: float = 0.0) -> Point:
-    """geometry_msgs Point from floats."""
-    return Point(x=float(x), y=float(y), z=float(z))
-
-
-def circle(
-    frame,
-    stamp,
-    ns,
-    mid,
-    cx,
-    cy,
-    radius,
-    rgb,
-    width=0.02,
-    alpha=0.8,
-    segments=48,
-    start=0.0,
-    sweep=2 * math.pi,
-    z=0.02,
-) -> Marker:
-    """Circle or arc outline."""
-    m = base(frame, stamp, ns, mid, Marker.LINE_STRIP)
-    m.scale.x = width
-    m.color = rgba(rgb, alpha)
-    for i in range(segments + 1):
-        a = start + sweep * i / segments
-        m.points.append(pt(cx + radius * math.cos(a), cy + radius * math.sin(a), z))
-    return m
-
-
-def disc(frame, stamp, ns, mid, x, y, radius, rgb, alpha=0.35, height=0.01) -> Marker:
-    """Flat filled disc."""
-    m = base(frame, stamp, ns, mid, Marker.CYLINDER)
-    m.pose.position = pt(x, y, height / 2)
-    m.scale.x = m.scale.y = 2 * radius
-    m.scale.z = height
-    m.color = rgba(rgb, alpha)
-    return m
-
-
-def sphere(frame, stamp, ns, mid, x, y, size, rgb, z=0.05, alpha=1.0) -> Marker:
-    """Small sphere."""
-    m = base(frame, stamp, ns, mid, Marker.SPHERE)
-    m.pose.position = pt(x, y, z)
-    m.scale.x = m.scale.y = m.scale.z = size
-    m.color = rgba(rgb, alpha)
-    return m
-
-
-def line(frame, stamp, ns, mid, points, rgb, width=0.03, alpha=1.0, z=0.03) -> Marker:
-    """Polyline through (x, y) points."""
-    m = base(frame, stamp, ns, mid, Marker.LINE_STRIP)
-    m.scale.x = width
-    m.color = rgba(rgb, alpha)
-    m.points = [pt(x, y, z) for x, y in points]
-    return m
-
-
-def arrow(frame, stamp, ns, mid, x, y, yaw, length, rgb, width=0.05, z=0.05, alpha=1.0) -> Marker:
-    """Arrow from (x, y) pointing along yaw."""
-    m = base(frame, stamp, ns, mid, Marker.ARROW)
-    m.points = [pt(x, y, z), pt(x + length * math.cos(yaw), y + length * math.sin(yaw), z)]
-    m.scale.x = width
-    m.scale.y = 2 * width
-    m.scale.z = 2 * width
-    m.color = rgba(rgb, alpha)
-    return m
-
-
-def label(frame, stamp, ns, mid, x, y, text, rgb=(1.0, 1.0, 1.0), size=0.15, z=0.4) -> Marker:
-    """Small view-facing label."""
-    m = base(frame, stamp, ns, mid, Marker.TEXT_VIEW_FACING)
-    m.pose.position = pt(x, y, z)
-    m.scale.z = size
-    m.color = rgba(rgb)
-    m.text = text
-    return m
-
-
-def arc_from_curvature(frame, stamp, ns, mid, curvature, length, rgb, segments=24) -> Marker:
-    """Path the robot would drive at a constant curvature, in the robot frame."""
-    points = []
-    for i in range(segments + 1):
-        s = length * i / segments
-        if abs(curvature) < 1e-4:
-            points.append((s, 0.0))
-        else:
-            points.append(
-                (math.sin(curvature * s) / curvature, (1.0 - math.cos(curvature * s)) / curvature)
-            )
-    return line(frame, stamp, ns, mid, points, rgb, width=0.025, alpha=0.9)
-
-
-def cone(frame, stamp, ns, mid, half_angle, radius, rgb, alpha=0.25, segments=24) -> Marker:
-    """Return a filled sector centred on +x, in the robot frame."""
-    m = base(frame, stamp, ns, mid, Marker.TRIANGLE_LIST)
-    m.scale.x = m.scale.y = m.scale.z = 1.0
-    m.color = rgba(rgb, alpha)
-    for i in range(segments):
-        a0 = -half_angle + 2 * half_angle * i / segments
-        a1 = -half_angle + 2 * half_angle * (i + 1) / segments
-        m.points += [
-            pt(0, 0, 0.01),
-            pt(radius * math.cos(a0), radius * math.sin(a0), 0.01),
-            pt(radius * math.cos(a1), radius * math.sin(a1), 0.01),
-        ]
-    return m
-
-
-def delete_all(frame: str, stamp) -> Marker:
-    """Return a marker that clears every marker of the receiving display."""
-    m = Marker()
-    m.header.frame_id = frame
-    m.header.stamp = stamp
-    m.action = Marker.DELETEALL
-    return m
diff --git a/src/robot_navigation/setup.cfg b/src/robot_navigation/setup.cfg
deleted file mode 100644
index 948cd18..0000000
--- a/src/robot_navigation/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/robot_navigation
-[install]
-install_scripts=$base/lib/robot_navigation
diff --git a/src/robot_navigation/setup.py b/src/robot_navigation/setup.py
deleted file mode 100644
index 10e70ef..0000000
--- a/src/robot_navigation/setup.py
+++ /dev/null
@@ -1,32 +0,0 @@
-from glob import glob
-import os
-
-from setuptools import find_packages, setup
-
-package_name = 'robot_navigation'
-
-setup(
-    name=package_name,
-    version='0.5.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
-        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
-    ],
-    install_requires=['setuptools'],
-    tests_require=['pytest'],
-    zip_safe=True,
-    maintainer='Agus Darmawan',
-    maintainer_email='darmawandeveloper@gmail.com',
-    description='Patrol route following, obstacle avoidance and inspection stop points',
-    license='Apache-2.0',
-    entry_points={
-        'console_scripts': [
-            'path_follower_node = robot_navigation.path_follower_node:main',
-            'obstacle_avoidance_node = robot_navigation.obstacle_avoidance_node:main',
-            'path_loader_node = robot_navigation.path_loader_node:main',
-        ],
-    },
-)
diff --git a/src/robot_navigation/test/conftest.py b/src/robot_navigation/test/conftest.py
deleted file mode 100644
index e669d95..0000000
--- a/src/robot_navigation/test/conftest.py
+++ /dev/null
@@ -1,4 +0,0 @@
-import os
-import sys
-
-sys.path.insert(0, os.path.dirname(__file__))
diff --git a/src/robot_navigation/test/sim_utils.py b/src/robot_navigation/test/sim_utils.py
deleted file mode 100644
index ba1f999..0000000
--- a/src/robot_navigation/test/sim_utils.py
+++ /dev/null
@@ -1,58 +0,0 @@
-import math
-import random
-
-from robot_navigation.utils.pose2d import Pose2D
-
-
-def noisy_route(corners, spacing=0.2, noise=0.03, seed=0):
-    """Recorded-route-like waypoints along the given corner polyline."""
-    rng = random.Random(seed)
-    route = []
-    for (ax, ay), (bx, by) in zip(corners, corners[1:]):
-        n = int(math.hypot(bx - ax, by - ay) / spacing)
-        for i in range(n):
-            t = i / n
-            route.append(
-                (
-                    ax + t * (bx - ax) + rng.gauss(0, noise),
-                    ay + t * (by - ay) + rng.gauss(0, noise),
-                    0.0,
-                    0.0,
-                )
-            )
-    route.append((corners[-1][0], corners[-1][1], 0.0, 0.0))
-    return route
-
-
-LOOP = [(0, 0), (10, 0), (10, 4), (0, 4), (0, 0)]
-OUT_AND_BACK = [(0, 0), (10, 0), (10, 0.8), (0, 0.8)]
-
-
-def step(pose, out, dt, max_yaw_rate=1.0):
-    """Move a pose by a body-frame command (forward, sideways, yaw rate) for dt."""
-    w = max(-max_yaw_rate, min(max_yaw_rate, out.angular))
-    pose.yaw += w * dt
-    c, s = math.cos(pose.yaw), math.sin(pose.yaw)
-    pose.x += (out.linear * c - out.lateral * s) * dt
-    pose.y += (out.linear * s + out.lateral * c) * dt
-
-
-def straight(x0, x1, y=0.0, spacing=0.1):
-    """Points of a straight path along x."""
-    n = int(abs(x1 - x0) / spacing)
-    return [(x0 + (x1 - x0) * i / n, y) for i in range(n + 1)]
-
-
-def drive(controller, path_xy, speed=0.4, dt=0.05, max_time=200.0, start=Pose2D(0.0, 0.1, 0.0)):
-    """Kinematic simulation (unicycle unless the controller sets lateral); (finished, errors)."""
-    pose = Pose2D(start.x, start.y, start.yaw)
-    controller.set_path(path_xy)
-    errors, t = [], 0.0
-    while not controller.is_finished(pose) and t < max_time:
-        out = controller.update(pose, dt, speed)
-        step(pose, out, dt)
-        i = controller.progress_index()
-        near = path_xy[max(0, i - 30):i + 30]
-        errors.append(min(math.hypot(px - pose.x, py - pose.y) for px, py in near))
-        t += dt
-    return controller.is_finished(pose), errors
diff --git a/src/robot_navigation/test/test_avoidance.py b/src/robot_navigation/test/test_avoidance.py
deleted file mode 100644
index 6fa0401..0000000
--- a/src/robot_navigation/test/test_avoidance.py
+++ /dev/null
@@ -1,47 +0,0 @@
-import math
-
-import pytest
-from robot_navigation.utils.avoidance.registry import AVOIDANCE_ALGORITHMS, create_avoidance
-
-N, ANGLE_MIN = 32, -math.pi / 2
-INC = math.pi / N
-
-
-def scan(fn):
-    """Sector ranges from a function of the sector angle in degrees."""
-    return [fn(math.degrees(ANGLE_MIN + (i + 0.5) * INC)) for i in range(N)]
-
-
-def make(name):
-    """Avoidance instance with smoothing disabled."""
-    return create_avoidance(
-        name,
-        {
-            'safe_distance': 1.5,
-            'emergency_distance': 0.75,
-            'emergency_cone_deg': 60.0,
-            'smoothing': 0.0,
-        },
-    )
-
-
-@pytest.mark.parametrize('name', sorted(AVOIDANCE_ALGORITHMS))
-def test_clear_path_is_untouched(name):
-    """No obstacle: no steering, no slowdown, no emergency."""
-    r = make(name).update(scan(lambda a: 8.0), ANGLE_MIN, INC, 8.0)
-    assert (
-        abs(r.steering_bias) < 1e-6 and r.velocity_scale == pytest.approx(1.0) and not r.emergency
-    )
-
-
-@pytest.mark.parametrize('name', sorted(AVOIDANCE_ALGORITHMS))
-def test_wall_ahead_triggers_emergency(name):
-    """Anything inside the emergency distance in front must stop the robot."""
-    assert make(name).update(scan(lambda a: 0.6), ANGLE_MIN, INC, 8.0).emergency
-
-
-@pytest.mark.parametrize('name', ['vfh', 'potential_field', 'follow_gap'])
-def test_obstacle_left_steers_right(name):
-    """An obstacle slightly left of the heading steers the robot to the right."""
-    r = make(name).update(scan(lambda a: 1.0 if 0 < a < 20 else 8.0), ANGLE_MIN, INC, 8.0)
-    assert r.steering_bias < 0.0
diff --git a/src/robot_navigation/test/test_controllers.py b/src/robot_navigation/test/test_controllers.py
deleted file mode 100644
index fe21133..0000000
--- a/src/robot_navigation/test/test_controllers.py
+++ /dev/null
@@ -1,42 +0,0 @@
-import numpy as np
-import pytest
-from robot_navigation.utils.controllers.registry import CONTROLLERS, create_controller
-from robot_navigation.utils.path_processing import process_route
-from sim_utils import drive, LOOP, noisy_route, OUT_AND_BACK
-
-PARAMS = {
-    'holonomic': {'lookahead_distance': 0.8},
-    'pure_pursuit': {'lookahead_distance': 1.0},
-    'pid': {'lookahead_distance': 1.0},
-    'mppi': {'max_angular_velocity': 1.0, 'temperature': 0.05},
-    'lqr': {'max_angular_velocity': 1.0},
-    'stanley': {'max_angular_velocity': 1.0},
-}
-
-
-def smoothed(corners):
-    """Smoothed version of a noisy recorded route as x,y points."""
-    route, _ = process_route(noisy_route(corners), 0.1, 0.05, 0.4)
-    return [(w[0], w[1]) for w in route]
-
-
-@pytest.mark.parametrize('name', sorted(CONTROLLERS))
-@pytest.mark.parametrize('corners', [LOOP, OUT_AND_BACK], ids=['loop', 'out_and_back'])
-def test_controller_completes_route(name, corners):
-    """Every controller finishes closed loops and overlapping corridors close to the path."""
-    np.random.seed(1)
-    controller = create_controller(name, dict(goal_tolerance=0.3, **PARAMS[name]))
-    finished, errors = drive(controller, smoothed(corners))
-    assert finished
-    assert np.mean(errors) < 0.1
-    assert max(errors) < 0.5
-
-
-def test_closed_loop_not_finished_at_start():
-    """A loop starts and ends at the same point but must not be finished at the start."""
-    controller = create_controller('pure_pursuit', {'lookahead_distance': 1.0})
-    path = smoothed(LOOP)
-    controller.set_path(path)
-    from robot_navigation.utils.pose2d import Pose2D
-
-    assert not controller.is_finished(Pose2D(0.0, 0.1, 0.0))
diff --git a/src/robot_navigation/test/test_copyright.py b/src/robot_navigation/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/src/robot_navigation/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/src/robot_navigation/test/test_final_approach.py b/src/robot_navigation/test/test_final_approach.py
deleted file mode 100644
index f11c425..0000000
--- a/src/robot_navigation/test/test_final_approach.py
+++ /dev/null
@@ -1,22 +0,0 @@
-import math
-
-import pytest
-from robot_navigation.utils.final_approach import FinalApproach
-from robot_navigation.utils.pose2d import Pose2D
-
-
-@pytest.mark.parametrize('holonomic', [True, False])
-def test_final_approach_converges(holonomic):
-    """The robot ends within 5 cm and 3 deg of the target from an offset start."""
-    approach = FinalApproach(holonomic=holonomic)
-    pose, dt, cmd = Pose2D(-0.4, 0.15, math.radians(20)), 0.05, None
-    for _ in range(600):
-        cmd = approach.update(pose, 0.0, 0.0, math.radians(90))
-        if cmd.done:
-            break
-        pose.yaw += cmd.wz * dt
-        pose.x += (cmd.vx * math.cos(pose.yaw) - cmd.vy * math.sin(pose.yaw)) * dt
-        pose.y += (cmd.vx * math.sin(pose.yaw) + cmd.vy * math.cos(pose.yaw)) * dt
-    assert cmd.done
-    assert math.hypot(pose.x, pose.y) < 0.05
-    assert abs(math.degrees(cmd.yaw_error)) < 3.0
diff --git a/src/robot_navigation/test/test_flake8.py b/src/robot_navigation/test/test_flake8.py
deleted file mode 100644
index d0d8a70..0000000
--- a/src/robot_navigation/test/test_flake8.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors)
diff --git a/src/robot_navigation/test/test_follower_node.py b/src/robot_navigation/test/test_follower_node.py
deleted file mode 100644
index d0db4d2..0000000
--- a/src/robot_navigation/test/test_follower_node.py
+++ /dev/null
@@ -1,118 +0,0 @@
-"""In-process tests of path_follower_node behaviour that needs a real rclpy node."""
-
-import pytest
-
-rclpy = pytest.importorskip('rclpy')
-BatteryState = pytest.importorskip('sensor_msgs.msg').BatteryState
-Trigger = pytest.importorskip('std_srvs.srv').Trigger
-PathFollowerNode = pytest.importorskip(
-    'robot_navigation.path_follower_node'
-).PathFollowerNode
-
-
-@pytest.fixture
-def follower():
-    """Provide a path follower node."""
-    rclpy.init()
-    node = PathFollowerNode()
-    yield node
-    node.destroy_node()
-    rclpy.shutdown()
-
-
-def test_status_changes_between_info_and_warn_states(follower):
-    """FOLLOWING -> BLOCKED -> FOLLOWING must not raise (rclpy severity per call site)."""
-    stamp = follower.get_clock().now().to_msg()
-    for state in ('FOLLOWING', 'BLOCKED', 'FOLLOWING', 'OFF_PATH', 'PAUSED', 'EMERGENCY_STOP'):
-        follower.publish_status(stamp, state, 'test')
-
-
-def test_low_battery_flag_clears_after_charging(follower):
-    """The low-battery flag sets below the threshold and clears 5 % above it."""
-    def battery(pct):
-        msg = BatteryState()
-        msg.percentage = pct
-        follower.battery_callback(msg)
-        return follower._battery_low
-
-    assert not battery(0.5)
-    assert battery(0.15)
-    assert battery(0.22)  # within the hysteresis band
-    assert battery(float('nan'))  # unknown readings change nothing
-    assert not battery(0.3)
-
-
-def test_stop_reports_inspection_when_every_inspector_is_down(follower):
-    """With no inspection service up, 'inspected' still goes out (saying so) at the stop."""
-    events = []
-    follower.publish_stop_event = lambda event, message='', index=None: events.append(
-        (event, message, index)
-    )
-    follower._inspection_clients = [follower.create_client(Trigger, 'no_such_inspector')]
-    follower._next_stop_index = 0
-    follower.start_inspection()
-    assert len(events) == 1
-    event, message, index = events[0]
-    assert (event, index) == ('inspected', 0)
-    assert 'no_such_inspector: not available' in message
-    assert follower._inspection_pending == 0
-
-
-def straight_route(follower, stop_at=30):
-    """Load a 6 m straight route along x with one stop point."""
-    follower._waypoints = [
-        (0.1 * i, 0.0, 0.0, 5.0 if i == stop_at else 0.0) for i in range(60)
-    ]
-    follower.compute_speed_profile()
-    follower.restart_route()
-
-
-def test_start_finds_the_robot_again_after_the_route_end(follower):
-    """A finished or far-away progress is replaced by the robot's place on the route at start."""
-    from robot_navigation.utils.pose2d import Pose2D
-
-    straight_route(follower)
-    follower._controller.set_path([(w[0], w[1]) for w in follower._waypoints], 59)
-    follower._follower_state = 'GOAL_HOLD'
-    follower.current_pose = lambda: Pose2D(1.0, 0.05, 0.0)
-    located, note = follower.find_robot_on_route()
-    assert located and 'point' in note
-    assert follower._follower_state == 'FOLLOWING'
-    assert abs(follower._controller.progress_index() - 10) <= 1
-    assert follower._next_stop_index == 30
-
-    follower.current_pose = lambda: Pose2D(1.2, 0.0, 0.0)  # still on its progress: kept
-    assert follower.find_robot_on_route() == (True, '')
-
-
-def test_start_off_the_route_joins_it_or_is_refused(follower):
-    """A robot beside the route walks onto it first; one too far away does not start."""
-    from robot_navigation.utils.pose2d import Pose2D
-
-    straight_route(follower)
-    follower._controller.set_path([(w[0], w[1]) for w in follower._waypoints], 0)
-    follower.current_pose = lambda: Pose2D(3.0, 2.0, 0.0)
-    located, note = follower.find_robot_on_route()
-    assert located and 'joining' in note
-    assert follower._follower_state == 'JOINING'
-    assert abs(follower._join_index - 30) <= 1
-
-    follower.current_pose = lambda: Pose2D(3.0, 8.0, 0.0)
-    located, note = follower.find_robot_on_route()
-    assert not located and 'from the route' in note
-
-
-def test_loading_the_same_route_again_starts_it_over(follower):
-    """A redelivered route keeps the progress, the same file loaded again restarts it."""
-    from robot_interfaces.msg import Waypoint, WaypointPath
-
-    msg = WaypointPath()
-    msg.header.stamp.sec = 10
-    msg.waypoints = [Waypoint(x=0.1 * i, y=0.0, yaw=0.0, dwell_sec=0.0) for i in range(60)]
-    follower.waypoints_callback(msg)
-    follower._follower_state = 'GOAL_HOLD'
-    follower.waypoints_callback(msg)
-    assert follower._follower_state == 'GOAL_HOLD'
-    msg.header.stamp.sec = 20
-    follower.waypoints_callback(msg)
-    assert follower._follower_state == 'FOLLOWING'
diff --git a/src/robot_navigation/test/test_holonomic.py b/src/robot_navigation/test/test_holonomic.py
deleted file mode 100644
index 28243dc..0000000
--- a/src/robot_navigation/test/test_holonomic.py
+++ /dev/null
@@ -1,154 +0,0 @@
-"""Holonomic path following and path-shift avoidance, on kinematic simulations."""
-
-import math
-
-import numpy as np
-import pytest
-from robot_navigation.utils.controllers.holonomic_controller import HolonomicController
-from robot_navigation.utils.path_progress import PathProgress
-from robot_navigation.utils.path_shift import PathShift, PathShiftConfig
-from robot_navigation.utils.pose2d import Pose2D
-from sim_utils import drive, LOOP, noisy_route, step, straight
-
-
-def run(controller, pose, seconds, speed=0.4, dt=0.05):
-    """Drive for a while; return the poses and commands."""
-    poses, commands = [], []
-    for _ in range(int(seconds / dt)):
-        out = controller.update(pose, dt, speed)
-        step(pose, out, dt)
-        poses.append(Pose2D(pose.x, pose.y, pose.yaw))
-        commands.append(out)
-    return poses, commands
-
-
-def test_closes_cross_track_sideways_without_turning():
-    """0.5 m beside the route: slides back without overshoot, the heading stays on the route."""
-    controller = HolonomicController()
-    controller.set_path(straight(0.0, 20.0))
-    poses, commands = run(controller, Pose2D(0.0, 0.5, 0.0), 12.0)
-    ys = [p.y for p in poses]
-    assert abs(ys[-1]) < 0.02
-    assert min(ys) > -0.01                                   # no overshoot
-    assert all(b <= a + 1e-9 for a, b in zip(ys, ys[1:]))     # monotonic
-    assert max(abs(p.yaw) for p in poses) < 0.02             # no weaving
-    assert any(c.lateral < -0.05 for c in commands)          # it strafed
-
-
-def test_backs_up_and_turns_when_the_route_is_behind():
-    """Facing away from the route direction: reverse slowly while turning around, then follow."""
-    controller = HolonomicController(max_reverse_velocity=0.15)
-    controller.set_path(straight(5.0, -5.0))
-    first = controller.update(Pose2D(0.0, 0.0, 0.0), 0.05, 0.4)
-    assert -0.15 - 1e-9 <= first.linear < 0.0
-    assert abs(first.angular) > 0.5
-    finished, errors = drive(controller, straight(5.0, -5.0), start=Pose2D(0.0, 0.0, 0.0))
-    assert finished and max(errors) < 0.2
-
-
-def test_sideways_departure_after_a_stop():
-    """Facing 90 deg off the route (as after a stop): moves along the route sideways, turns."""
-    controller = HolonomicController(max_lateral_velocity=0.25)
-    controller.set_path(straight(0.0, 10.0))
-    out = controller.update(Pose2D(0.0, 0.0, math.pi / 2), 0.05, 0.4)
-    assert -0.25 - 1e-9 <= out.lateral < 0.0   # body right = +x
-    assert out.angular < -0.5
-    poses, commands = run(controller, Pose2D(0.0, 0.0, math.pi / 2), 8.0)
-    assert abs(poses[-1].yaw) < 0.05 and abs(poses[-1].y) < 0.05
-    assert all(abs(c.lateral) <= 0.25 + 1e-9 for c in commands)
-
-
-def test_follows_a_parallel_line_with_an_offset():
-    """set_offset() moves the robot onto a line beside the route, heading unchanged."""
-    controller = HolonomicController()
-    controller.set_path(straight(0.0, 20.0))
-    controller.set_offset(0.5)
-    poses, _ = run(controller, Pose2D(0.0, 0.0, 0.0), 12.0)
-    assert abs(poses[-1].y - 0.5) < 0.02
-    assert max(abs(p.yaw) for p in poses) < 0.02
-
-
-def test_robot_that_cannot_strafe_still_completes_the_loop():
-    """max_lateral_velocity 0: turn toward the carrot and drive forward, never sideways."""
-    route = [(w[0], w[1]) for w in noisy_route(LOOP)]
-    controller = HolonomicController(max_lateral_velocity=0.0)
-    controller.set_path(route)
-    finished, errors = drive(controller, route)
-    assert finished and np.mean(errors) < 0.15
-
-
-def shift(points, current=0.0, s_robot=0.0, **cfg):
-    """Choose an offset on a straight route along x for obstacle points (map frame)."""
-    progress = PathProgress()
-    progress.set_path(straight(-5.0, 15.0))
-    ps = PathShift(PathShiftConfig(**cfg))
-    s_pts, d_pts = ps.path_coordinates(
-        progress.path, progress.arc_lengths, s_robot + 5.0, np.asarray(points, dtype=float)
-    )
-    return ps, ps.choose(s_pts, d_pts, s_robot + 5.0, current)
-
-
-def person(x, y, r=0.25, n=12):
-    """Points around the outline of someone standing at x, y (a circle of radius r)."""
-    return [(x + r * math.cos(a), y + r * math.sin(a)) for a in np.linspace(0, 2 * math.pi, n)]
-
-
-def test_clear_route_keeps_the_offset_at_zero():
-    """Nothing near the route: offset 0, nothing limits the speed."""
-    _, result = shift([(3.0, 2.5), (6.0, -2.0)])
-    assert result.clear and result.offset == 0.0 and math.isinf(result.free_distance)
-
-
-def test_person_on_the_route_is_passed_with_clearance():
-    """A person on the route: smallest offset that keeps body + clearance off them."""
-    _, result = shift(person(2.0, 0.0))
-    assert result.clear
-    assert 0.25 + 0.28 + 0.10 <= abs(result.offset) <= 0.8 + 1e-9
-
-
-def test_keeps_its_side_and_returns_only_when_the_body_is_past():
-    """No flip-flop between sides; back on the route once the obstacle is behind the body."""
-    _, keep = shift(person(2.0, 0.0), current=0.7)
-    assert keep.offset == pytest.approx(0.7)
-    _, beside = shift(person(0.0, 0.0), current=0.7)        # obstacle next to the robot
-    assert beside.offset == pytest.approx(0.7)
-    _, behind = shift(person(-1.2, 0.0), current=0.7)       # past it
-    assert behind.offset == 0.0
-
-
-def test_no_gap_within_max_offset_slows_to_a_stop():
-    """A wall of people across the corridor: no clear offset, speed limited to stop short."""
-    wall = [p for y in np.arange(-1.4, 1.5, 0.4) for p in person(2.0, y)]
-    ps, result = shift(wall)
-    assert not result.clear
-    assert 0.0 < ps.speed_limit(result.free_distance) < 1.2
-    _, close = shift(wall, s_robot=1.2)                     # front edge 0.4 m from the wall
-    assert ps.speed_limit(close.free_distance) == 0.0
-
-
-def test_wall_behind_the_route_start_is_not_beside_the_robot():
-    """At the start of a route, a wall behind it must not count as an obstacle beside the body."""
-    progress = PathProgress()
-    progress.set_path(straight(0.0, 10.0))
-    ps = PathShift()
-    wall = np.array([(-1.5, y) for y in np.arange(-1.5, 1.51, 0.1)])
-    s_pts, d_pts = ps.path_coordinates(progress.path, progress.arc_lengths, 0.0, wall)
-    result = ps.choose(s_pts, d_pts, 0.0, 0.0)
-    assert result.clear and result.offset == 0.0
-
-
-def test_obstacles_past_the_route_end_are_ignored():
-    """Near the end of the route nothing beyond its last point is in the way."""
-    progress = PathProgress()
-    progress.set_path(straight(0.0, 10.0))
-    ps = PathShift()
-    beyond = np.array(person(11.0, 0.0))
-    s_pts, d_pts = ps.path_coordinates(progress.path, progress.arc_lengths, 9.0, beyond)
-    assert ps.choose(s_pts, d_pts, 9.0, 0.0).offset == 0.0
-
-
-def test_offset_never_exceeds_max_offset():
-    """A wide obstacle needing more than max_offset is not passed by wandering off."""
-    box = [(2.0, y) for y in np.arange(-0.7, 0.71, 0.05)]
-    _, result = shift(box, max_offset=0.8)
-    assert not result.clear
diff --git a/src/robot_navigation/test/test_node_members.py b/src/robot_navigation/test/test_node_members.py
deleted file mode 100644
index 95a734e..0000000
--- a/src/robot_navigation/test/test_node_members.py
+++ /dev/null
@@ -1,105 +0,0 @@
-"""
-Node subclasses must not shadow rclpy.node.Node members.
-
-rclpy keeps its own state in attributes such as ``handle``, ``context`` or ``_clock``. A subclass
-that defines a method or attribute with one of those names breaks the node at runtime, e.g.
-``AttributeError: __enter__`` (a method called ``handle``) or
-``'function' object has no attribute 'handle'`` (a method called ``context``).
-"""
-
-import ast
-from pathlib import Path
-
-# Set by rclpy.node.Node.__init__ (Humble and Jazzy), not visible through dir(Node).
-NODE_INTERNALS = {
-    '_allow_undeclared_parameters',
-    '_clock',
-    '_context',
-    '_default_callback_group',
-    '_descriptors',
-    '_logger',
-    '_parameter_event_publisher',
-    '_parameter_overrides',
-    '_parameter_service',
-    '_parameters',
-    '_rate_group',
-    '_time_source',
-}
-# Public members, kept here so the test also works without ROS installed.
-NODE_PUBLIC = {
-    'clients',
-    'context',
-    'default_callback_group',
-    'executor',
-    'guards',
-    'handle',
-    'publishers',
-    'services',
-    'subscriptions',
-    'timers',
-    'waitables',
-}
-# Members a subclass may override on purpose (always calling super()).
-ALLOWED = {'destroy_node'}
-
-
-def node_members() -> set:
-    """Return every name a Node subclass must not redefine."""
-    names = NODE_PUBLIC | NODE_INTERNALS
-    try:
-        from rclpy.node import Node
-    except ImportError:
-        return names - ALLOWED
-    return (names | {n for n in dir(Node) if not n.startswith('__')}) - ALLOWED
-
-
-def is_node_class(cls: ast.ClassDef) -> bool:
-    """Return True for `class X(Node)` and `class X(rclpy.node.Node)`."""
-    for base in cls.bases:
-        name = base.attr if isinstance(base, ast.Attribute) else getattr(base, 'id', '')
-        if name == 'Node':
-            return True
-    return False
-
-
-def defined_names(cls: ast.ClassDef):
-    """Yield (name, line) for methods and self.<name> assignments of a class."""
-    for item in cls.body:
-        if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef)):
-            yield item.name, item.lineno
-        elif isinstance(item, ast.Assign):
-            for target in item.targets:
-                if isinstance(target, ast.Name):
-                    yield target.id, item.lineno
-    for node in ast.walk(cls):
-        targets = []
-        if isinstance(node, ast.Assign):
-            targets = node.targets
-        elif isinstance(node, (ast.AnnAssign, ast.AugAssign)):
-            targets = [node.target]
-        for target in targets:
-            for t in ast.walk(target):
-                if (
-                    isinstance(t, ast.Attribute)
-                    and isinstance(t.value, ast.Name)
-                    and t.value.id == 'self'
-                ):
-                    yield t.attr, node.lineno
-
-
-def test_no_shadowed_node_members():
-    """Every Node subclass in this package keeps rclpy's attributes intact."""
-    forbidden = node_members()
-    package = Path(__file__).resolve().parents[1]
-    problems = []
-    for path in sorted(package.glob('*/**/*.py')):
-        if 'test' in path.relative_to(package).parts[:1]:
-            continue
-        tree = ast.parse(path.read_text(), filename=str(path))
-        for cls in (n for n in ast.walk(tree) if isinstance(n, ast.ClassDef)):
-            if not is_node_class(cls):
-                continue
-            for name, line in defined_names(cls):
-                if name in forbidden:
-                    problems.append(f'{path.name}:{line} {cls.name}.{name} shadows rclpy Node')
-    assert not problems, '\n'.join(sorted(set(problems)))
diff --git a/src/robot_navigation/test/test_path_processing.py b/src/robot_navigation/test/test_path_processing.py
deleted file mode 100644
index 86f2540..0000000
--- a/src/robot_navigation/test/test_path_processing.py
+++ /dev/null
@@ -1,68 +0,0 @@
-import math
-
-from robot_navigation.utils.path_processing import (
-    curvatures,
-    parse_route_rows,
-    process_route,
-    resample,
-    velocity_profile,
-)
-from sim_utils import LOOP, noisy_route
-
-
-def test_resample_uniform_spacing():
-    """Resampled points are evenly spaced and keep both endpoints."""
-    pts = resample([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)], 0.1)
-    gaps = [math.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(pts, pts[1:])]
-    assert pts[0] == (0.0, 0.0) and pts[-1] == (1.0, 1.0)
-    assert max(gaps) <= 0.1 + 1e-9
-
-
-def test_smoothing_keeps_stop_points_and_reduces_jitter():
-    """Stop points stay exactly where they were recorded and the straight gets straighter."""
-    raw = noisy_route([(0, 0), (12, 0)], noise=0.03)
-    raw[30] = (raw[30][0], raw[30][1], 90.0, 5.0)
-    route, deviation = process_route(raw, 0.1, 0.05, 0.4)
-    assert raw[30] in route
-    assert deviation < 0.15
-    raw_k = max(curvatures([(w[0], w[1]) for w in raw], 1))
-    smooth_k = max(curvatures([(w[0], w[1]) for w in route]))
-    assert smooth_k < raw_k / 5.0
-
-
-def test_velocity_profile_brakes_into_stops():
-    """Speed is capped at stop points and ramps down before them."""
-    route, _ = process_route(noisy_route(LOOP), 0.1, 0.05, 0.4)
-    pts = [(w[0], w[1]) for w in route]
-    v = velocity_profile(
-        pts, [50], cruise=0.4, min_speed=0.1, max_lateral_accel=0.3, decel=0.3, approach_speed=0.15
-    )
-    assert v[50] <= 0.15 + 1e-9
-    assert v[48] < v[45] <= 0.4
-    assert v[-1] <= 0.15 + 1e-9
-    assert max(v) <= 0.4 + 1e-9
-
-
-def test_route_rows_drop_what_cannot_be_driven():
-    """Comments and blanks are skipped; NaN, inf and text rows are dropped, negative dwell is 0."""
-    rows = [
-        ['# x', 'y', 'yaw_deg', 'dwell_sec'],
-        [],
-        ['0.0', '0.0'],
-        ['1.0', '0.0', '90'],
-        [' 2.0', '0.0', '0', '5.0'],
-        ['nan', '0.0'],
-        ['3.0', 'inf'],
-        ['x', 'y'],
-        ['4.0'],
-        ['5.0', '0.0', '0', '-2'],
-    ]
-    waypoints, problems = parse_route_rows(rows)
-    assert waypoints == [
-        (0.0, 0.0, 0.0, 0.0),
-        (1.0, 0.0, 90.0, 0.0),
-        (2.0, 0.0, 0.0, 5.0),
-        (5.0, 0.0, 0.0, 0.0),
-    ]
-    lines = [p.split(':')[0] for p in problems]
-    assert lines == ['line 6', 'line 7', 'line 8', 'line 9', 'line 10']
diff --git a/src/robot_navigation/test/test_pep257.py b/src/robot_navigation/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/src/robot_navigation/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/robot_odometry/README.md b/src/robot_odometry/README.md
deleted file mode 100644
index 24fc508..0000000
--- a/src/robot_odometry/README.md
+++ /dev/null
@@ -1,77 +0,0 @@
-# robot_odometry
-
-LiDAR-inertial odometry on the Livox Mid-360 and teach-and-repeat route recording.
-
-The odometry publishes `camera_init → body` and `/Odometry`. It starts at the robot's pose on every
-launch and drifts slowly. `map → odom` is set by [robot_localization](../robot_localization).
-
-`odometry.launch.py` starts four things.
-
-**The estimator, chosen with `method:=`** (default `$ODOMETRY` or `fast_lio`):
-
-| Method | Source | Notes |
-|---|---|---|
-| `fast_lio` | `src/xtras/slam/fast_lio2` | [config/fast_lio.yaml](config/fast_lio.yaml) is tuned indoors: 0.3 m voxels, 4 iterations, 50 m range, no ever-growing `/path` |
-| `leg_kilo` | `src/xtras/slam/leg_kilo` (optional) | [config/leg_kilo.yaml](config/leg_kilo.yaml): Gaussian voxel map (plane + NDT), two-stage ESKF built for legged-robot dynamics, headless |
-| `external` | started elsewhere | no estimator, the monitor and recorder still run; used by [robot_sim](../robot_sim), which publishes `/Odometry` itself |
-
-**`wait_still`**, the launch prefix of the estimator. Both estimators estimate gravity and the IMU
-biases from their first IMU samples and assume the robot is not moving. `wait_still` holds the
-start until the IMU has been still and level for `still_sec`. It runs again on every respawn.
-
-**`odometry_monitor_node`**, which publishes `odometry/status`. It checks for:
-
-- stale `/Odometry`
-- jumps
-- z drift on a flat floor, which points to a tilted start
-- LIO speed that disagrees with the robot's own `/<robot_ns>/odom`, which points to a degenerate corridor
-  or slip
-- restarts, published on `odometry/restarted`; `localization_node` then drops `map → odom`
-
-**`route_recorder_node`** (`record:=true`) for teach. It samples `map → base_link` and writes
-`map/<date>/<time>_path.csv` (`x,y,yaw_deg,dwell_sec`).
-
-With `localization:=true tags_config_file:=…`, the launch also starts `localization_node`
-(development, `script/start_tmux.sh`).
-
-| Interface | Type | |
-|---|---|---|
-| `/Odometry` | `nav_msgs/Odometry` | estimator, `camera_init → body` |
-| `/cloud_registered_body` | `sensor_msgs/PointCloud2` | deskewed scan in `body`, feeds lidar_sector_node |
-| `odometry/status` | `std_msgs/String`, latched | `initializing`, `ok`, `degraded: …`, `lost: …` |
-| `odometry/restarted` | `std_msgs/Empty` | odom frame reset |
-| `route/record` | `std_srvs/SetBool` | start / stop recording |
-| `route/mark_stop_point` | `robot_interfaces/MarkStopPoint` | stop point at the current pose |
-| `route/saved` | `std_msgs/String` | path of the CSV just written (uploaded to NETRA) |
-
-```bash
-ros2 launch robot_odometry odometry.launch.py                          # FAST-LIO, patrol
-ros2 launch robot_odometry odometry.launch.py method:=leg_kilo save_map:=true
-ros2 launch robot_odometry odometry.launch.py method:=leg_kilo leg_kilo_viewer:=true  # with a display
-ros2 topic echo /odometry/status
-```
-
-## Leg-KILO
-
-Leg-KILO is a submodule from the DarmawanRobotics fork, branch `pashupati`, which adds options for
-running on the robot:
-
-- `--viewer=false`
-- `odom_frame` / `body_frame`
-- `publish_path: false`
-- `backend_enable`
-- the `/cloud_registered_body` output
-- `COLCON_IGNORE` on its bundled copy of `livox_ros_driver2`
-
-Build dependencies come from `script/setup.sh install legkilo`. Without the submodule, the rest of
-the workspace builds as before.
-
-Leg-KILO runs in **LIO** mode here. Its `KILO` mode adds leg kinematics, but it needs per-foot
-contact forces in a Unitree `HighState`, which most robot drivers do not publish. On a
-wheel-legged robot, the wheels also break its static-foot assumption.
-
-In teach (`save_map:=true`) its backend runs: submaps, loop closure, and recording under
-`leg_kilo/legkilo/result/temp/`. On patrol the backend is off.
-
-Gate and monitor thresholds and recorder settings:
-[config/odometry_params.yaml](config/odometry_params.yaml).
diff --git a/src/robot_odometry/launch/odometry.launch.py b/src/robot_odometry/launch/odometry.launch.py
deleted file mode 100644
index 4404c9f..0000000
--- a/src/robot_odometry/launch/odometry.launch.py
+++ /dev/null
@@ -1,164 +0,0 @@
-#!/usr/bin/env python3
-import os
-import re
-import tempfile
-
-from ament_index_python.packages import get_package_prefix, get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-METHODS = ('fast_lio', 'leg_kilo', 'external')  # external: simulator or another estimator
-
-
-def truthy(value: str) -> bool:
-    """Interpret a launch argument as a bool."""
-    return value.strip().lower() in ('1', 'true', 'yes', 'on')
-
-
-def leg_kilo_config(path: str, backend: bool) -> str:
-    """Copy the Leg-KILO yaml with backend_enable set (it has no parameter overrides)."""
-    with open(path) as f:
-        text = f.read()
-    line = f'backend_enable: {"true" if backend else "false"}'
-    text, n = re.subn(r'(?m)^backend_enable:.*$', line, text)
-    if n == 0:
-        text += f'\n{line}\n'
-    fd, out = tempfile.mkstemp(prefix='pashupati_leg_kilo_', suffix='.yaml')
-    with os.fdopen(fd, 'w') as f:
-        f.write(text)
-    return out
-
-
-def launch_setup(context):
-    """Build the odometry nodes for the selected method."""
-    arg = lambda name: LaunchConfiguration(name).perform(context)  # noqa: E731
-    method = arg('method')
-    if method not in METHODS:
-        raise RuntimeError(f'method:={method!r}, choose one of {METHODS}')
-    save_map = truthy(arg('save_map'))
-    respawn = truthy(arg('respawn'))
-    params = arg('params_file')
-    site = [arg('site_config')] if os.path.isfile(arg('site_config')) else []
-    lib = os.path.join(get_package_prefix('robot_odometry'), 'lib', 'robot_odometry')
-    gate = os.path.join(lib, 'wait_still')
-    # FAST-LIO and Leg-KILO both estimate gravity/biases at start: hold them until still.
-    gate_params = ''.join(f'--params-file {p} ' for p in (params, *site))
-    prefix = f'{gate} {gate_params}--' if truthy(arg('wait_still')) else None
-    common = {'output': 'screen', 'respawn': respawn, 'respawn_delay': 3.0}
-
-    estimators = []  # external: /Odometry and camera_init -> body come from elsewhere (robot_sim)
-    if method == 'fast_lio':
-        estimators.append(
-            Node(
-                package='fast_lio',
-                executable='fastlio_mapping',
-                name='fastlio_mapping_node',
-                parameters=[
-                    arg('fast_lio_params_file'),
-                    *site,
-                    {'pcd_save.pcd_save_en': save_map},
-                ],
-                prefix=prefix,
-                **common,
-            )
-        )
-    elif method == 'leg_kilo':
-        config = leg_kilo_config(arg('leg_kilo_config_file'), backend=save_map)
-        viewer = 'true' if truthy(arg('leg_kilo_viewer')) else 'false'
-        estimators.append(
-            Node(
-                package='legkilo',
-                executable='legkilo_node',
-                arguments=['--config_file', config, f'--viewer={viewer}'],
-                prefix=prefix,
-                **common,
-            )
-        )
-
-    actions = [
-        *estimators,
-        Node(
-            package='robot_odometry',
-            executable='odometry_monitor_node',
-            name='odometry_monitor_node',
-            parameters=[params, *site, {'leg_odom_topic': arg('leg_odom_topic')}],
-            **common,
-        ),
-    ]
-    if truthy(arg('record')):
-        actions.append(
-            Node(
-                package='robot_odometry',
-                executable='route_recorder_node',
-                name='route_recorder_node',
-                parameters=[params, *site],
-                **common,
-            )
-        )
-    if truthy(arg('localization')):
-        actions.append(
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    os.path.join(
-                        get_package_share_directory('robot_localization'),
-                        'launch',
-                        'localization.launch.py',
-                    )
-                ),
-                launch_arguments={'tags_config_file': arg('tags_config_file')}.items(),
-            )
-        )
-    return actions
-
-
-def generate_launch_description():
-    """Launch LiDAR-inertial odometry, its monitor and the route recorder."""
-    share = get_package_share_directory('robot_odometry')
-    config = lambda name: os.path.join(share, 'config', name)  # noqa: E731
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                'method',
-                default_value=os.environ.get('ODOMETRY', 'fast_lio'),
-                description='fast_lio | leg_kilo | external (default: $ODOMETRY or fast_lio)',
-            ),
-            DeclareLaunchArgument(
-                'save_map',
-                default_value='false',
-                description='Teach: keep the map (FAST-LIO PCD / Leg-KILO backend). RAM grows.',
-            ),
-            DeclareLaunchArgument(
-                'wait_still',
-                default_value='true',
-                description='Hold the estimator (also on respawn) until the IMU is still.',
-            ),
-            DeclareLaunchArgument('respawn', default_value='false'),
-            DeclareLaunchArgument('record', default_value='true', description='route recorder'),
-            DeclareLaunchArgument(
-                'localization',
-                default_value='false',
-                description='Also start localization_node (map->odom, frame bridge).',
-            ),
-            DeclareLaunchArgument('tags_config_file', default_value=''),
-            DeclareLaunchArgument(
-                'site_config',
-                default_value='',
-                description='Extra parameter file applied on top of the defaults (if it exists).',
-            ),
-            DeclareLaunchArgument(
-                'leg_odom_topic', default_value='/robot/odom', description='"" disables the check'
-            ),
-            DeclareLaunchArgument('params_file', default_value=config('odometry_params.yaml')),
-            DeclareLaunchArgument('fast_lio_params_file', default_value=config('fast_lio.yaml')),
-            DeclareLaunchArgument('leg_kilo_config_file', default_value=config('leg_kilo.yaml')),
-            DeclareLaunchArgument(
-                'leg_kilo_viewer',
-                default_value='false',
-                description='Open the Leg-KILO Iridescence viewer (needs a display).',
-            ),
-            OpaqueFunction(function=launch_setup),
-        ]
-    )
diff --git a/src/robot_odometry/package.xml b/src/robot_odometry/package.xml
deleted file mode 100644
index 84f40b9..0000000
--- a/src/robot_odometry/package.xml
+++ /dev/null
@@ -1,33 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>robot_odometry</name>
-  <version>0.2.0</version>
-  <description>LiDAR-inertial odometry for the Livox Mid-360 (FAST-LIO2, or Leg-KILO from src/xtras/slam/leg_kilo), a stillness gate for IMU initialisation, an odometry monitor and the teach-and-repeat route recorder.</description>
-  <maintainer email="darmawandeveloper@gmail.com">Agus Darmawan</maintainer>
-  <license>Apache-2.0</license>
-
-  <exec_depend>livox_ros_driver2</exec_depend>
-  <exec_depend>fast_lio</exec_depend>
-  <!-- legkilo is optional (method:=leg_kilo), so it is not a dependency -->
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>tf_transformations</exec_depend>
-  <exec_depend>python3-transforms3d</exec_depend>
-  <exec_depend>std_srvs</exec_depend>
-  <exec_depend>robot_interfaces</exec_depend>
-  <exec_depend>robot_localization</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>python3-numpy</exec_depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/src/robot_odometry/resource/robot_odometry b/src/robot_odometry/resource/robot_odometry
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_odometry/robot_odometry/__init__.py b/src/robot_odometry/robot_odometry/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_odometry/robot_odometry/checks.py b/src/robot_odometry/robot_odometry/checks.py
deleted file mode 100644
index 11c09d2..0000000
--- a/src/robot_odometry/robot_odometry/checks.py
+++ /dev/null
@@ -1,42 +0,0 @@
-"""Pure helpers for the stillness gate and the odometry monitor (no ROS imports, unit-tested)."""
-
-import math
-
-import numpy as np
-
-
-def stillness(gyro: np.ndarray, acc: np.ndarray) -> tuple[float, float, float]:
-    """
-    Return (max gyro norm, relative std of |acc|, tilt of mean acc from +z in degrees).
-
-    gyro and acc are Nx3 arrays. The acceleration check is relative, so it works whether the IMU
-    reports m/s^2 or g (the Livox Mid-360 reports g).
-    """
-    gyro_max = float(np.max(np.linalg.norm(gyro, axis=1)))
-    acc_norm = np.linalg.norm(acc, axis=1)
-    mean_norm = float(np.mean(acc_norm))
-    if mean_norm <= 1e-6:
-        return gyro_max, math.inf, 180.0
-    acc_rel_std = float(np.std(acc_norm)) / mean_norm
-    mean_acc = np.mean(acc, axis=0)
-    cos_tilt = float(mean_acc[2]) / float(np.linalg.norm(mean_acc))
-    tilt_deg = math.degrees(math.acos(max(-1.0, min(1.0, cos_tilt))))
-    return gyro_max, acc_rel_std, tilt_deg
-
-
-def yaw_from_quaternion(x: float, y: float, z: float, w: float) -> float:
-    """Return the yaw (rad) of a quaternion."""
-    return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
-
-
-def looks_like_restart(prev_xy_dist: float, prev_yaw: float, xy_dist: float, yaw: float,
-                       origin_radius: float, min_prev_offset: float) -> bool:
-    """
-    Return True when the pose snapped back to the origin, i.e. FAST-LIO restarted.
-
-    A fresh FAST-LIO starts at identity; a robot that was away from the origin cannot get there in
-    one frame.
-    """
-    at_origin = xy_dist < origin_radius and abs(yaw) < math.radians(1.0)
-    was_away = prev_xy_dist > min_prev_offset or abs(prev_yaw) > math.radians(5.0)
-    return at_origin and was_away
diff --git a/src/robot_odometry/robot_odometry/odometry_monitor_node.py b/src/robot_odometry/robot_odometry/odometry_monitor_node.py
deleted file mode 100644
index dedd7e3..0000000
--- a/src/robot_odometry/robot_odometry/odometry_monitor_node.py
+++ /dev/null
@@ -1,175 +0,0 @@
-#!/usr/bin/env python3
-"""Watch FAST-LIO and report when its odometry can no longer be trusted."""
-
-import math
-import time
-
-from nav_msgs.msg import Odometry
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSProfile
-from robot_odometry.checks import looks_like_restart, yaw_from_quaternion
-from std_msgs.msg import Empty, String
-
-
-class OdometryMonitorNode(Node):
-    """
-    Publish odometry/status and odometry/restarted from FAST-LIO (/Odometry) and leg odometry.
-
-    Status: initializing | ok | degraded: <why> | lost: <why>
-    """
-
-    def __init__(self):
-        """Declare parameters, subscribe and start the watchdog."""
-        super().__init__('odometry_monitor_node')
-        self.declare_parameter('odom_topic', '/Odometry')
-        self.declare_parameter('leg_odom_topic', '/robot/odom')   # empty: no cross-check
-        self.declare_parameter('stale_sec', 0.5)
-        self.declare_parameter('max_speed_mps', 3.0)
-        self.declare_parameter('max_z_drift_m', 0.5)            # 0: off (ramps, other floors)
-        self.declare_parameter('max_speed_diff_mps', 0.35)
-        self.declare_parameter('speed_diff_sec', 1.5)
-        self.declare_parameter('min_check_speed_mps', 0.1)
-        self.declare_parameter('restart_origin_radius_m', 0.05)
-        self.declare_parameter('restart_min_offset_m', 0.3)
-
-        p = self.get_parameter
-        self._stale = float(p('stale_sec').value)
-        self._max_speed = float(p('max_speed_mps').value)
-        self._max_z = float(p('max_z_drift_m').value)
-        self._max_diff = float(p('max_speed_diff_mps').value)
-        self._diff_sec = float(p('speed_diff_sec').value)
-        self._min_check = float(p('min_check_speed_mps').value)
-        self._origin_r = float(p('restart_origin_radius_m').value)
-        self._min_offset = float(p('restart_min_offset_m').value)
-
-        self._last = None            # (stamp_sec, x, y, z, yaw)
-        self._last_rx = None         # monotonic receipt time of the last /Odometry
-        self._lio_speed = 0.0        # smoothed planar speed
-        self._leg_speed = None
-        self._leg_rx = 0.0
-        self._diff_since = None
-        self._event = None           # (message, until monotonic) for one-off problems
-        self._status = None
-
-        latched = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
-        self._status_pub = self.create_publisher(String, 'odometry/status', latched)
-        self._restart_pub = self.create_publisher(Empty, 'odometry/restarted', 10)
-        self.create_subscription(Odometry, p('odom_topic').value, self.on_odometry, 20)
-        leg_topic = p('leg_odom_topic').value
-        if leg_topic:
-            self.create_subscription(Odometry, leg_topic, self.on_leg_odometry, 20)
-        self.create_timer(0.2, self.update_status)
-        self.publish_status('initializing')
-
-    def on_leg_odometry(self, msg: Odometry):
-        """Keep the planar body speed reported by the robot (wheels + legs)."""
-        v = msg.twist.twist.linear
-        self._leg_speed = math.hypot(v.x, v.y)
-        self._leg_rx = time.monotonic()
-
-    def on_odometry(self, msg: Odometry):
-        """Check every FAST-LIO pose for restarts, jumps, z drift and speed mismatch."""
-        now = time.monotonic()
-        stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
-        pos, q = msg.pose.pose.position, msg.pose.pose.orientation
-        yaw = yaw_from_quaternion(q.x, q.y, q.z, q.w)
-        current = (stamp, pos.x, pos.y, pos.z, yaw)
-        self._last_rx = now
-
-        if self._last is not None:
-            t0, x0, y0, _, yaw0 = self._last
-            dt = stamp - t0
-            if dt == 0.0:  # the same pose again (republished), nothing new to check
-                return
-            step = math.hypot(pos.x - x0, pos.y - y0)
-            restarted = dt < 0.0 or looks_like_restart(
-                math.hypot(x0, y0), yaw0, math.hypot(pos.x, pos.y), yaw,
-                self._origin_r, self._min_offset,
-            )
-            if restarted:
-                self.get_logger().error('odometry restarted: odom frame reset, map->odom invalid')
-                self._restart_pub.publish(Empty())
-                self.flag('lost: odometry restarted, relocalize', 5.0)
-                self._lio_speed, self._diff_since = 0.0, None
-            elif dt < 0.5:
-                speed = step / dt
-                if speed > self._max_speed:
-                    self.get_logger().error(f'odometry jumped {step:.2f} m in {dt:.2f} s')
-                    self.flag(f'degraded: jumped {step:.2f} m', 5.0)
-                else:
-                    self._lio_speed += 0.3 * (speed - self._lio_speed)
-                    self.check_speed_mismatch(now)
-        self._last = current
-
-    def check_speed_mismatch(self, now: float):
-        """Track how long FAST-LIO and the robot's own speed disagree."""
-        fresh = self._leg_speed is not None and now - self._leg_rx < 0.3
-        moving = fresh and max(self._lio_speed, self._leg_speed) > self._min_check
-        if moving and abs(self._lio_speed - self._leg_speed) > self._max_diff:
-            if self._diff_since is None:
-                self._diff_since = now
-        else:
-            self._diff_since = None
-
-    def flag(self, message: str, hold_sec: float):
-        """Show a one-off problem for hold_sec seconds."""
-        self._event = (message, time.monotonic() + hold_sec)
-        self.update_status()
-
-    def update_status(self):
-        """Derive the status from the latest checks; publish only when it changes."""
-        now = time.monotonic()
-        if self._last_rx is None:
-            status = 'initializing'
-        elif now - self._last_rx > self._stale:
-            status = f'lost: no odometry for {now - self._last_rx:.1f} s'
-        elif self._event is not None and now < self._event[1]:
-            status = self._event[0]
-        elif self._max_z > 0.0 and self._last is not None and abs(self._last[3]) > self._max_z:
-            status = f'degraded: z drift {self._last[3]:+.2f} m (restart standing still)'
-        elif self._diff_since is not None and now - self._diff_since > self._diff_sec:
-            status = (
-                f'degraded: LIO {self._lio_speed:.2f} m/s vs robot {self._leg_speed:.2f} m/s'
-            )
-        else:
-            status = 'ok'
-        self.publish_status(status)
-
-    def publish_status(self, status: str):
-        """Publish the status (latched) and log transitions."""
-        if status == self._status:
-            return
-        if self._status is not None:
-            # one call site per severity: rclpy refuses to change it between calls
-            if status == 'ok':
-                self.get_logger().info(f'odometry: {status}')
-            else:
-                self.get_logger().warn(f'odometry: {status}')
-        self._status = status
-        self._status_pub.publish(String(data=status))
-
-
-def main(args=None):
-    """Spin the odometry monitor."""
-    rclpy.init(args=args)
-    node = OdometryMonitorNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_odometry/robot_odometry/route_recorder_node.py b/src/robot_odometry/robot_odometry/route_recorder_node.py
deleted file mode 100644
index 03f67d5..0000000
--- a/src/robot_odometry/robot_odometry/route_recorder_node.py
+++ /dev/null
@@ -1,207 +0,0 @@
-#!/usr/bin/env python3
-import csv
-from datetime import datetime
-import math
-import os
-
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from robot_interfaces.srv import MarkStopPoint
-from std_msgs.msg import String
-from std_srvs.srv import SetBool
-from tf2_ros import Buffer, TransformListener
-import tf_transformations
-
-
-class RouteRecorderNode(Node):
-    """Teach: record the robot path from TF and save it as a route CSV with stop points."""
-
-    def __init__(self):
-        """Initialize parameters and services; TF and sampling start with a recording."""
-        super().__init__('route_recorder_node')
-        self.declare_parameter('map_frame', 'map')
-        self.declare_parameter('base_frame', 'base_link')
-        self.declare_parameter('min_waypoint_spacing_m', 0.2)
-        self.declare_parameter('record_rate_hz', 10.0)
-        self.declare_parameter('output_path', '/home/robot/dev/Pashupati/map')
-
-        self._map_frame = self.get_parameter('map_frame').value
-        self._base_frame = self.get_parameter('base_frame').value
-        self._min_spacing = float(self.get_parameter('min_waypoint_spacing_m').value)
-        self._record_period = 1.0 / float(self.get_parameter('record_rate_hz').value)
-        self._output_path = self.get_parameter('output_path').value
-
-        self._recording = False
-        self._waypoints: list[tuple[float, float, float, float]] = []
-        self._recording_file = None
-
-        self._tf_buffer = Buffer()
-        self._tf_listener = None  # only while recording, see start_sampling()
-        self._sample_timer = None
-
-        self._saved_pub = self.create_publisher(String, 'route/saved', 10)
-        self.create_service(SetBool, 'route/record', self.recording_callback)
-        self.create_service(
-            MarkStopPoint, 'route/mark_stop_point', self.mark_stop_point_callback
-        )
-
-    def recording_callback(self, request, response):
-        """Start or stop path recording."""
-        if request.data:
-            if self._recording:
-                response.success = False
-                response.message = 'already recording'
-                return response
-
-            self._waypoints = []
-            self._recording = True
-
-            timestamp = datetime.now()
-            date_dir = timestamp.strftime('%Y-%m-%d')
-            time_name = timestamp.strftime('%H-%M-%S')
-            record_dir = os.path.join(self._output_path, date_dir)
-
-            os.makedirs(record_dir, exist_ok=True)
-            self._recording_file = os.path.join(record_dir, f'{time_name}_path.csv')
-            self.start_sampling()
-
-            response.success = True
-            response.message = 'recording started'
-            self.get_logger().info(f'path recording started: {self._recording_file}')
-            return response
-
-        if not self._recording:
-            response.success = False
-            response.message = 'not recording'
-            return response
-
-        self._recording = False
-        self.stop_sampling()
-        self.write_csv()
-        self._saved_pub.publish(String(data=self._recording_file))
-
-        response.success = True
-        response.message = (
-            f'recording stopped, {len(self._waypoints)} waypoints saved to {self._recording_file}'
-        )
-        self.get_logger().info(response.message)
-        return response
-
-    def start_sampling(self):
-        """Listen to TF and sample the pose while a route is recorded."""
-        if self._tf_listener is None:
-            self._tf_listener = TransformListener(self._tf_buffer, self)
-        if self._sample_timer is None:
-            self._sample_timer = self.create_timer(self._record_period, self.record_tick)
-
-    def stop_sampling(self):
-        """Release TF and the sampling timer between recordings (nothing to do then)."""
-        if self._sample_timer is not None:
-            self.destroy_timer(self._sample_timer)
-            self._sample_timer = None
-        if self._tf_listener is not None:
-            self._tf_listener.unregister()
-            self._tf_listener = None
-
-    def lookup_current_pose(self):
-        """Look up the current map->base_link pose."""
-        try:
-            transform = self._tf_buffer.lookup_transform(
-                self._map_frame, self._base_frame, rclpy.time.Time()
-            )
-        except Exception as error:
-            self.get_logger().warning(
-                f'TF lookup {self._map_frame}->{self._base_frame} failed: {error}',
-                throttle_duration_sec=2.0,
-            )
-            return None
-
-        x = transform.transform.translation.x
-        y = transform.transform.translation.y
-        q = transform.transform.rotation
-        yaw = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
-
-        return x, y, math.degrees(yaw)
-
-    def record_tick(self):
-        """Sample and record the robot pose when it moved far enough."""
-        if not self._recording:
-            return
-
-        pose = self.lookup_current_pose()
-        if pose is None:
-            return
-
-        x, y, yaw_deg = pose
-
-        if self._waypoints:
-            last_x, last_y, _, _ = self._waypoints[-1]
-            if math.hypot(x - last_x, y - last_y) < self._min_spacing:
-                return
-
-        self._waypoints.append((x, y, yaw_deg, 0.0))
-
-    def mark_stop_point_callback(self, request, response):
-        """Append the current pose as an inspection stop point."""
-        if not self._recording:
-            response.success = False
-            response.message = 'not recording, call route/record(true) first'
-            return response
-
-        pose = self.lookup_current_pose()
-        if pose is None:
-            response.success = False
-            response.message = 'TF not available, could not sample current pose'
-            return response
-
-        x, y, yaw_deg = pose
-        self._waypoints.append((x, y, yaw_deg, request.dwell_sec))
-        self.write_csv()
-
-        response.success = True
-        response.message = (
-            f'stop point marked at ({x:.2f}, {y:.2f}), dwell {request.dwell_sec:.1f}s'
-        )
-        self.get_logger().info(response.message)
-        return response
-
-    def write_csv(self):
-        """Write recorded waypoints to the current recording CSV file."""
-        if not self._recording_file:
-            self.get_logger().error('No recording file configured.')
-            return
-
-        os.makedirs(os.path.dirname(self._recording_file), exist_ok=True)
-
-        with open(self._recording_file, 'w', newline='') as file:
-            writer = csv.writer(file)
-            writer.writerow(['# x', 'y', 'yaw_deg', 'dwell_sec'])
-
-            for x, y, yaw_deg, dwell_sec in self._waypoints:
-                writer.writerow([f'{x:.4f}', f'{y:.4f}', f'{yaw_deg:.2f}', f'{dwell_sec:.1f}'])
-
-
-def main(args=None):
-    """Initialize ROS 2 and run the route recorder node."""
-    rclpy.init(args=args)
-    node = RouteRecorderNode()
-
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_odometry/robot_odometry/wait_still.py b/src/robot_odometry/robot_odometry/wait_still.py
deleted file mode 100644
index 3bab60e..0000000
--- a/src/robot_odometry/robot_odometry/wait_still.py
+++ /dev/null
@@ -1,147 +0,0 @@
-#!/usr/bin/env python3
-"""
-Launch prefix: wait until the IMU is still and level, then exec the real command.
-
-FAST-LIO estimates gravity and the IMU biases from its first ~10 IMU samples and assumes the robot
-is not moving. Starting it while the robot is standing up or being carried bakes a tilted gravity
-into the whole run. Used as a launch prefix, so a respawned FAST-LIO waits again:
-
-    wait_still [--params-file odometry_params.yaml] -- fastlio_mapping --ros-args ...
-"""
-
-from collections import deque
-import os
-import sys
-import time
-
-import numpy as np
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.qos import qos_profile_sensor_data
-from robot_odometry.checks import stillness
-from sensor_msgs.msg import Imu
-
-
-class StillGate(Node):
-    """Collect IMU samples and decide when the robot is still enough to start odometry."""
-
-    def __init__(self):
-        """Declare parameters and subscribe to the IMU."""
-        super().__init__('odometry_gate')
-        self.declare_parameter('imu_topic', '/livox/imu')
-        self.declare_parameter('still_sec', 2.0)
-        self.declare_parameter('max_gyro_rad_s', 0.03)
-        self.declare_parameter('max_acc_rel_std', 0.01)
-        self.declare_parameter('max_tilt_deg', 8.0)
-        self.declare_parameter('timeout_sec', 60.0)
-
-        p = self.get_parameter
-        self._still_sec = float(p('still_sec').value)
-        self._max_gyro = float(p('max_gyro_rad_s').value)
-        self._max_acc_std = float(p('max_acc_rel_std').value)
-        self._max_tilt = float(p('max_tilt_deg').value)
-        self._timeout = float(p('timeout_sec').value)
-
-        self.done = False
-        self._start = time.monotonic()
-        self._last_log = 0.0
-        self._samples = deque()
-        topic = p('imu_topic').value
-        self.create_subscription(Imu, topic, self.on_imu, qos_profile_sensor_data)
-        self.create_timer(0.2, self.evaluate)
-        self.get_logger().info(
-            f'waiting for {self._still_sec:.1f} s of stillness on {topic} before starting odometry'
-        )
-
-    def on_imu(self, msg: Imu):
-        """Keep the samples of the last still_sec seconds."""
-        now = time.monotonic()
-        w, a = msg.angular_velocity, msg.linear_acceleration
-        self._samples.append((now, (w.x, w.y, w.z), (a.x, a.y, a.z)))
-        while self._samples and now - self._samples[0][0] > self._still_sec:
-            self._samples.popleft()
-
-    def evaluate(self):
-        """Release the gate when the window is still and level, or when the timeout passes."""
-        if self.done:
-            return
-        now = time.monotonic()
-        elapsed = now - self._start
-        window_full = (
-            len(self._samples) >= 20
-            and now - self._samples[0][0] >= 0.9 * self._still_sec
-        )
-        reason = 'no IMU data yet'
-        if window_full:
-            gyro = np.array([s[1] for s in self._samples])
-            acc = np.array([s[2] for s in self._samples])
-            gyro_max, acc_std, tilt = stillness(gyro, acc)
-            problems = []
-            if gyro_max > self._max_gyro:
-                problems.append(f'rotating {gyro_max:.3f} rad/s')
-            if acc_std > self._max_acc_std:
-                problems.append(f'shaking {acc_std * 100:.1f} %')
-            if self._max_tilt > 0.0 and tilt > self._max_tilt:
-                problems.append(f'tilted {tilt:.1f} deg (sitting?)')
-            if not problems:
-                self.get_logger().info(
-                    f'IMU still (gyro {gyro_max:.3f} rad/s, acc {acc_std * 100:.2f} %, '
-                    f'tilt {tilt:.1f} deg) after {elapsed:.1f} s, starting odometry'
-                )
-                self.done = True
-                return
-            reason = ', '.join(problems)
-        if self._timeout > 0.0 and elapsed > self._timeout:
-            self.get_logger().error(
-                f'not still after {self._timeout:.0f} s ({reason}), starting odometry anyway; '
-                'expect drift, restart it with the robot standing still'
-            )
-            self.done = True
-            return
-        if now - self._last_log > 5.0:
-            self._last_log = now
-            self.get_logger().warn(f'odometry start held: {reason}')
-
-
-def split_args(argv: list[str]) -> tuple[list[str], list[str]]:
-    """Split `[--params-file F]... -- command...` into (rclpy args, command)."""
-    if '--' not in argv:
-        raise SystemExit('usage: wait_still [--params-file FILE]... -- COMMAND [ARGS...]')
-    i = argv.index('--')
-    own, command = argv[:i], argv[i + 1:]
-    if not command:
-        raise SystemExit('wait_still: no command after --')
-    files = [own[k + 1] for k in range(0, len(own) - 1, 2) if own[k] == '--params-file']
-    files = [f for f in files if f]
-    if not files:
-        return [], command
-    return ['--ros-args', *[a for f in files for a in ('--params-file', f)]], command
-
-
-def main(argv=None):
-    """Wait for stillness, then replace this process with the command."""
-    ros_args, command = split_args(sys.argv[1:] if argv is None else argv)
-    # Only our own args: the command's --ros-args (node name, params) must not reach the gate.
-    rclpy.init(args=['wait_still', *ros_args])
-    node = StillGate()
-    try:
-        while rclpy.ok() and not node.done:
-            rclpy.spin_once(node, timeout_sec=0.1)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        return 130
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-    if not node.done:
-        return 1
-    sys.stdout.flush()
-    sys.stderr.flush()
-    os.execvp(command[0], command)
-
-
-if __name__ == '__main__':
-    sys.exit(main())
diff --git a/src/robot_odometry/setup.cfg b/src/robot_odometry/setup.cfg
deleted file mode 100644
index 19618d5..0000000
--- a/src/robot_odometry/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/robot_odometry
-[install]
-install_scripts=$base/lib/robot_odometry
diff --git a/src/robot_odometry/setup.py b/src/robot_odometry/setup.py
deleted file mode 100644
index 8e40735..0000000
--- a/src/robot_odometry/setup.py
+++ /dev/null
@@ -1,36 +0,0 @@
-from glob import glob
-import os
-
-from setuptools import find_packages, setup
-
-package_name = 'robot_odometry'
-
-setup(
-    name=package_name,
-    version='0.2.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
-        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='Agus Darmawan',
-    maintainer_email='darmawandeveloper@gmail.com',
-    description='LIO odometry (FAST-LIO2 or Leg-KILO), stillness gate, monitor, route recorder',
-    license='Apache-2.0',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
-    entry_points={
-        'console_scripts': [
-            'wait_still = robot_odometry.wait_still:main',
-            'odometry_monitor_node = robot_odometry.odometry_monitor_node:main',
-            'route_recorder_node = robot_odometry.route_recorder_node:main',
-        ],
-    },
-)
diff --git a/src/robot_odometry/test/test_checks.py b/src/robot_odometry/test/test_checks.py
deleted file mode 100644
index 8807bc5..0000000
--- a/src/robot_odometry/test/test_checks.py
+++ /dev/null
@@ -1,53 +0,0 @@
-import math
-
-import numpy as np
-from robot_odometry.checks import looks_like_restart, stillness, yaw_from_quaternion
-from robot_odometry.wait_still import split_args
-
-
-def still_samples(n=200, tilt_deg=0.0, noise=0.0, gyro=0.0):
-    rng = np.random.default_rng(0)
-    t = math.radians(tilt_deg)
-    acc = np.tile([math.sin(t), 0.0, math.cos(t)], (n, 1)) + rng.normal(0.0, noise, (n, 3))
-    return np.full((n, 3), gyro), acc
-
-
-def test_still_level_imu_passes():
-    gyro_max, acc_std, tilt = stillness(*still_samples(noise=0.002))
-    assert gyro_max < 0.03 and acc_std < 0.01 and tilt < 1.0
-
-
-def test_tilt_and_rotation_are_reported():
-    _, _, tilt = stillness(*still_samples(tilt_deg=15.0))
-    assert abs(tilt - 15.0) < 0.1
-    gyro_max, _, _ = stillness(*still_samples(gyro=0.1))
-    assert gyro_max > 0.1
-
-
-def test_shaking_is_unit_free():
-    g_units = stillness(*still_samples(noise=0.05))[1]
-    gyro, acc = still_samples(noise=0.05)
-    si_units = stillness(gyro, acc * 9.81)[1]
-    assert g_units > 0.01 and abs(g_units - si_units) < 1e-9
-
-
-def test_yaw():
-    half = math.radians(30.0) / 2
-    yaw = yaw_from_quaternion(0.0, 0.0, math.sin(half), math.cos(half))
-    assert abs(yaw - math.radians(30)) < 1e-9
-
-
-def test_restart_detection():
-    assert looks_like_restart(5.0, 0.0, 0.0, 0.0, 0.05, 0.3)
-    assert looks_like_restart(0.0, math.radians(90), 0.01, 0.0, 0.05, 0.3)
-    assert not looks_like_restart(0.02, 0.0, 0.0, 0.0, 0.05, 0.3)   # standing at the start
-    assert not looks_like_restart(5.0, 0.0, 4.9, 0.0, 0.05, 0.3)    # normal motion
-
-
-def test_split_args():
-    ros, cmd = split_args(['--params-file', 'p.yaml', '--', '/bin/fl', '--ros-args', '-r', 'x'])
-    assert ros == ['--ros-args', '--params-file', 'p.yaml']
-    assert cmd == ['/bin/fl', '--ros-args', '-r', 'x']
-    assert split_args(['--', 'a'])[0] == []
-    ros, _ = split_args(['--params-file', 'a.yaml', '--params-file', 'site.yaml', '--', 'x'])
-    assert ros == ['--ros-args', '--params-file', 'a.yaml', '--params-file', 'site.yaml']
diff --git a/src/robot_odometry/test/test_copyright.py b/src/robot_odometry/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/src/robot_odometry/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/src/robot_odometry/test/test_flake8.py b/src/robot_odometry/test/test_flake8.py
deleted file mode 100644
index d0d8a70..0000000
--- a/src/robot_odometry/test/test_flake8.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors)
diff --git a/src/robot_odometry/test/test_node_members.py b/src/robot_odometry/test/test_node_members.py
deleted file mode 100644
index 95a734e..0000000
--- a/src/robot_odometry/test/test_node_members.py
+++ /dev/null
@@ -1,105 +0,0 @@
-"""
-Node subclasses must not shadow rclpy.node.Node members.
-
-rclpy keeps its own state in attributes such as ``handle``, ``context`` or ``_clock``. A subclass
-that defines a method or attribute with one of those names breaks the node at runtime, e.g.
-``AttributeError: __enter__`` (a method called ``handle``) or
-``'function' object has no attribute 'handle'`` (a method called ``context``).
-"""
-
-import ast
-from pathlib import Path
-
-# Set by rclpy.node.Node.__init__ (Humble and Jazzy), not visible through dir(Node).
-NODE_INTERNALS = {
-    '_allow_undeclared_parameters',
-    '_clock',
-    '_context',
-    '_default_callback_group',
-    '_descriptors',
-    '_logger',
-    '_parameter_event_publisher',
-    '_parameter_overrides',
-    '_parameter_service',
-    '_parameters',
-    '_rate_group',
-    '_time_source',
-}
-# Public members, kept here so the test also works without ROS installed.
-NODE_PUBLIC = {
-    'clients',
-    'context',
-    'default_callback_group',
-    'executor',
-    'guards',
-    'handle',
-    'publishers',
-    'services',
-    'subscriptions',
-    'timers',
-    'waitables',
-}
-# Members a subclass may override on purpose (always calling super()).
-ALLOWED = {'destroy_node'}
-
-
-def node_members() -> set:
-    """Return every name a Node subclass must not redefine."""
-    names = NODE_PUBLIC | NODE_INTERNALS
-    try:
-        from rclpy.node import Node
-    except ImportError:
-        return names - ALLOWED
-    return (names | {n for n in dir(Node) if not n.startswith('__')}) - ALLOWED
-
-
-def is_node_class(cls: ast.ClassDef) -> bool:
-    """Return True for `class X(Node)` and `class X(rclpy.node.Node)`."""
-    for base in cls.bases:
-        name = base.attr if isinstance(base, ast.Attribute) else getattr(base, 'id', '')
-        if name == 'Node':
-            return True
-    return False
-
-
-def defined_names(cls: ast.ClassDef):
-    """Yield (name, line) for methods and self.<name> assignments of a class."""
-    for item in cls.body:
-        if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef)):
-            yield item.name, item.lineno
-        elif isinstance(item, ast.Assign):
-            for target in item.targets:
-                if isinstance(target, ast.Name):
-                    yield target.id, item.lineno
-    for node in ast.walk(cls):
-        targets = []
-        if isinstance(node, ast.Assign):
-            targets = node.targets
-        elif isinstance(node, (ast.AnnAssign, ast.AugAssign)):
-            targets = [node.target]
-        for target in targets:
-            for t in ast.walk(target):
-                if (
-                    isinstance(t, ast.Attribute)
-                    and isinstance(t.value, ast.Name)
-                    and t.value.id == 'self'
-                ):
-                    yield t.attr, node.lineno
-
-
-def test_no_shadowed_node_members():
-    """Every Node subclass in this package keeps rclpy's attributes intact."""
-    forbidden = node_members()
-    package = Path(__file__).resolve().parents[1]
-    problems = []
-    for path in sorted(package.glob('*/**/*.py')):
-        if 'test' in path.relative_to(package).parts[:1]:
-            continue
-        tree = ast.parse(path.read_text(), filename=str(path))
-        for cls in (n for n in ast.walk(tree) if isinstance(n, ast.ClassDef)):
-            if not is_node_class(cls):
-                continue
-            for name, line in defined_names(cls):
-                if name in forbidden:
-                    problems.append(f'{path.name}:{line} {cls.name}.{name} shadows rclpy Node')
-    assert not problems, '\n'.join(sorted(set(problems)))
diff --git a/src/robot_odometry/test/test_odometry_nodes.py b/src/robot_odometry/test/test_odometry_nodes.py
deleted file mode 100644
index 3900d25..0000000
--- a/src/robot_odometry/test/test_odometry_nodes.py
+++ /dev/null
@@ -1,182 +0,0 @@
-"""In-process tests of the odometry gate, monitor and route recorder against fake sensor data."""
-
-import csv
-import math
-import time
-
-import pytest
-
-rclpy = pytest.importorskip('rclpy')
-SingleThreadedExecutor = pytest.importorskip('rclpy.executors').SingleThreadedExecutor
-qos = pytest.importorskip('rclpy.qos')
-Imu = pytest.importorskip('sensor_msgs.msg').Imu
-Odometry = pytest.importorskip('nav_msgs.msg').Odometry
-std_msgs = pytest.importorskip('std_msgs.msg')
-std_srvs = pytest.importorskip('std_srvs.srv')
-TransformStamped = pytest.importorskip('geometry_msgs.msg').TransformStamped
-tf2_ros = pytest.importorskip('tf2_ros')
-MarkStopPoint = pytest.importorskip('robot_interfaces.srv').MarkStopPoint
-StillGate = pytest.importorskip('robot_odometry.wait_still').StillGate
-OdometryMonitorNode = pytest.importorskip(
-    'robot_odometry.odometry_monitor_node'
-).OdometryMonitorNode
-RouteRecorderNode = pytest.importorskip('robot_odometry.route_recorder_node').RouteRecorderNode
-
-
-@pytest.fixture
-def ros(request):
-    """Init rclpy with test parameters; yields (executor, helper node)."""
-    params = getattr(request, 'param', [])
-    rclpy.init(args=['--ros-args', *[a for p in params for a in ('-p', p)]])
-    helper = rclpy.create_node('odometry_test_helper')
-    executor = SingleThreadedExecutor()
-    executor.add_node(helper)
-    yield executor, helper
-    executor.shutdown()
-    helper.destroy_node()
-    rclpy.shutdown()
-
-
-def spin(executor, seconds: float, every=None, period: float = 0.0):
-    """Spin for a while, calling every() at most once per period."""
-    end, last = time.monotonic() + seconds, 0.0
-    while time.monotonic() < end:
-        if every and time.monotonic() - last >= period:
-            last = time.monotonic()
-            every()
-        executor.spin_once(timeout_sec=0.002)
-
-
-def imu(tilt_deg: float = 0.0, gyro: float = 0.0) -> Imu:
-    """Return a Livox-like IMU sample (acceleration in g)."""
-    msg = Imu()
-    t = math.radians(tilt_deg)
-    msg.linear_acceleration.x, msg.linear_acceleration.z = math.sin(t), math.cos(t)
-    msg.angular_velocity.z = gyro
-    return msg
-
-
-@pytest.mark.parametrize('ros', [['still_sec:=0.5', 'timeout_sec:=0.0']], indirect=True)
-def test_gate_opens_when_still_and_level(ros):
-    """A still, level IMU releases the gate after still_sec."""
-    executor, helper = ros
-    gate = StillGate()
-    executor.add_node(gate)
-    pub = helper.create_publisher(Imu, '/livox/imu', 50)
-    spin(executor, 1.5, lambda: pub.publish(imu()), 0.005)
-    assert gate.done
-    executor.remove_node(gate)
-    gate.destroy_node()
-
-
-@pytest.mark.parametrize('ros', [['still_sec:=0.5', 'timeout_sec:=0.0']], indirect=True)
-def test_gate_holds_while_tilted_or_turning(ros):
-    """A sitting (tilted) or rotating robot keeps the gate closed."""
-    executor, helper = ros
-    gate = StillGate()
-    executor.add_node(gate)
-    pub = helper.create_publisher(Imu, '/livox/imu', 50)
-    spin(executor, 1.2, lambda: pub.publish(imu(tilt_deg=15.0)), 0.005)
-    assert not gate.done
-    spin(executor, 1.2, lambda: pub.publish(imu(gyro=0.2)), 0.005)
-    assert not gate.done
-    executor.remove_node(gate)
-    gate.destroy_node()
-
-
-@pytest.mark.parametrize('ros', [['leg_odom_topic:=/test/no_leg_odom']], indirect=True)
-def test_monitor_reports_ok_jump_restart_and_stale(ros):
-    """Smooth motion is ok; a jump degrades; snapping back to the origin is a restart."""
-    executor, helper = ros
-    monitor = OdometryMonitorNode()
-    executor.add_node(monitor)
-    statuses, restarts = [], []
-    latched = qos.QoSProfile(depth=1, durability=qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)
-    helper.create_subscription(
-        std_msgs.String, 'odometry/status', lambda m: statuses.append(m.data), latched
-    )
-    helper.create_subscription(
-        std_msgs.Empty, 'odometry/restarted', lambda m: restarts.append(m), 10
-    )
-    pub = helper.create_publisher(Odometry, '/Odometry', 10)
-    state = {'x': 0.0, 't': 0.0}
-
-    def odom(x=None):
-        state['t'] += 0.1
-        state['x'] = state['x'] + 0.05 if x is None else x
-        msg = Odometry()
-        msg.header.stamp.sec = int(state['t'])
-        msg.header.stamp.nanosec = int((state['t'] % 1.0) * 1e9)
-        msg.pose.pose.position.x = state['x']
-        msg.pose.pose.orientation.w = 1.0
-        pub.publish(msg)
-
-    spin(executor, 1.5, odom, 0.1)
-    assert statuses[-1] == 'ok'
-    state['t'] -= 0.1
-    odom(x=state['x'])  # the same stamp again: a duplicate, not a restart
-    spin(executor, 0.3)
-    assert statuses[-1] == 'ok' and not restarts
-    odom(x=state['x'] + 2.0)  # 2 m in one 0.1 s frame
-    spin(executor, 0.3)
-    assert statuses[-1].startswith('degraded: jumped')
-    odom(x=0.0)  # FAST-LIO came back at the origin
-    spin(executor, 0.3)
-    assert restarts and statuses[-1].startswith('lost: odometry restarted')
-    spin(executor, 1.0)  # nothing published any more
-    assert statuses[-1].startswith('lost: no odometry')
-    executor.remove_node(monitor)
-    monitor.destroy_node()
-
-
-def test_recorder_writes_route_with_stop_point(tmp_path):
-    """Recording samples map->base_link, marks a stop and writes the CSV."""
-    rclpy.init(args=['--ros-args', '-p', f'output_path:={tmp_path}', '-p', 'record_rate_hz:=20.0'])
-    try:
-        helper = rclpy.create_node('recorder_test_helper')
-        recorder = RouteRecorderNode()
-        executor = SingleThreadedExecutor()
-        executor.add_node(helper)
-        executor.add_node(recorder)
-        broadcaster = tf2_ros.TransformBroadcaster(helper)
-        saved = []
-        helper.create_subscription(std_msgs.String, 'route/saved', saved.append, 10)
-        record = helper.create_client(std_srvs.SetBool, 'route/record')
-        mark = helper.create_client(MarkStopPoint, 'route/mark_stop_point')
-        pose = {'x': 0.0}
-
-        def move():
-            pose['x'] += 0.02
-            t = TransformStamped()
-            t.header.stamp = helper.get_clock().now().to_msg()
-            t.header.frame_id, t.child_frame_id = 'map', 'base_link'
-            t.transform.translation.x = pose['x']
-            t.transform.rotation.w = 1.0
-            broadcaster.sendTransform(t)
-
-        def call(client, request):
-            future = client.call_async(request)
-            end = time.monotonic() + 3.0
-            while not future.done() and time.monotonic() < end:
-                move()
-                executor.spin_once(timeout_sec=0.01)
-            return future.result()
-
-        assert call(record, std_srvs.SetBool.Request(data=True)).success
-        spin(executor, 1.5, move, 0.01)
-        assert call(mark, MarkStopPoint.Request(dwell_sec=5.0)).success
-        spin(executor, 0.5, move, 0.01)
-        assert call(record, std_srvs.SetBool.Request(data=False)).success
-        spin(executor, 0.2)
-        assert saved, 'route/saved not published'
-        with open(saved[-1].data) as f:
-            rows = [r for r in csv.reader(f) if r and not r[0].startswith('#')]
-        assert len(rows) >= 3
-        assert sum(1 for r in rows if float(r[3]) > 0.0) == 1
-        xs = [float(r[0]) for r in rows]
-        assert all(b >= a for a, b in zip(xs, xs[1:]))
-        executor.shutdown()
-        recorder.destroy_node()
-        helper.destroy_node()
-    finally:
-        rclpy.shutdown()
diff --git a/src/robot_odometry/test/test_pep257.py b/src/robot_odometry/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/src/robot_odometry/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/robot_perception/launch/perception.launch.py b/src/robot_perception/launch/perception.launch.py
deleted file mode 100644
index bc74763..0000000
--- a/src/robot_perception/launch/perception.launch.py
+++ /dev/null
@@ -1,81 +0,0 @@
-#!/usr/bin/env python3
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    """Launch lidar sectors, AprilTag detection and, optionally, people detection."""
-    default_params = os.path.join(
-        get_package_share_directory('robot_perception'), 'config', 'perception_params.yaml'
-    )
-    params_file = LaunchConfiguration('params_file')
-    image = '/camera/camera/color/image_raw'
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument('params_file', default_value=default_params),
-            DeclareLaunchArgument('people', default_value='true',
-                                  description='Run the person detector (crowd heatmap).'),
-            DeclareLaunchArgument('draw_tags', default_value='false',
-                                  description='Run apriltag_draw (needs that package).'),
-            Node(
-                package='robot_perception',
-                executable='lidar_sector_node',
-                name='lidar_sector_node',
-                output='screen',
-                parameters=[params_file],
-            ),
-            # the tag detector sees the camera at rate_hz only (localization needs a few poses/s)
-            Node(
-                package='robot_perception',
-                executable='image_throttle_node',
-                name='apriltag_throttle',
-                output='screen',
-                parameters=[params_file],
-                remappings=[
-                    ('in/image', image),
-                    ('in/camera_info', '/camera/camera/color/camera_info'),
-                    ('out/image', '/perception/apriltag/image_rect'),
-                    ('out/camera_info', '/perception/apriltag/camera_info'),
-                ],
-            ),
-            Node(
-                package='apriltag_ros',
-                executable='apriltag_node',
-                name='apriltag',
-                output='screen',
-                parameters=[params_file],
-                remappings=[
-                    ('image_rect', '/perception/apriltag/image_rect'),
-                    ('camera_info', '/perception/apriltag/camera_info'),
-                    ('detections', '/perception/apriltag/detections'),
-                ],
-            ),
-            Node(
-                package='robot_perception',
-                executable='person_detector_node',
-                name='person_detector_node',
-                output='screen',
-                parameters=[params_file],
-                condition=IfCondition(LaunchConfiguration('people')),
-            ),
-            Node(
-                package='apriltag_draw',
-                executable='apriltag_draw_node',
-                name='apriltag_draw_node',
-                output='screen',
-                remappings=[
-                    ('tags', '/perception/apriltag/detections'),
-                    ('image', image),
-                    ('image_tags', '/perception/apriltag/image_tags'),
-                ],
-                condition=IfCondition(LaunchConfiguration('draw_tags')),
-            ),
-        ]
-    )
diff --git a/src/robot_perception/resource/robot_perception b/src/robot_perception/resource/robot_perception
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_perception/robot_perception/__init__.py b/src/robot_perception/robot_perception/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/robot_perception/robot_perception/image_throttle_node.py b/src/robot_perception/robot_perception/image_throttle_node.py
deleted file mode 100644
index 973ae3a..0000000
--- a/src/robot_perception/robot_perception/image_throttle_node.py
+++ /dev/null
@@ -1,70 +0,0 @@
-#!/usr/bin/env python3
-import time
-
-import rclpy
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.qos import qos_profile_sensor_data, QoSProfile
-from sensor_msgs.msg import CameraInfo, Image
-
-
-class ImageThrottleNode(Node):
-    """
-    Pass a camera stream on at a lower rate, as serialized bytes (no image is deserialized).
-
-    The AprilTag detector processes every frame it receives, but localization only needs a tag
-    pose a few times per second. Every camera info is passed on, so each forwarded image finds
-    the info with its stamp (image_transport pairs them by exact time). Outputs are reliable:
-    the detector reads camera_info reliable and would get nothing from a best-effort publisher;
-    best-effort readers still receive them.
-    """
-
-    def __init__(self):
-        """Declare the rate and relay in/* to out/*."""
-        super().__init__('image_throttle_node')
-        self.declare_parameter('rate_hz', 5.0)
-        rate = float(self.get_parameter('rate_hz').value)
-        self._period = 1.0 / rate if rate > 0.0 else 0.0
-        self._last = -1e9
-        out = QoSProfile(depth=5)  # reliable, keep last
-        self._image_pub = self.create_publisher(Image, 'out/image', out)
-        self._info_pub = self.create_publisher(CameraInfo, 'out/camera_info', out)
-        sensor = qos_profile_sensor_data
-        self.create_subscription(Image, 'in/image', self.on_image, sensor, raw=True)
-        self.create_subscription(CameraInfo, 'in/camera_info', self.on_info, sensor, raw=True)
-
-    def on_image(self, data: bytes):
-        """Forward an image when the period has passed since the last one."""
-        now = time.monotonic()
-        if now - self._last < self._period:
-            return
-        self._last = now
-        self._image_pub.publish(data)
-
-    def on_info(self, data: bytes):
-        """Forward every camera info (small)."""
-        self._info_pub.publish(data)
-
-
-def main(args=None):
-    """Spin the image throttle node."""
-    rclpy.init(args=args)
-    node = ImageThrottleNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_perception/robot_perception/lidar_sector_node.py b/src/robot_perception/robot_perception/lidar_sector_node.py
deleted file mode 100644
index 19c7231..0000000
--- a/src/robot_perception/robot_perception/lidar_sector_node.py
+++ /dev/null
@@ -1,327 +0,0 @@
-#!/usr/bin/env python3
-import math
-
-from geometry_msgs.msg import Point
-import numpy as np
-import rclpy
-from rclpy.duration import Duration
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from rclpy.serialization import deserialize_message
-from robot_interfaces.msg import SectorScan
-from robot_perception.livox_cdr import parse_custom_msg
-from robot_perception.sectors import bin_ranges, obstacle_points
-from sensor_msgs.msg import PointCloud2
-from std_msgs.msg import ColorRGBA, Header
-from tf2_ros import Buffer, TransformException, TransformListener
-import tf_transformations
-from visualization_msgs.msg import Marker, MarkerArray
-
-
-class LidarSectorNode(Node):
-    """
-    Bins lidar points into fixed angular sectors and publishes a SectorScan.
-
-    perception/sector_scan covers the front (fov_deg) for obstacle avoidance;
-    perception/safety_scan covers the full circle for the protective field in the control mux,
-    and reports every sector blocked when the LiDAR returns almost nothing (covered or failed).
-    """
-
-    def __init__(self):
-        """Declare params, set up TF, and wire the subscription/publishers."""
-        super().__init__('lidar_sector_node')
-        self.declare_parameter('input_type', 'livox')
-        self.declare_parameter('pointcloud_topic', '/cloud_registered_body')
-        self.declare_parameter('base_frame', 'base_link')
-        self.declare_parameter('lidar_frame', 'livox_frame')
-        self.declare_parameter('num_sectors', 32)
-        self.declare_parameter('fov_deg', 180.0)
-        self.declare_parameter('range_max', 8.0)
-        self.declare_parameter('range_min', 0.1)
-        self.declare_parameter('obstacle_z_min', -0.25)
-        self.declare_parameter('obstacle_z_max', 1.20)
-        self.declare_parameter('publish_markers', True)
-        self.declare_parameter('marker_lifetime_sec', 0.3)
-        self.declare_parameter('corridor_x_min', 0.0)
-        self.declare_parameter('corridor_x_max', 5.0)
-        self.declare_parameter('corridor_y_min', -1.0)
-        self.declare_parameter('corridor_y_max', 1.0)
-        self.declare_parameter('self_filter_x_min', -0.40)
-        self.declare_parameter('self_filter_x_max', 0.40)
-        self.declare_parameter('self_filter_y_min', -0.28)
-        self.declare_parameter('self_filter_y_max', 0.28)
-        self.declare_parameter('safety_scan_enabled', True)
-        self.declare_parameter('safety_num_sectors', 72)
-        self.declare_parameter('safety_range_max', 4.0)
-        self.declare_parameter('min_raw_points', 100)
-
-        self._num_sectors = int(self.get_parameter('num_sectors').value)
-        self._fov = math.radians(float(self.get_parameter('fov_deg').value))
-        self._range_max = float(self.get_parameter('range_max').value)
-        self._range_min = float(self.get_parameter('range_min').value)
-        self._z_min = float(self.get_parameter('obstacle_z_min').value)
-        self._z_max = float(self.get_parameter('obstacle_z_max').value)
-        self._publish_markers = bool(self.get_parameter('publish_markers').value)
-        self._marker_lifetime = Duration(
-            seconds=float(self.get_parameter('marker_lifetime_sec').value)
-        ).to_msg()
-        self._corridor_x_min = float(self.get_parameter('corridor_x_min').value)
-        self._corridor_x_max = float(self.get_parameter('corridor_x_max').value)
-        self._corridor_y_min = float(self.get_parameter('corridor_y_min').value)
-        self._corridor_y_max = float(self.get_parameter('corridor_y_max').value)
-        self._self_box = [
-            float(self.get_parameter(f'self_filter_{k}').value)
-            for k in ('x_min', 'x_max', 'y_min', 'y_max')
-        ]
-
-        self._angle_min = -self._fov / 2.0
-        self._angle_increment = self._fov / self._num_sectors
-        self._safety_sectors = int(self.get_parameter('safety_num_sectors').value)
-        self._safety_range_max = float(self.get_parameter('safety_range_max').value)
-        self._min_raw_points = int(self.get_parameter('min_raw_points').value)
-
-        self._base_frame = self.get_parameter('base_frame').value
-        self._fallback_frame = self.get_parameter('lidar_frame').value
-        self._tf_buffer = Buffer()
-        self._tf_listener = None  # only until the LiDAR mount is known, see extrinsic()
-        self._extrinsics: dict[str, tuple[np.ndarray, np.ndarray]] = {}
-
-        input_type = self.get_parameter('input_type').value
-        if input_type == 'pointcloud2':
-            topic = self.get_parameter('pointcloud_topic').value
-            self.create_subscription(PointCloud2, topic, self.pointcloud_callback, 10)
-        elif input_type == 'livox':
-            from livox_ros_driver2.msg import CustomMsg  # only needed for raw Livox input
-
-            self._custom_msg = CustomMsg
-            self.create_subscription(CustomMsg, 'livox/lidar', self.livox_callback, 10, raw=True)
-        else:
-            raise ValueError(f"input_type must be 'livox' or 'pointcloud2', got {input_type!r}")
-        self._sectorscan_pub = self.create_publisher(SectorScan, 'perception/sector_scan', 10)
-        self._safety_pub = (
-            self.create_publisher(SectorScan, 'perception/safety_scan', 10)
-            if bool(self.get_parameter('safety_scan_enabled').value)
-            else None
-        )
-        self._markers_pub = (
-            self.create_publisher(MarkerArray, 'perception/obstacles', 10)
-            if self._publish_markers
-            else None
-        )
-
-    def extrinsic(self, frame_id: str):
-        """
-        Return the cached (rotation, translation) from frame_id to base_frame, or None.
-
-        The mount is fixed, so TF is only listened to until it is known instead of deserializing
-        every /tf message for the whole run.
-        """
-        if frame_id not in self._extrinsics:
-            if self._tf_listener is None:
-                self._tf_listener = TransformListener(self._tf_buffer, self)
-            try:
-                tf = self._tf_buffer.lookup_transform(
-                    self._base_frame, frame_id, rclpy.time.Time()
-                )
-            except TransformException as error:
-                self.get_logger().warn(
-                    f'waiting for TF {frame_id} -> {self._base_frame}: {error}',
-                    throttle_duration_sec=2.0,
-                )
-                return None
-            q, t = tf.transform.rotation, tf.transform.translation
-            rotation = tf_transformations.quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3]
-            self._extrinsics[frame_id] = (rotation, np.array([t.x, t.y, t.z]))
-            self._tf_listener.unregister()
-            self._tf_listener = None
-        return self._extrinsics[frame_id]
-
-    def range_to_color(self, r: float) -> ColorRGBA:
-        """Map a range value to a red-to-green ColorRGBA for visualization."""
-        t = max(0.0, min(1.0, r / self._range_max)) if self._range_max > 0 else 1.0
-        if t < 0.5:
-            ratio = t / 0.5
-            return ColorRGBA(r=1.0, g=ratio, b=0.0, a=0.9)
-        ratio = (t - 0.5) / 0.5
-        return ColorRGBA(r=1.0 - ratio, g=1.0, b=0.0, a=0.9)
-
-    def corridor_ray_length(self, theta: float) -> float:
-        """Clamp range_max to the rectangular corridor at this angle (RViz rays only)."""
-        length = self._range_max
-        sin_t, cos_t = math.sin(theta), math.cos(theta)
-
-        if sin_t != 0.0:
-            if length * sin_t > self._corridor_y_max:
-                length = self._corridor_y_max / sin_t
-            elif length * sin_t < self._corridor_y_min:
-                length = self._corridor_y_min / sin_t
-
-        if cos_t != 0.0:
-            if length * cos_t > self._corridor_x_max:
-                length = self._corridor_x_max / cos_t
-            elif length * cos_t < self._corridor_x_min:
-                length = self._corridor_x_min / cos_t
-
-        return max(0.0, length)
-
-    def livox_callback(self, data: bytes):
-        """Read the points of a serialized Livox CustomMsg as numpy (no per-point objects)."""
-        parsed = parse_custom_msg(data)
-        if parsed is None:
-            self.livox_fallback(data)
-            return
-        sec, nanosec, frame_id, xyz = parsed
-        header = Header(frame_id=frame_id)
-        header.stamp.sec, header.stamp.nanosec = sec, nanosec
-        self.process(header, xyz)
-
-    def livox_fallback(self, data: bytes):
-        """Deserialize with rclpy when the bytes are not little-endian XCDR1 (slow path)."""
-        self.get_logger().warn('Livox scan is not plain CDR, using the slow path', once=True)
-        msg = deserialize_message(data, self._custom_msg)
-        xyz = np.array([(p.x, p.y, p.z) for p in msg.points], dtype=np.float32).reshape(-1, 3)
-        self.process(msg.header, xyz[np.isfinite(xyz).all(axis=1) & xyz.any(axis=1)])
-
-    def pointcloud_callback(self, msg: PointCloud2):
-        """Read x/y/z straight from the PointCloud2 buffer (no per-point Python objects)."""
-        offsets = {f.name: f.offset for f in msg.fields}
-        if not {'x', 'y', 'z'} <= offsets.keys():
-            self.get_logger().error('PointCloud2 has no x/y/z fields', throttle_duration_sec=5.0)
-            return
-        dtype = np.dtype(
-            {
-                'names': ['x', 'y', 'z'],
-                'formats': ['<f4' if not msg.is_bigendian else '>f4'] * 3,
-                'offsets': [offsets['x'], offsets['y'], offsets['z']],
-                'itemsize': msg.point_step,
-            }
-        )
-        cloud = np.frombuffer(msg.data, dtype=dtype, count=msg.width * msg.height)
-        xyz = np.stack([cloud['x'], cloud['y'], cloud['z']], axis=1).astype(np.float32)
-        self.process(msg.header, xyz[np.isfinite(xyz).all(axis=1)])
-
-    def process(self, header, xyz: np.ndarray):
-        """Transform points into base_frame, bin them per sector, and publish."""
-        extrinsic = self.extrinsic(header.frame_id or self._fallback_frame)
-        if extrinsic is None:
-            return
-        rotation, translation = extrinsic
-        raw_points = xyz.shape[0]
-        points = xyz @ rotation.T + translation if raw_points else np.empty((0, 3))
-        xy = obstacle_points(points, self._z_min, self._z_max, self._self_box)
-
-        ranges = bin_ranges(
-            xy, self._angle_min, self._fov, self._num_sectors, self._range_min, self._range_max
-        )
-        self.publish_scan(
-            self._sectorscan_pub, header, ranges, self._angle_min, self._fov, self._range_max
-        )
-        if self._safety_pub is not None:
-            if raw_points < self._min_raw_points:
-                self.get_logger().warn(
-                    f'LiDAR blind ({raw_points} points), safety scan reports every sector blocked',
-                    throttle_duration_sec=2.0,
-                )
-                safety = np.zeros(self._safety_sectors, dtype=np.float32)
-            else:
-                safety = bin_ranges(
-                    xy,
-                    -math.pi,
-                    2.0 * math.pi,
-                    self._safety_sectors,
-                    self._range_min,
-                    self._safety_range_max,
-                )
-            self.publish_scan(
-                self._safety_pub, header, safety, -math.pi, 2.0 * math.pi, self._safety_range_max
-            )
-        if self._publish_markers:
-            self.publish_marker_array(header, ranges)
-
-    def publish_scan(self, publisher, header, ranges, angle_min, fov, range_max):
-        """Fill and publish a SectorScan message from the binned ranges."""
-        out = SectorScan()
-        out.header.stamp = header.stamp
-        out.header.frame_id = self._base_frame
-        out.num_sectors = len(ranges)
-        out.angle_min = float(angle_min)
-        out.angle_increment = float(fov / len(ranges))
-        out.range_max = float(range_max)
-        out.ranges = ranges.tolist()
-        publisher.publish(out)
-
-    def publish_marker_array(self, header, ranges: np.ndarray):
-        """Build and publish ray and point markers for RViz visualization."""
-        ray_marker = Marker()
-        ray_marker.header.frame_id = self._base_frame  # stamp 0: RViz draws on the latest TF
-        ray_marker.ns = 'sector_rays'
-        ray_marker.id = 0
-        ray_marker.type = Marker.LINE_LIST
-        ray_marker.action = Marker.ADD
-        ray_marker.pose.orientation.w = 1.0
-        ray_marker.scale.x = 0.02
-        ray_marker.lifetime = self._marker_lifetime
-
-        point_marker = Marker()
-        point_marker.header = ray_marker.header
-        point_marker.ns = 'sector_points'
-        point_marker.id = 1
-        point_marker.type = Marker.SPHERE_LIST
-        point_marker.action = Marker.ADD
-        point_marker.pose.orientation.w = 1.0
-        point_marker.scale.x = point_marker.scale.y = point_marker.scale.z = 0.08
-        point_marker.lifetime = self._marker_lifetime
-
-        origin = Point(x=0.0, y=0.0, z=0.0)
-        detection_eps = 1e-3
-        for i, r in enumerate(ranges):
-            r = float(r)
-            theta = self._angle_min + (i + 0.5) * self._angle_increment
-            color = self.range_to_color(r)
-
-            ray_length = self.corridor_ray_length(theta)
-            ray_end = Point(
-                x=float(ray_length * math.cos(theta)),
-                y=float(ray_length * math.sin(theta)),
-                z=0.0,
-            )
-            ray_marker.points.append(origin)
-            ray_marker.points.append(ray_end)
-            ray_marker.colors.append(color)
-            ray_marker.colors.append(color)
-
-            has_detection = r < (self._range_max - detection_eps)
-            within_corridor = r <= ray_length + detection_eps
-            if has_detection and within_corridor:
-                detected = Point(x=float(r * math.cos(theta)), y=float(r * math.sin(theta)), z=0.0)
-                point_marker.points.append(detected)
-                point_marker.colors.append(color)
-
-        markers = MarkerArray()
-        markers.markers.append(ray_marker)
-        markers.markers.append(point_marker)
-        self._markers_pub.publish(markers)
-
-
-def main(args=None):
-    """Spin the lidar sector perception node."""
-    rclpy.init(args=args)
-    node = LidarSectorNode()
-    try:
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    except Exception:  # noqa: B902
-        if rclpy.ok():
-            raise  # a real error while running
-        # else: Ctrl-C shut the context down under spin(), a normal stop
-    finally:
-        node.destroy_node()
-        try:
-            rclpy.shutdown()
-        except Exception:  # noqa: B902  already shut down by the SIGINT handler
-            pass
-
-
-if __name__ == '__main__':
-    main()
diff --git a/src/robot_perception/robot_perception/livox_cdr.py b/src/robot_perception/robot_perception/livox_cdr.py
deleted file mode 100644
index 80bb151..0000000
--- a/src/robot_perception/robot_perception/livox_cdr.py
+++ /dev/null
@@ -1,56 +0,0 @@
-"""
-Read a serialized Livox CustomMsg straight from its CDR bytes (no ROS needed, unit-tested).
-
-rclpy turns every CustomPoint of a deserialized CustomMsg into a Python object: ~20k objects per
-Mid-360 scan, which held lidar_sector_node to ~6 Hz on a full Jetson core. The node subscribes with
-raw=True instead and reads the points here with numpy, as a strided view of the buffer.
-
-Layout (XCDR1 little endian, offsets aligned from the end of the 4-byte encapsulation header):
-header (int32 sec, uint32 nanosec, string frame_id), uint64 timebase, uint32 point_num,
-uint8 lidar_id, uint8[3] rsvd, sequence<CustomPoint> points; CustomPoint is uint32 offset_time,
-float32 x y z, uint8 reflectivity tag line, padded to 20 bytes between elements.
-"""
-
-import struct
-
-import numpy as np
-
-CDR_LE = b'\x00\x01'
-POINT_STRIDE = 20
-POINT_SIZE = 19
-
-
-def _align(offset: int, size: int) -> int:
-    """Round offset up to a multiple of size."""
-    return (offset + size - 1) // size * size
-
-
-def parse_custom_msg(data: bytes):
-    """
-    Return (sec, nanosec, frame_id, xyz) from a serialized CustomMsg, or None if it does not fit.
-
-    xyz is an Nx3 float32 array; points at the sensor origin (no return) or not finite are dropped.
-    """
-    buf = memoryview(data)
-    if len(buf) < 4 or bytes(buf[:2]) != CDR_LE:
-        return None
-    body = buf[4:]
-    try:
-        sec, nanosec, name_len = struct.unpack_from('<iII', body, 0)
-        offset = 12 + name_len
-        frame_id = bytes(body[12:offset]).rstrip(b'\x00').decode()
-        offset = _align(offset, 8) + 8  # timebase
-        offset = _align(offset + 4 + 1 + 3, 4)  # point_num, lidar_id, rsvd
-        (count,) = struct.unpack_from('<I', body, offset)
-    except (struct.error, UnicodeDecodeError):
-        return None
-    offset += 4
-    if name_len == 0 or (count and offset + (count - 1) * POINT_STRIDE + POINT_SIZE > len(body)):
-        return None
-    if count == 0:
-        return sec, nanosec, frame_id, np.empty((0, 3), dtype=np.float32)
-    xyz = np.ndarray(
-        (count, 3), dtype='<f4', buffer=body, offset=offset + 4, strides=(POINT_STRIDE, 4)
-    )
-    keep = np.isfinite(xyz).all(axis=1) & xyz.any(axis=1)
-    return sec, nanosec, frame_id, xyz[keep].astype(np.float32, copy=False)
diff --git a/src/robot_perception/robot_perception/sectors.py b/src/robot_perception/robot_perception/sectors.py
deleted file mode 100644
index ed1da99..0000000
--- a/src/robot_perception/robot_perception/sectors.py
+++ /dev/null
@@ -1,52 +0,0 @@
-"""Pure helpers that turn LiDAR points into nearest-range sectors (unit-tested without ROS)."""
-
-import math
-
-import numpy as np
-
-
-def obstacle_points(points: np.ndarray, z_min: float, z_max: float, self_box) -> np.ndarray:
-    """
-    Return the (x, y) of points that can block the robot, all in base_link.
-
-    Keeps the points between z_min and z_max (obstacles, not floor or ceiling) and drops the
-    points inside self_box = (x_min, x_max, y_min, y_max), the robot's own body and legs.
-    """
-    if points.shape[0] == 0:
-        return np.empty((0, 2), dtype=np.float32)
-    x, y, z = points[:, 0], points[:, 1], points[:, 2]
-    sx0, sx1, sy0, sy1 = self_box
-    on_body = (x > sx0) & (x < sx1) & (y > sy0) & (y < sy1)
-    keep = (z >= z_min) & (z <= z_max) & ~on_body
-    return np.stack([x[keep], y[keep]], axis=1)
-
-
-def bin_ranges(
-    xy: np.ndarray,
-    angle_min: float,
-    fov: float,
-    num_sectors: int,
-    range_min: float,
-    range_max: float,
-) -> np.ndarray:
-    """
-    Return the nearest range per angular sector; range_max where a sector saw nothing.
-
-    Sector i spans [angle_min + i * fov / n, angle_min + (i + 1) * fov / n) (REP-103, +left).
-    """
-    ranges = np.full(num_sectors, range_max, dtype=np.float32)
-    if xy.shape[0] == 0:
-        return ranges
-    r = np.hypot(xy[:, 0], xy[:, 1])
-    theta = np.arctan2(xy[:, 1], xy[:, 0])
-    if fov < 2.0 * math.pi - 1e-6:
-        inside = (theta >= angle_min) & (theta < angle_min + fov)
-    else:  # full circle: wrap every angle into [angle_min, angle_min + 2 pi)
-        theta = np.mod(theta - angle_min, 2.0 * math.pi) + angle_min
-        inside = np.ones(theta.shape, dtype=bool)
-    valid = inside & (r >= range_min) & (r <= range_max)
-    r, theta = r[valid], theta[valid]
-    if r.size:
-        idx = np.clip(((theta - angle_min) / (fov / num_sectors)).astype(int), 0, num_sectors - 1)
-        np.minimum.at(ranges, idx, r.astype(np.float32))
-    return ranges
diff --git a/src/robot_perception/setup.cfg b/src/robot_perception/setup.cfg
deleted file mode 100644
index 3311c7a..0000000
--- a/src/robot_perception/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/robot_perception
-[install]
-install_scripts=$base/lib/robot_perception
diff --git a/src/robot_perception/setup.py b/src/robot_perception/setup.py
deleted file mode 100644
index b7b3052..0000000
--- a/src/robot_perception/setup.py
+++ /dev/null
@@ -1,32 +0,0 @@
-from glob import glob
-import os
-
-from setuptools import find_packages, setup
-
-package_name = 'robot_perception'
-
-setup(
-    name=package_name,
-    version='0.2.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
-        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='Agus Darmawan',
-    maintainer_email='darmawandeveloper@gmail.com',
-    description='Lidar obstacle sectors, AprilTag detection and person detection for crowd maps',
-    license='Apache-2.0',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'lidar_sector_node = robot_perception.lidar_sector_node:main',
-            'image_throttle_node = robot_perception.image_throttle_node:main',
-            'person_detector_node = robot_perception.person_detector_node:main',
-        ],
-    },
-)
diff --git a/src/robot_perception/test/test_copyright.py b/src/robot_perception/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/src/robot_perception/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/src/robot_perception/test/test_flake8.py b/src/robot_perception/test/test_flake8.py
deleted file mode 100644
index d0d8a70..0000000
--- a/src/robot_perception/test/test_flake8.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors)
diff --git a/src/robot_perception/test/test_image_throttle_node.py b/src/robot_perception/test/test_image_throttle_node.py
deleted file mode 100644
index bcb9ef6..0000000
--- a/src/robot_perception/test/test_image_throttle_node.py
+++ /dev/null
@@ -1,57 +0,0 @@
-"""In-process test of image_throttle_node: 30 fps in, rate_hz out, camera infos all through."""
-
-import time
-
-import pytest
-
-rclpy = pytest.importorskip('rclpy')
-SingleThreadedExecutor = pytest.importorskip('rclpy.executors').SingleThreadedExecutor
-qos_profile_sensor_data = pytest.importorskip('rclpy.qos').qos_profile_sensor_data
-sensor_msgs = pytest.importorskip('sensor_msgs.msg')
-ImageThrottleNode = pytest.importorskip(
-    'robot_perception.image_throttle_node'
-).ImageThrottleNode
-
-
-def test_images_are_thinned_out_and_infos_are_not():
-    """One second of a 30 fps camera gives about rate_hz images and every camera info."""
-    rclpy.init(args=['--ros-args', '-p', 'rate_hz:=5.0'])
-    node = ImageThrottleNode()
-    helper = rclpy.create_node('throttle_test_helper')
-    images, infos = [], []
-    # as apriltag_ros reads them: images best effort, camera_info reliable
-    helper.create_subscription(
-        sensor_msgs.Image, 'out/image', images.append, qos_profile_sensor_data
-    )
-    helper.create_subscription(sensor_msgs.CameraInfo, 'out/camera_info', infos.append, 10)
-    image_pub = helper.create_publisher(sensor_msgs.Image, 'in/image', 50)
-    info_pub = helper.create_publisher(sensor_msgs.CameraInfo, 'in/camera_info', 50)
-    executor = SingleThreadedExecutor()
-    executor.add_node(node)
-    executor.add_node(helper)
-    try:
-        pubs = (image_pub, info_pub, node._image_pub, node._info_pub)
-        ready = time.monotonic() + 5.0  # let discovery match every pair first
-        while time.monotonic() < ready and not all(p.get_subscription_count() for p in pubs):
-            executor.spin_once(timeout_sec=0.05)
-        sent = 0
-        end = time.monotonic() + 1.0
-        while time.monotonic() < end:
-            image = sensor_msgs.Image(height=2, width=2, encoding='rgb8', step=6, data=[7] * 12)
-            image.header.frame_id = 'camera'
-            image_pub.publish(image)
-            info_pub.publish(sensor_msgs.CameraInfo(width=2, height=2))
-            sent += 1
-            stop = time.monotonic() + 1 / 30
-            while time.monotonic() < stop:
-                executor.spin_once(timeout_sec=0.005)
-        for _ in range(50):
-            executor.spin_once(timeout_sec=0.01)
-        assert 3 <= len(images) <= 7, f'{len(images)} of {sent} images forwarded'
-        assert len(infos) >= 0.8 * sent
-        assert list(images[0].data) == [7] * 12 and images[0].header.frame_id == 'camera'
-    finally:
-        executor.shutdown()
-        helper.destroy_node()
-        node.destroy_node()
-        rclpy.shutdown()
diff --git a/src/robot_perception/test/test_lidar_sector_node.py b/src/robot_perception/test/test_lidar_sector_node.py
deleted file mode 100644
index 43544c1..0000000
--- a/src/robot_perception/test/test_lidar_sector_node.py
+++ /dev/null
@@ -1,85 +0,0 @@
-"""In-process test of lidar_sector_node with a fake PointCloud2 and a static TF."""
-
-import math
-import time
-
-import numpy as np
-import pytest
-
-rclpy = pytest.importorskip('rclpy')
-SingleThreadedExecutor = pytest.importorskip('rclpy.executors').SingleThreadedExecutor
-sensor_msgs = pytest.importorskip('sensor_msgs.msg')
-TransformStamped = pytest.importorskip('geometry_msgs.msg').TransformStamped
-tf2_ros = pytest.importorskip('tf2_ros')
-SectorScan = pytest.importorskip('robot_interfaces.msg').SectorScan
-LidarSectorNode = pytest.importorskip('robot_perception.lidar_sector_node').LidarSectorNode
-
-
-def cloud(points: np.ndarray) -> 'sensor_msgs.PointCloud2':
-    """Pack Nx3 float32 points into a PointCloud2 in livox_frame."""
-    msg = sensor_msgs.PointCloud2()
-    msg.header.frame_id = 'livox_frame'
-    msg.height, msg.width = 1, points.shape[0]
-    msg.fields = [
-        sensor_msgs.PointField(name=n, offset=4 * i, datatype=7, count=1)
-        for i, n in enumerate('xyz')
-    ]
-    msg.point_step, msg.row_step = 12, 12 * points.shape[0]
-    msg.data = points.astype(np.float32).tobytes()
-    return msg
-
-
-@pytest.fixture
-def rig():
-    """Run the node (PointCloud2 input) with a static base_link->livox_frame TF and recorders."""
-    rclpy.init(args=['--ros-args', '-p', 'input_type:=pointcloud2', '-p',
-                     'pointcloud_topic:=/test/cloud', '-p', 'publish_markers:=false'])
-    node = LidarSectorNode()
-    helper = rclpy.create_node('sector_test_helper')
-    tf = tf2_ros.StaticTransformBroadcaster(helper)
-    t = TransformStamped()
-    t.header.frame_id, t.child_frame_id = 'base_link', 'livox_frame'
-    t.transform.translation.z = 0.2
-    t.transform.rotation.w = 1.0
-    tf.sendTransform(t)
-    scans = {'front': [], 'safety': []}
-    helper.create_subscription(SectorScan, 'perception/sector_scan', scans['front'].append, 10)
-    helper.create_subscription(SectorScan, 'perception/safety_scan', scans['safety'].append, 10)
-    pub = helper.create_publisher(sensor_msgs.PointCloud2, '/test/cloud', 10)
-    executor = SingleThreadedExecutor()
-    executor.add_node(node)
-    executor.add_node(helper)
-
-    def run(points, seconds=1.0):
-        end = time.monotonic() + seconds
-        while time.monotonic() < end:
-            pub.publish(cloud(points))
-            for _ in range(10):
-                executor.spin_once(timeout_sec=0.01)
-        return scans
-
-    yield run
-    executor.shutdown()
-    helper.destroy_node()
-    node.destroy_node()
-    rclpy.shutdown()
-
-
-def test_front_and_safety_scans(rig):
-    """A box 1 m ahead and 1.5 m behind: front scan sees one, the 360 deg scan sees both."""
-    rng = np.random.default_rng(1)
-    clutter = rng.uniform(-3.0, 3.0, (500, 3)) * [1, 1, 0] + [0, 0, 3.0]  # ceiling, filtered
-    ahead = np.array([[1.0, y, 0.0] for y in np.linspace(-0.1, 0.1, 5)])
-    behind = np.array([[-1.5, y, 0.0] for y in np.linspace(-0.1, 0.1, 5)])
-    scans = rig(np.vstack([clutter, ahead, behind]).astype(np.float32))
-    front, safety = scans['front'][-1], scans['safety'][-1]
-    assert min(front.ranges) == pytest.approx(1.0, abs=0.01)
-    assert safety.num_sectors == 72 and safety.angle_min == pytest.approx(-math.pi)
-    hits = sorted(r for r in safety.ranges if r < safety.range_max)
-    assert hits[0] == pytest.approx(1.0, abs=0.01) and hits[-1] == pytest.approx(1.5, abs=0.01)
-
-
-def test_blind_lidar_blocks_every_safety_sector(rig):
-    """Fewer than min_raw_points points: the safety scan reports everything blocked."""
-    scans = rig(np.array([[2.0, 0.0, 0.0]] * 10, dtype=np.float32))
-    assert set(scans['safety'][-1].ranges) == {0.0}
diff --git a/src/robot_perception/test/test_livox_cdr.py b/src/robot_perception/test/test_livox_cdr.py
deleted file mode 100644
index 549299a..0000000
--- a/src/robot_perception/test/test_livox_cdr.py
+++ /dev/null
@@ -1,113 +0,0 @@
-"""Livox CustomMsg CDR parsing, checked against a hand-packed buffer and against rosbags."""
-
-import struct
-
-import numpy as np
-import pytest
-from robot_perception.livox_cdr import parse_custom_msg
-
-POINTS = np.array(
-    [[1.0, 2.0, 3.0], [0.0, 0.0, 0.0], [-4.5, 0.25, -0.5], [np.nan, 1.0, 1.0], [7.0, -8.0, 0.1]],
-    dtype=np.float32,
-)
-VALID = POINTS[[0, 2, 4]]
-
-
-def pad(buf: bytearray, size: int):
-    """Pad buf with zeros to a multiple of size (offsets from the end of the 4-byte header)."""
-    buf.extend(b'\x00' * (-(len(buf) - 4) % size))
-
-
-def pack(points: np.ndarray, frame_id: str = 'livox_frame', sec: int = 12, nanosec: int = 34):
-    """Serialize a CustomMsg by hand, the way rmw_fastrtps does (XCDR1, little endian)."""
-    name = frame_id.encode() + b'\x00'
-    buf = bytearray(b'\x00\x01\x00\x00')
-    buf += struct.pack('<iII', sec, nanosec, len(name)) + name
-    pad(buf, 8)
-    buf += struct.pack('<Q', 42)
-    buf += struct.pack('<IB3x', len(points), 1)
-    buf += struct.pack('<I', len(points))
-    for i, (x, y, z) in enumerate(points):
-        pad(buf, 4)
-        buf += struct.pack('<IfffBBB', i, x, y, z, 5, 0, 1)
-    return bytes(buf)
-
-
-def test_header_and_points():
-    """Stamp, frame and points come back; points at the origin or with NaN are dropped."""
-    sec, nanosec, frame_id, xyz = parse_custom_msg(pack(POINTS))
-    assert (sec, nanosec, frame_id) == (12, 34, 'livox_frame')
-    assert xyz.dtype == np.float32 and xyz.shape == (3, 3)
-    np.testing.assert_array_equal(xyz, VALID)
-
-
-@pytest.mark.parametrize('frame_id', ['a', 'abc', 'abcd', 'lidar_frame_x'])
-def test_frame_name_length_moves_the_alignment(frame_id):
-    """Every frame_id length lands the uint64 timebase and the points on their alignment."""
-    _, _, parsed, xyz = parse_custom_msg(pack(POINTS, frame_id))
-    assert parsed == frame_id
-    np.testing.assert_array_equal(xyz, VALID)
-
-
-def test_empty_cloud():
-    """A scan without points parses to an empty Nx3 array."""
-    _, _, _, xyz = parse_custom_msg(pack(POINTS[:0]))
-    assert xyz.shape == (0, 3)
-
-
-@pytest.mark.parametrize(
-    'data',
-    [
-        b'',
-        b'\x00\x01',
-        b'\x01\x01\x00\x00' + pack(POINTS)[4:],
-        pack(POINTS)[:-5],
-        pack(POINTS)[:30],
-    ],
-)
-def test_broken_buffers_are_refused(data):
-    """Wrong encapsulation or a truncated buffer returns None instead of garbage."""
-    assert parse_custom_msg(data) is None
-
-
-def test_matches_rosbags_serializer():
-    """Parity with an independent CDR serializer (skipped when rosbags is not installed)."""
-    typesys = pytest.importorskip('rosbags.typesys')
-    store = typesys.get_typestore(typesys.Stores.ROS2_HUMBLE)
-    store.register(
-        {
-            **typesys.get_types_from_msg(
-                'uint32 offset_time\nfloat32 x\nfloat32 y\nfloat32 z\n'
-                'uint8 reflectivity\nuint8 tag\nuint8 line',
-                'livox_ros_driver2/msg/CustomPoint',
-            ),
-            **typesys.get_types_from_msg(
-                'std_msgs/Header header\nuint64 timebase\nuint32 point_num\nuint8 lidar_id\n'
-                'uint8[3] rsvd\nlivox_ros_driver2/CustomPoint[] points',
-                'livox_ros_driver2/msg/CustomMsg',
-            ),
-        }
-    )
-    t = store.types
-    points = [
-        t['livox_ros_driver2/msg/CustomPoint'](
-            offset_time=i, x=float(x), y=float(y), z=float(z), reflectivity=5, tag=0, line=1
-        )
-        for i, (x, y, z) in enumerate(POINTS)
-    ]
-    header = t['std_msgs/msg/Header'](
-        stamp=t['builtin_interfaces/msg/Time'](sec=1700000000, nanosec=999), frame_id='livox_frame'
-    )
-    msg = t['livox_ros_driver2/msg/CustomMsg'](
-        header=header,
-        timebase=42,
-        point_num=len(points),
-        lidar_id=1,
-        rsvd=np.zeros(3, dtype=np.uint8),
-        points=points,
-    )
-    data = bytes(store.serialize_cdr(msg, 'livox_ros_driver2/msg/CustomMsg'))
-    assert data == pack(POINTS, sec=1700000000, nanosec=999)  # the hand packer above is right
-    sec, nanosec, frame_id, xyz = parse_custom_msg(data)
-    assert (sec, nanosec, frame_id) == (1700000000, 999, 'livox_frame')
-    np.testing.assert_array_equal(xyz, VALID)
diff --git a/src/robot_perception/test/test_node_members.py b/src/robot_perception/test/test_node_members.py
deleted file mode 100644
index 95a734e..0000000
--- a/src/robot_perception/test/test_node_members.py
+++ /dev/null
@@ -1,105 +0,0 @@
-"""
-Node subclasses must not shadow rclpy.node.Node members.
-
-rclpy keeps its own state in attributes such as ``handle``, ``context`` or ``_clock``. A subclass
-that defines a method or attribute with one of those names breaks the node at runtime, e.g.
-``AttributeError: __enter__`` (a method called ``handle``) or
-``'function' object has no attribute 'handle'`` (a method called ``context``).
-"""
-
-import ast
-from pathlib import Path
-
-# Set by rclpy.node.Node.__init__ (Humble and Jazzy), not visible through dir(Node).
-NODE_INTERNALS = {
-    '_allow_undeclared_parameters',
-    '_clock',
-    '_context',
-    '_default_callback_group',
-    '_descriptors',
-    '_logger',
-    '_parameter_event_publisher',
-    '_parameter_overrides',
-    '_parameter_service',
-    '_parameters',
-    '_rate_group',
-    '_time_source',
-}
-# Public members, kept here so the test also works without ROS installed.
-NODE_PUBLIC = {
-    'clients',
-    'context',
-    'default_callback_group',
-    'executor',
-    'guards',
-    'handle',
-    'publishers',
-    'services',
-    'subscriptions',
-    'timers',
-    'waitables',
-}
-# Members a subclass may override on purpose (always calling super()).
-ALLOWED = {'destroy_node'}
-
-
-def node_members() -> set:
-    """Return every name a Node subclass must not redefine."""
-    names = NODE_PUBLIC | NODE_INTERNALS
-    try:
-        from rclpy.node import Node
-    except ImportError:
-        return names - ALLOWED
-    return (names | {n for n in dir(Node) if not n.startswith('__')}) - ALLOWED
-
-
-def is_node_class(cls: ast.ClassDef) -> bool:
-    """Return True for `class X(Node)` and `class X(rclpy.node.Node)`."""
-    for base in cls.bases:
-        name = base.attr if isinstance(base, ast.Attribute) else getattr(base, 'id', '')
-        if name == 'Node':
-            return True
-    return False
-
-
-def defined_names(cls: ast.ClassDef):
-    """Yield (name, line) for methods and self.<name> assignments of a class."""
-    for item in cls.body:
-        if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef)):
-            yield item.name, item.lineno
-        elif isinstance(item, ast.Assign):
-            for target in item.targets:
-                if isinstance(target, ast.Name):
-                    yield target.id, item.lineno
-    for node in ast.walk(cls):
-        targets = []
-        if isinstance(node, ast.Assign):
-            targets = node.targets
-        elif isinstance(node, (ast.AnnAssign, ast.AugAssign)):
-            targets = [node.target]
-        for target in targets:
-            for t in ast.walk(target):
-                if (
-                    isinstance(t, ast.Attribute)
-                    and isinstance(t.value, ast.Name)
-                    and t.value.id == 'self'
-                ):
-                    yield t.attr, node.lineno
-
-
-def test_no_shadowed_node_members():
-    """Every Node subclass in this package keeps rclpy's attributes intact."""
-    forbidden = node_members()
-    package = Path(__file__).resolve().parents[1]
-    problems = []
-    for path in sorted(package.glob('*/**/*.py')):
-        if 'test' in path.relative_to(package).parts[:1]:
-            continue
-        tree = ast.parse(path.read_text(), filename=str(path))
-        for cls in (n for n in ast.walk(tree) if isinstance(n, ast.ClassDef)):
-            if not is_node_class(cls):
-                continue
-            for name, line in defined_names(cls):
-                if name in forbidden:
-                    problems.append(f'{path.name}:{line} {cls.name}.{name} shadows rclpy Node')
-    assert not problems, '\n'.join(sorted(set(problems)))
diff --git a/src/robot_perception/test/test_pep257.py b/src/robot_perception/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/src/robot_perception/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/robot_perception/test/test_sectors.py b/src/robot_perception/test/test_sectors.py
deleted file mode 100644
index 8279914..0000000
--- a/src/robot_perception/test/test_sectors.py
+++ /dev/null
@@ -1,48 +0,0 @@
-import math
-
-import numpy as np
-from robot_perception.sectors import bin_ranges, obstacle_points
-
-BOX = (-0.4, 0.4, -0.28, 0.28)
-
-
-def test_obstacle_points_drop_floor_ceiling_and_body():
-    """Only points between z_min and z_max and outside the robot box remain."""
-    pts = np.array(
-        [
-            [2.0, 0.0, 0.0],   # obstacle
-            [2.0, 0.0, -0.5],  # floor
-            [2.0, 0.0, 2.0],   # ceiling
-            [0.1, 0.1, 0.0],   # own leg
-            [-1.0, 1.0, 0.5],  # obstacle behind-left
-        ],
-        dtype=np.float32,
-    )
-    xy = obstacle_points(pts, -0.25, 1.2, BOX)
-    assert xy.tolist() == [[2.0, 0.0], [-1.0, 1.0]]
-    assert obstacle_points(np.empty((0, 3)), -0.25, 1.2, BOX).shape == (0, 2)
-
-
-def test_front_scan_keeps_nearest_per_sector():
-    """Front fov: nearest range wins, points outside the fov are ignored."""
-    xy = np.array([[2.0, 0.0], [1.5, 0.01], [-3.0, 0.0], [0.0, 2.0]], dtype=np.float32)
-    ranges = bin_ranges(xy, -math.pi / 2, math.pi, 4, 0.1, 8.0)
-    # sectors: [-90,-45) [-45,0) [0,45) [45,90); (0, 2) is exactly at +90 -> outside
-    assert ranges[2] == np.float32(np.hypot(1.5, 0.01))
-    assert ranges[0] == ranges[1] == ranges[3] == 8.0
-
-
-def test_full_circle_wraps_behind():
-    """The 360 deg scan sees obstacles behind the robot, at +-180 deg too."""
-    xy = np.array([[-1.0, 0.001], [-1.2, -0.001], [0.0, -2.0]], dtype=np.float32)
-    ranges = bin_ranges(xy, -math.pi, 2 * math.pi, 8, 0.1, 4.0)
-    assert ranges[7] == np.float32(np.hypot(-1.0, 0.001))  # just below +180
-    assert ranges[0] == np.float32(np.hypot(-1.2, 0.001))  # just above -180
-    assert ranges[2] == 2.0  # -90 deg
-    assert np.count_nonzero(ranges < 4.0) == 3
-
-
-def test_ranges_outside_limits_are_ignored():
-    """Points closer than range_min or beyond range_max do not count."""
-    xy = np.array([[0.05, 0.0], [9.0, 0.0]], dtype=np.float32)
-    assert (bin_ranges(xy, -math.pi, 2 * math.pi, 8, 0.1, 4.0) == 4.0).all()
diff --git a/src/robot_rviz/CMakeLists.txt b/src/robot_rviz/CMakeLists.txt
index 45d51df..3469bc4 100644
--- a/src/robot_rviz/CMakeLists.txt
+++ b/src/robot_rviz/CMakeLists.txt
@@ -15,7 +15,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
-find_package(robot_interfaces REQUIRED)
+find_package(dharma_interfaces REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
 set(CMAKE_AUTOMOC ON)
@@ -51,7 +51,7 @@ ament_target_dependencies(${PROJECT_NAME}
   std_msgs
   geometry_msgs
   diagnostic_msgs
-  robot_interfaces
+  dharma_interfaces
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -81,6 +81,6 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(
   rclcpp rviz_common pluginlib std_srvs sensor_msgs std_msgs geometry_msgs diagnostic_msgs
-  robot_interfaces)
+  dharma_interfaces)
 
 ament_package()
\ No newline at end of file
diff --git a/src/robot_rviz/include/robot_rviz/control_panel.hpp b/src/robot_rviz/include/robot_rviz/control_panel.hpp
index bbac284..b0ce648 100644
--- a/src/robot_rviz/include/robot_rviz/control_panel.hpp
+++ b/src/robot_rviz/include/robot_rviz/control_panel.hpp
@@ -8,10 +8,10 @@
 #include <QLineEdit>
 #include <QPushButton>
 
-#include <robot_interfaces/msg/mission_status.hpp>
-#include <robot_interfaces/msg/navigation_status.hpp>
-#include <robot_interfaces/srv/load_path.hpp>
-#include <robot_interfaces/srv/mark_stop_point.hpp>
+#include <dharma_interfaces/msg/mission_status.hpp>
+#include <dharma_interfaces/msg/navigation_status.hpp>
+#include <dharma_interfaces/srv/load_path.hpp>
+#include <dharma_interfaces/srv/mark_stop_point.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -42,8 +42,8 @@ private Q_SLOTS:
   void onTogglePause();
 
 private:
-  void navStatusCallback(const robot_interfaces::msg::NavigationStatus::SharedPtr msg);
-  void missionStatusCallback(const robot_interfaces::msg::MissionStatus::SharedPtr msg);
+  void navStatusCallback(const dharma_interfaces::msg::NavigationStatus::SharedPtr msg);
+  void missionStatusCallback(const dharma_interfaces::msg::MissionStatus::SharedPtr msg);
   void localizationStatusCallback(const std_msgs::msg::String::SharedPtr msg);
 
   // Localization
@@ -59,14 +59,14 @@ private Q_SLOTS:
   QLabel * recording_status_label_;
   bool is_recording_{false};
   rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr record_client_;
-  rclcpp::Client<robot_interfaces::srv::MarkStopPoint>::SharedPtr mark_stop_point_client_;
+  rclcpp::Client<dharma_interfaces::srv::MarkStopPoint>::SharedPtr mark_stop_point_client_;
 
   // Route loading
   QLineEdit * waypoints_file_edit_;
   QPushButton * browse_button_;
   QPushButton * load_button_;
   QLabel * load_status_label_;
-  rclcpp::Client<robot_interfaces::srv::LoadPath>::SharedPtr load_path_client_;
+  rclcpp::Client<dharma_interfaces::srv::LoadPath>::SharedPtr load_path_client_;
 
   // Navigation
   QPushButton * nav_button_;
@@ -77,8 +77,8 @@ private Q_SLOTS:
   bool is_paused_{false};
   rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr nav_client_;
   rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr pause_client_;
-  rclcpp::Subscription<robot_interfaces::msg::NavigationStatus>::SharedPtr nav_status_sub_;
-  rclcpp::Subscription<robot_interfaces::msg::MissionStatus>::SharedPtr mission_status_sub_;
+  rclcpp::Subscription<dharma_interfaces::msg::NavigationStatus>::SharedPtr nav_status_sub_;
+  rclcpp::Subscription<dharma_interfaces::msg::MissionStatus>::SharedPtr mission_status_sub_;
 };
 
 }  // namespace robot_rviz
diff --git a/src/robot_rviz/package.xml b/src/robot_rviz/package.xml
index 86dbd65..28dacc2 100644
--- a/src/robot_rviz/package.xml
+++ b/src/robot_rviz/package.xml
@@ -17,7 +17,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
-  <depend>robot_interfaces</depend>
+  <depend>dharma_interfaces</depend>
   <depend>qtbase5-dev</depend>
 
   <export>
diff --git a/src/robot_rviz/src/control_panel.cpp b/src/robot_rviz/src/control_panel.cpp
index 6d7fd7f..6a7fcf6 100644
--- a/src/robot_rviz/src/control_panel.cpp
+++ b/src/robot_rviz/src/control_panel.cpp
@@ -92,8 +92,8 @@ void ControlPanel::setupRos()
   localization_client_ = node_->create_client<std_srvs::srv::Trigger>("localization/start");
   record_client_ = node_->create_client<std_srvs::srv::SetBool>("route/record");
   mark_stop_point_client_ =
-    node_->create_client<robot_interfaces::srv::MarkStopPoint>("route/mark_stop_point");
-  load_path_client_ = node_->create_client<robot_interfaces::srv::LoadPath>("navigation/load_path");
+    node_->create_client<dharma_interfaces::srv::MarkStopPoint>("route/mark_stop_point");
+  load_path_client_ = node_->create_client<dharma_interfaces::srv::LoadPath>("navigation/load_path");
   nav_client_ = node_->create_client<std_srvs::srv::SetBool>("navigation/start_nav");
   pause_client_ = node_->create_client<std_srvs::srv::SetBool>("navigation/pause");
 
@@ -101,10 +101,10 @@ void ControlPanel::setupRos()
   localization_status_sub_ = node_->create_subscription<std_msgs::msg::String>(
     "localization/status", latched,
     std::bind(&ControlPanel::localizationStatusCallback, this, std::placeholders::_1));
-  nav_status_sub_ = node_->create_subscription<robot_interfaces::msg::NavigationStatus>(
+  nav_status_sub_ = node_->create_subscription<dharma_interfaces::msg::NavigationStatus>(
     "navigation/status", 10,
     std::bind(&ControlPanel::navStatusCallback, this, std::placeholders::_1));
-  mission_status_sub_ = node_->create_subscription<robot_interfaces::msg::MissionStatus>(
+  mission_status_sub_ = node_->create_subscription<dharma_interfaces::msg::MissionStatus>(
     "navigation/mission_status", 10,
     std::bind(&ControlPanel::missionStatusCallback, this, std::placeholders::_1));
 }
@@ -151,11 +151,11 @@ void ControlPanel::onMarkStopPoint()
     setStatus(recording_status_label_, "route/mark_stop_point not available", "#ffcc00");
     return;
   }
-  auto request = std::make_shared<robot_interfaces::srv::MarkStopPoint::Request>();
+  auto request = std::make_shared<dharma_interfaces::srv::MarkStopPoint::Request>();
   request->dwell_sec = static_cast<float>(dwell_spinbox_->value());
   mark_stop_point_client_->async_send_request(
     request,
-    [this](rclcpp::Client<robot_interfaces::srv::MarkStopPoint>::SharedFuture future) {
+    [this](rclcpp::Client<dharma_interfaces::srv::MarkStopPoint>::SharedFuture future) {
       auto response = future.get();
       setStatus(
         recording_status_label_, response->message, response->success ? "#88ff88" : "#ff8888");
@@ -177,11 +177,11 @@ void ControlPanel::onLoadPath()
     setStatus(load_status_label_, "navigation/load_path not available", "#ffcc00");
     return;
   }
-  auto request = std::make_shared<robot_interfaces::srv::LoadPath::Request>();
+  auto request = std::make_shared<dharma_interfaces::srv::LoadPath::Request>();
   request->waypoints_file = waypoints_file_edit_->text().toStdString();
   setStatus(load_status_label_, "loading...", "#ffffff");
   load_path_client_->async_send_request(
-    request, [this](rclcpp::Client<robot_interfaces::srv::LoadPath>::SharedFuture future) {
+    request, [this](rclcpp::Client<dharma_interfaces::srv::LoadPath>::SharedFuture future) {
       auto response = future.get();
       setStatus(load_status_label_, response->message, response->success ? "#88ff88" : "#ff8888");
     });
@@ -216,7 +216,7 @@ void ControlPanel::onTogglePause()
     request, [](rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture) {});
 }
 
-void ControlPanel::navStatusCallback(const robot_interfaces::msg::NavigationStatus::SharedPtr msg)
+void ControlPanel::navStatusCallback(const dharma_interfaces::msg::NavigationStatus::SharedPtr msg)
 {
   const std::string & s = msg->state;
   std::string color = "#cccccc";
@@ -243,7 +243,7 @@ void ControlPanel::navStatusCallback(const robot_interfaces::msg::NavigationStat
   setStatus(nav_status_label_, s + ": " + msg->message, color);
 }
 
-void ControlPanel::missionStatusCallback(const robot_interfaces::msg::MissionStatus::SharedPtr msg)
+void ControlPanel::missionStatusCallback(const dharma_interfaces::msg::MissionStatus::SharedPtr msg)
 {
   mission_label_->setText(QString::fromStdString(msg->active_behavior));
 }
diff --git a/src/robot_sim/test/test_sim_patrol.py b/src/robot_sim/test/test_sim_patrol.py
index f766ed4..5b1b078 100644
--- a/src/robot_sim/test/test_sim_patrol.py
+++ b/src/robot_sim/test/test_sim_patrol.py
@@ -22,8 +22,8 @@
 qos = pytest.importorskip('rclpy.qos')
 geometry_msgs = pytest.importorskip('geometry_msgs.msg')
 std_msgs = pytest.importorskip('std_msgs.msg')
-StopPointEvent = pytest.importorskip('robot_interfaces.msg').StopPointEvent
-SetMode = pytest.importorskip('robot_interfaces.srv').SetMode
+StopPointEvent = pytest.importorskip('dharma_interfaces.msg').StopPointEvent
+SetMode = pytest.importorskip('dharma_interfaces.srv').SetMode
 
 pytestmark = pytest.mark.skipif(
     os.environ.get('PASHUPATI_SKIP_SIM') == '1', reason='PASHUPATI_SKIP_SIM=1'