feat(webui): waypoint editor with click-to-navigate (Issue #261)

Adds multi-pass spatial-Gaussian hole filler for D435i depth images.
Each pass replaces zero/NaN pixels with the Gaussian-weighted mean of valid
neighbours in a growing kernel (×1, ×2.5, ×6 default); original valid
pixels are never modified.  Handles uint16 mm → float32 m conversion,
border pixels via BORDER_REFLECT, and above-d_max pixels as holes.
Publishes filled float32 depth on /camera/depth/filled at camera rate.
37/37 pure-Python tests pass (no ROS2 required).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

.claude/plans/floofy-forging-hellman.md

@@ -0,0 +1,208 @@
+# Issue #194 — Speed Limiter Node Implementation Plan
+
+## Context
+
+The SaltyBot control stack currently lacks a dedicated speed limiting aggregator. Individual subsystems publish speed scale factors:
+- **TerrainAdaptationNode** → `/saltybot/terrain_speed_scale` (Float32, 0.15–1.0)
+- **EmergencyNode** → `/saltybot/emergency` (EmergencyEvent with severity level)
+- Hypothetical external source → `/saltybot/speed_limit` (to be determined)
+
+These must be unified into a single authoritative speed scale and applied to `/cmd_vel` before forwarding to the drive system. Currently, each subsystem is independent, creating potential conflicts and inconsistent behavior.
+
+## Goal
+
+Create `saltybot_speed_limiter` ROS2 package that:
+1. Subscribes to terrain_speed_scale, speed_limit, and emergency signals
+2. Computes effective minimum speed scale from all three inputs
+3. Applies scale to /cmd_vel messages → /saltybot/cmd_vel_limited
+4. Publishes diagnostic JSON on /saltybot/speed_limit_info
+5. Operates at 50 Hz with proper message synchronization
+
+## Architecture
+
+### Subscriptions
+
+| Topic | Type | Source | Frequency | Use |
+|-------|------|--------|-----------|-----|
+| `/saltybot/terrain_speed_scale` | Float32 | TerrainAdaptationNode | 50Hz | Terrain-based scaling (0.15–1.0) |
+| `/saltybot/speed_limit` | Float32 | External (TBD) | ≥10Hz | Global speed limit (0.0–1.0) |
+| `/saltybot/emergency` | EmergencyEvent | EmergencyNode | 20Hz | Emergency severity level |
+| `/cmd_vel` | Twist | SpeedControllerNode | ≥20Hz | Raw commanded velocity |
+
+### Outputs
+
+| Topic | Type | Frequency | Content |
+|-------|------|-----------|---------|
+| `/saltybot/cmd_vel_limited` | Twist | 50Hz | Scaled cmd_vel |
+| `/saltybot/speed_limit_info` | String (JSON) | 50Hz | Diagnostic state |
+
+### Message Types
+
+**EmergencyEvent** (from saltybot_emergency_msgs):
+```
+string severity  # "CLEAR"|"MINOR"|"MAJOR"|"CRITICAL"
+```
+
+**speed_limit_info JSON**:
+```json
+{
+  "terrain_scale": 0.638,
+  "speed_limit": 1.0,
+  "emergency_scale": 1.0,
+  "effective_scale": 0.638,
+  "cmd_vel_input": {"linear": 1.0, "angular": 0.5},
+  "cmd_vel_limited": {"linear": 0.638, "angular": 0.319},
+  "timestamp": 1740000000.123456
+}
+```
+
+### Control Logic
+
+```python
+# Emergency severity → speed scale mapping
+emergency_scale = {
+    "CLEAR": 1.0,
+    "MINOR": 0.95,      # Yellow alert: slight caution
+    "MAJOR": 0.7,       # Orange alert: significant slowdown
+    "CRITICAL": 0.3,    # Red alert: severe limitation
+}
+
+# Effective scale = minimum of all sources
+effective_scale = min(
+    terrain_scale,           # From /saltybot/terrain_speed_scale
+    speed_limit,             # From /saltybot/speed_limit
+    emergency_scale,         # From /saltybot/emergency severity
+)
+
+# Apply to all cmd_vel components
+cmd_vel_limited.linear.x *= effective_scale
+cmd_vel_limited.linear.y *= effective_scale
+cmd_vel_limited.linear.z *= effective_scale
+cmd_vel_limited.angular.x *= effective_scale
+cmd_vel_limited.angular.y *= effective_scale
+cmd_vel_limited.angular.z *= effective_scale
+```
+
+## Implementation Details
+
+### Package Structure
+
+```
+saltybot_speed_limiter/
+├── CMakeLists.txt
+├── package.xml
+├── setup.py
+├── README.md
+├── launch/
+│   └── speed_limiter.launch.py
+├── resource/
+│   └── saltybot_speed_limiter
+├── saltybot_speed_limiter/
+│   ├── __init__.py
+│   └── speed_limiter_node.py
+├── config/
+│   └── speed_limiter_params.yaml
+└── test/
+    ├── __init__.py
+    └── test_speed_limiter.py
+```
+
+### Key Implementation Points
+
+1. **Message Synchronization**: Use `message_filters.ApproximateTimeSynchronizer` to align terrain_scale, speed_limit, emergency, and cmd_vel with small slop tolerance (50–100ms @ 50Hz)
+
+2. **State Management**:
+   - Store latest values for each input (timeout handling if stale)
+   - Fallback to safe defaults if messages stop arriving:
+     - terrain_scale → 1.0 (no scaling)
+     - speed_limit → 1.0 (no limit)
+     - emergency → "CLEAR" (no threat)
+
+3. **Parameters**:
+   - `publish_hz`: 50.0 (fixed, not tunable)
+   - `sync_slop_ms`: 100 (ApproximateTimeSynchronizer slop)
+   - `timeout_s`: 2.0 (signal timeout)
+   - `frame_id`: 'base_link'
+   - Emergency scale map (tunable per severity)
+
+4. **Performance**:
+   - Small package, minimal computation (3 float comparisons + geometry scaling)
+   - No heavy libraries (just numpy for Twist scaling)
+
+### Unit Tests
+
+Test file: `test/test_speed_limiter.py`
+
+Coverage:
+- Min-of-three logic (all combinations)
+- Twist scaling on all 6 components
+- Emergency severity mapping (CLEAR, MINOR, MAJOR, CRITICAL)
+- Timeout/stale handling (graceful fallback)
+- JSON output structure and values
+- Message synchronization edge cases
+
+### Integration Points
+
+**Upstream** (inputs):
+- SpeedControllerNode → `/cmd_vel`
+- TerrainAdaptationNode → `/saltybot/terrain_speed_scale`
+- EmergencyNode → `/saltybot/emergency`
+
+**Downstream** (outputs):
+- CmdVelBridgeNode ← `/saltybot/cmd_vel_limited` (replaces `/cmd_vel`)
+
+**Migration Path**:
+1. CmdVelBridgeNode currently reads `/cmd_vel`
+2. After speed_limiter is ready: modify CmdVelBridgeNode to read `/saltybot/cmd_vel_limited` instead
+3. Or: use ROS2 topic remapping in launch files during transition
+
+## Files to Create
+
+1. **saltybot_speed_limiter/speed_limiter_node.py** (~200 lines)
+   - SpeedLimiterNode class
+   - Message synchronization with fallback logic
+   - Scale computation and Twist application
+   - JSON diagnostic publishing
+
+2. **test/test_speed_limiter.py** (~150 lines)
+   - Min-of-three logic tests
+   - Twist scaling verification
+   - Emergency map tests
+   - Timeout/stale handling tests
+
+3. **launch/speed_limiter.launch.py** (~40 lines)
+   - Configurable parameters
+   - QoS setup
+
+4. **config/speed_limiter_params.yaml** (~20 lines)
+   - Emergency severity scales
+   - Timeout and sync parameters
+
+5. **package.xml, setup.py, CMakeLists.txt, README.md, resource/** (~150 lines total)
+   - Standard ROS2 package boilerplate
+
+## Verification Plan
+
+1. **Unit Tests**: Run pytest on test_speed_limiter.py (expect 15+ tests, all pass)
+2. **Package Build**: `colcon build --packages-select saltybot_speed_limiter`
+3. **Node Launch**: `ros2 launch saltybot_speed_limiter speed_limiter.launch.py`
+4. **Manual Testing** (hardware):
+   - Subscribe to /saltybot/cmd_vel_limited and verify scaling
+   - Inject different terrain_scale, speed_limit, emergency values
+   - Confirm effective_scale = min(all three)
+   - Verify JSON diagnostic content
+
+## Branch & Commit Strategy
+
+1. Branch: `sl-controls/issue-194-speed-limiter` from main
+2. Commit: Single commit with all 10 files
+3. PR: To main branch with detailed description
+4. MQTT Report: Send status to max after merge
+
+## Key Decisions
+
+- **Min-of-three logic** (not weighted average): Ensures safety — most conservative limit wins
+- **Separate topic output** (`/saltybot/cmd_vel_limited`): Preserves original /cmd_vel for debugging
+- **Emergency severity levels** (CLEAR/MINOR/MAJOR/CRITICAL): Map to scales 1.0/0.95/0.7/0.3 (can tune)
+- **ApproximateTimeSynchronizer**: Handles async inputs at different rates
+- **JSON string output** (not typed message): Easy to parse in logging/MQTT system

.claude/plans/humming-watching-toast.md

@@ -0,0 +1,140 @@
+# Plan: SaltyRover STM32 Firmware Variant
+
+## Context
+SaltyRover is the 4-wheel-drive variant of SaltyBot. It uses the same STM32F722 MCU, CRSF RC, USB CDC, safety systems, and IMU — but replaces the balance PID loop with direct 4-wheel independent motor commands. Two hoverboard ESCs are used: front axle on USART2 (same as balance bot), rear axle on UART5 (PC12/PD2).
+
+saltyrover-dev is 16 commits behind main and lacks: mode_manager, remote e-stop (PR #69), jetson_cmd updates, and some CDC additions. We'll apply all relevant current main-equivalent features on the new branch.
+
+## Branch
+- Source: `origin/saltyrover-dev`
+- Branch name: `sl-firmware/rover-firmware`
+- PR target: `saltyrover-dev`
+
+## Files to Create
+
+### `include/rover_driver.h`
+Public API for the 4-wheel rover motor layer:
+```c
+typedef struct {
+    int16_t fl, fr, rl, rr; /* last wheel commands (-1000..+1000) */
+    bool    armed;
+    bool    estop;
+    uint32_t last_cmd_ms;   /* HAL_GetTick() of last set_cmd() call */
+} rover_driver_t;
+
+void rover_driver_init(rover_driver_t *r);
+void rover_driver_set_cmd(rover_driver_t *r, int16_t fl, int16_t fr, int16_t rl, int16_t rr, uint32_t now);
+void rover_driver_update(rover_driver_t *r, uint32_t now); /* call at 50Hz */
+void rover_driver_arm(rover_driver_t *r);
+void rover_driver_disarm(rover_driver_t *r);
+void rover_driver_estop(rover_driver_t *r);
+void rover_driver_estop_clear(rover_driver_t *r);
+```
+
+### `src/rover_driver.c`
+Implementation — two hoverboard ESC control:
+- `rover_driver_init()`: calls existing `hoverboard_init()` (USART2 front), adds UART5 init for rear (HAL_UART_Init at 115200, PC12/PD2)
+- `rover_driver_update()` at 50Hz:
+  - estop or disarmed → send (0,0) to both ESCs
+  - armed → compute `front_spd=(fl+fr)/2`, `front_str=(fr-fl)/2`, `rear_spd=(rl+rr)/2`, `rear_str=(rr-rl)/2`; clamp each to ±ROVER_SPEED_MAX
+  - If `now - last_cmd_ms > ROVER_CMD_TIMEOUT_MS` → zero commands (safety fallback)
+  - Calls `hoverboard_send(front_spd, front_str)` for USART2
+  - Calls local `hoverboard2_send(rear_spd, rear_str)` for UART5 (static function in rover_driver.c, same 8-byte protocol)
+
+## Files to Modify
+
+### `include/config.h`
+Add rover-specific constants section:
+```c
+/* --- SaltyRover 4WD --- */
+#define ROVER_MAX_TILT_DEG   45.0f  /* ESC estop angle (vs 25° balance cutoff) */
+#define ROVER_CMD_TIMEOUT_MS  500   /* Zero wheels if no Jetson cmd for 500ms */
+#define ROVER_SPEED_MAX       800   /* Max per-wheel command (ESC units) */
+```
+
+### `include/safety.h` + `src/safety.c`
+Apply PR #69 remote-estop additions (same as main): `EstopSource` enum, `safety_remote_estop/clear/get/active()`, `s_estop_source` static. These are already on main but not saltyrover-dev — include them here.
+
+### `lib/USB_CDC/src/usbd_cdc_if.c` + `include/usbd_cdc_if.h`
+Apply PR #69 CDC additions: `cdc_estop_request`, `cdc_estop_clear_request`, cases 'E'/'F'/'Z'.
+
+Add rover JSON command handling:
+```c
+volatile uint8_t  rover_json_ready = 0;
+volatile char     rover_json_buf[80];
+// In CDC_Receive:
+case '{': {
+    uint32_t n = *len < 79 ? *len : 79;
+    for (uint32_t i = 0; i < n; i++) rover_json_buf[i] = (char)buf[i];
+    rover_json_buf[n] = '\0';
+    rover_json_ready = 1;
+    jetson_hb_tick = HAL_GetTick();  /* JSON cmd refreshes heartbeat */
+    break;
+}
+```
+
+Export in header: `extern volatile uint8_t rover_json_ready; extern volatile char rover_json_buf[80];`
+
+### `src/main.c`
+Full replacement of the main loop. Keep all init (clock, USB, IMU, CRSF, I2C sensors, safety), strip `balance_t`, replace with `rover_driver_t`. Key loop logic:
+
+**Per-iteration (1ms):**
+1. `safety_refresh()` — IWDG feed
+2. `mpu6000_read(&imu)` — IMU read
+3. `mode_manager_update(&mode, now)` — RC liveness + CH6 mode
+4. Remote e-stop block (from PR #69): `cdc_estop_request` → `rover_driver_estop()`
+5. Tilt watchdog: `if (fabsf(imu.pitch) > ROVER_MAX_TILT_DEG) rover_driver_estop()`
+6. RC CH5 arm/disarm (same hold interlock; pitch check relaxed to `< ROVER_MAX_TILT_DEG`)
+7. USB arm/disarm (A/D commands)
+8. Parse `rover_json_ready`: `sscanf(rover_json_buf, ...)` for `drive4` cmd → `rover_driver_set_cmd()`
+9. RC direct drive fallback (MANUAL mode): CH3→speed all 4 wheels, CH4→differential steer
+
+**50Hz ESC block:**
+```c
+if (rover.armed && !rover.estop) {
+    // If Jetson active: use stored fl/fr/rl/rr
+    // If RC MANUAL: compute fl=fr=rl=rr=speed, add steer diff
+    rover_driver_update(&rover, now);
+} else {
+    rover_driver_update(&rover, now); // sends zeros
+}
+```
+
+**50Hz telemetry:**
+```json
+{"p":<pitch×10>,"r":<roll×10>,"s":<armed>,"fl":<fl>,"fr":<fr>,"rl":<rl>,"rr":<rr>,"md":<mode>,"es":<es>,"txc":<N>,"rxc":<N>}
+```
+Optional fields: heading, altitude/temp/pressure (same as balance bot).
+
+**Status LEDs:** Same `status_update(now, imu_ok, rover.armed, |pitch|>ROVER_MAX_TILT_DEG, safety_remote_estop_active())`
+
+**Arm guard:** `|pitch_deg| < ROVER_MAX_TILT_DEG` (45°) instead of `< 10.0f`
+
+## Key Architectural Differences vs Balance Bot
+
+| | Balance Bot | SaltyRover |
+|---|---|---|
+| PID loop | 1kHz balance PID | None |
+| Motor cmd | `bal.motor_cmd` | Individual fl/fr/rl/rr |
+| ESCs | 1× USART2 | 2× USART2 + UART5 |
+| Tilt cutoff | 25° (balance state) | 45° (simple estop only) |
+| Drive cmd | `C<spd>,<str>\n` | `{"cmd":"drive4",...}` JSON |
+| Arm pitch guard | `< 10°` | `< 45°` |
+| State machine | DISARMED/ARMED/TILT_FAULT | armed bool + estop bool |
+
+## Test Plan
+1. Build: `pio run -e saltyrover` (or equivalent) — confirms compile
+2. Hardware: send `{"cmd":"drive4","fl":200,"fr":200,"rl":200,"rr":200}` → all 4 wheels spin forward
+3. Estop: send `E\n` → wheels stop, LED fast-blinks 200ms; `Z\n` + RC arm → resumes
+4. Tilt: tilt rover >45° → wheels cut immediately; no cut at 30°
+5. RC manual: CH5 arm + CH3/CH4 → wheels respond; CH5 disarm → stop
+6. Telemetry: verify `"fl"/"fr"/"rl"/"rr"` in JSON stream, `"es"` field changes on estop
+
+## Critical Files
+- `src/main.c` — rover main loop (full rewrite of loop body)
+- `include/rover_driver.h` — new file
+- `src/rover_driver.c` — new file (UART5 init + two-ESC update)
+- `include/config.h` — rover constants
+- `include/safety.h` + `src/safety.c` — remote estop additions
+- `lib/USB_CDC/src/usbd_cdc_if.c` — JSON '{' case + estop flags
+- `lib/USB_CDC/include/usbd_cdc_if.h` — export new flags

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_depth_hole_fill.py

@@ -0,0 +1,141 @@
+"""
+_depth_hole_fill.py — Depth image hole filling via bilateral interpolation (no ROS2 deps).
+
+Algorithm
+---------
+A "hole" is any pixel where depth == 0, depth is NaN, or depth is outside the
+valid range [d_min, d_max].
+
+Each pass replaces every hole pixel with the spatial-Gaussian-weighted mean of
+valid pixels in a (kernel_size × kernel_size) neighbourhood:
+
+    filled[x,y] = Σ G(||p - q||; σ) · d[q]  /  Σ G(||p - q||; σ)
+                  q ∈ valid neighbours of (x,y)
+
+The denominator (sum of spatial weights over valid pixels) normalises correctly
+even at image borders and around isolated valid pixels.
+
+Multiple passes with geometrically growing kernels are applied so that:
+  Pass 1  kernel_size           — fills small holes (≤ kernel_size/2 px radius)
+  Pass 2  kernel_size × 2.5    — fills medium holes
+  Pass 3  kernel_size × 6.0    — fills large holes / fronto-parallel surfaces
+
+After all passes any remaining zeros are left as-is (no valid neighbourhood data).
+
+Because only the spatial Gaussian (not a depth range term) is used as the weighting
+function, this is equivalent to a bilateral filter with σ_range → ∞.  In practice
+this produces smooth, physically plausible fills in the depth domain.
+
+Public API
+----------
+  fill_holes(depth, kernel_size=5, d_min=0.1, d_max=10.0, max_passes=3) → ndarray
+  valid_mask(depth, d_min=0.1, d_max=10.0) → bool ndarray
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Optional
+
+import numpy as np
+
+
+# Kernel size multipliers for successive passes
+_PASS_SCALE = [1.0, 2.5, 6.0]
+
+
+def valid_mask(
+    depth:  np.ndarray,
+    d_min:  float = 0.1,
+    d_max:  float = 10.0,
+) -> np.ndarray:
+    """
+    Return a boolean mask of valid (non-hole) pixels.
+
+    Parameters
+    ----------
+    depth : (H, W) float32 ndarray, depth in metres
+    d_min : minimum valid depth (m)
+    d_max : maximum valid depth (m)
+
+    Returns
+    -------
+    (H, W) bool ndarray — True where depth is finite and in [d_min, d_max]
+    """
+    return np.isfinite(depth) & (depth >= d_min) & (depth <= d_max)
+
+
+def fill_holes(
+    depth:       np.ndarray,
+    kernel_size: int   = 5,
+    d_min:       float = 0.1,
+    d_max:       float = 10.0,
+    max_passes:  int   = 3,
+) -> np.ndarray:
+    """
+    Fill zero/NaN depth pixels using multi-pass spatial Gaussian interpolation.
+
+    Parameters
+    ----------
+    depth       : (H, W) float32 ndarray, depth in metres
+    kernel_size : initial kernel side length (pixels, forced odd, ≥ 3)
+    d_min       : minimum valid depth — pixels below this are treated as holes
+    d_max       : maximum valid depth — pixels above this are treated as holes
+    max_passes  : number of fill passes (1–3); each uses a larger kernel
+
+    Returns
+    -------
+    (H, W) float32 ndarray — same as input, with holes filled where possible.
+    Pixels with no valid neighbours after all passes remain 0.0.
+    Original valid pixels are never modified.
+    """
+    import cv2
+
+    depth = np.asarray(depth, dtype=np.float32)
+    # Replace NaN with 0 so arithmetic is clean
+    depth = np.where(np.isfinite(depth), depth, 0.0).astype(np.float32)
+
+    mask = valid_mask(depth, d_min, d_max)    # True where already valid
+    result = depth.copy()
+    n_passes = max(1, min(max_passes, len(_PASS_SCALE)))
+
+    for i in range(n_passes):
+        if mask.all():
+            break   # no holes left
+
+        ks   = _odd_kernel_size(kernel_size, _PASS_SCALE[i])
+        half = ks // 2
+        sigma = max(half / 2.0, 0.5)
+
+        gk     = cv2.getGaussianKernel(ks, sigma).astype(np.float32)
+        kernel = (gk @ gk.T)
+
+        # Multiply depth by mask so invalid pixels contribute 0 weight
+        d_valid = np.where(mask, result, 0.0).astype(np.float32)
+        w_valid = mask.astype(np.float32)
+
+        sum_d = cv2.filter2D(d_valid, ddepth=-1, kernel=kernel,
+                             borderType=cv2.BORDER_REFLECT)
+        sum_w = cv2.filter2D(w_valid, ddepth=-1, kernel=kernel,
+                             borderType=cv2.BORDER_REFLECT)
+
+        # Where we have enough weight, compute the weighted mean
+        has_data = sum_w > 1e-6
+        interp   = np.where(has_data, sum_d / np.where(has_data, sum_w, 1.0), 0.0)
+
+        # Only fill holes — never overwrite original valid pixels
+        result = np.where(mask, result, interp.astype(np.float32))
+
+        # Update mask with newly filled pixels (for the next pass)
+        newly_filled = (~mask) & (result > 0.0)
+        mask = mask | newly_filled
+
+    return result.astype(np.float32)
+
+
+# ── Internal helpers ──────────────────────────────────────────────────────────
+
+def _odd_kernel_size(base: int, scale: float) -> int:
+    """Return the nearest odd integer to base * scale, minimum 3."""
+    raw = max(3, int(round(base * scale)))
+    return raw if raw % 2 == 1 else raw + 1

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/depth_hole_fill_node.py

@@ -0,0 +1,128 @@
+"""
+depth_hole_fill_node.py — D435i depth image hole filler (Issue #268).
+
+Subscribes to the raw D435i depth stream, fills zero/NaN pixels using
+multi-pass spatial-Gaussian bilateral interpolation, and republishes the
+filled image at camera rate.
+
+Subscribes (BEST_EFFORT):
+  /camera/depth/image_rect_raw     sensor_msgs/Image  float32 depth (m)
+
+Publishes:
+  /camera/depth/filled             sensor_msgs/Image  float32 depth (m), holes filled
+
+The filled image preserves all original valid pixels exactly and only
+modifies pixels that had no return (0 or NaN).  The output is suitable
+for all downstream consumers that expect a dense depth map (VO, RTAB-Map,
+collision avoidance, floor classifier).
+
+Parameters
+----------
+input_topic    str    /camera/depth/image_rect_raw   Input depth topic
+output_topic   str    /camera/depth/filled           Output depth topic
+kernel_size    int    5      Initial Gaussian kernel side length (pixels)
+d_min          float  0.1    Minimum valid depth (m)
+d_max          float  10.0   Maximum valid depth (m)
+max_passes     int    3      Fill passes (growing kernel per pass)
+"""
+
+from __future__ import annotations
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+import numpy as np
+from cv_bridge import CvBridge
+
+from sensor_msgs.msg import Image
+
+from ._depth_hole_fill import fill_holes
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=4,
+)
+
+
+class DepthHoleFillNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__('depth_hole_fill_node')
+
+        self.declare_parameter('input_topic',  '/camera/depth/image_rect_raw')
+        self.declare_parameter('output_topic', '/camera/depth/filled')
+        self.declare_parameter('kernel_size',  5)
+        self.declare_parameter('d_min',        0.1)
+        self.declare_parameter('d_max',        10.0)
+        self.declare_parameter('max_passes',   3)
+
+        input_topic   = self.get_parameter('input_topic').value
+        output_topic  = self.get_parameter('output_topic').value
+        self._ks      = int(self.get_parameter('kernel_size').value)
+        self._d_min   = self.get_parameter('d_min').value
+        self._d_max   = self.get_parameter('d_max').value
+        self._passes  = int(self.get_parameter('max_passes').value)
+
+        self._bridge = CvBridge()
+
+        self._sub = self.create_subscription(
+            Image, input_topic, self._on_depth, _SENSOR_QOS)
+        self._pub = self.create_publisher(Image, output_topic, 10)
+
+        self.get_logger().info(
+            f'depth_hole_fill_node ready — '
+            f'{input_topic} → {output_topic} '
+            f'kernel={self._ks} passes={self._passes} '
+            f'd=[{self._d_min},{self._d_max}]m'
+        )
+
+    # ── Callback ──────────────────────────────────────────────────────────────
+
+    def _on_depth(self, msg: Image) -> None:
+        try:
+            depth = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
+        except Exception as exc:
+            self.get_logger().error(
+                f'cv_bridge: {exc}', throttle_duration_sec=5.0)
+            return
+
+        depth = depth.astype(np.float32)
+
+        # Handle uint16 mm → float32 m conversion (D435i raw stream)
+        if depth.max() > 100.0:
+            depth /= 1000.0
+
+        filled = fill_holes(
+            depth,
+            kernel_size=self._ks,
+            d_min=self._d_min,
+            d_max=self._d_max,
+            max_passes=self._passes,
+        )
+
+        try:
+            out_msg = self._bridge.cv2_to_imgmsg(filled, encoding='32FC1')
+        except Exception as exc:
+            self.get_logger().error(
+                f'cv2_to_imgmsg: {exc}', throttle_duration_sec=5.0)
+            return
+
+        out_msg.header = msg.header
+        self._pub.publish(out_msg)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = DepthHoleFillNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_bringup/setup.py

@@ -37,6 +37,8 @@ setup(
             'floor_classifier        = saltybot_bringup.floor_classifier_node:main',
             # Visual odometry drift detector (Issue #260)
             'vo_drift_detector       = saltybot_bringup.vo_drift_node:main',
+            # Depth image hole filler (Issue #268)
+            'depth_hole_fill         = saltybot_bringup.depth_hole_fill_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bringup/test/test_depth_hole_fill.py

@@ -0,0 +1,281 @@
+"""
+test_depth_hole_fill.py — Unit tests for depth hole fill helpers (no ROS2 required).
+
+Covers:
+  valid_mask:
+    - valid range returns True
+    - zero / below d_min returns False
+    - NaN returns False
+    - above d_max returns False
+    - mixed array has correct mask
+
+  _odd_kernel_size:
+    - result is always odd
+    - result >= 3
+    - scales correctly
+
+  fill_holes — no-hole cases:
+    - fully valid image is returned unchanged
+    - output dtype is float32
+    - output shape matches input
+
+  fill_holes — basic fills:
+    - single centre hole in uniform depth → filled with correct depth
+    - single centre hole in uniform depth → original valid pixels unchanged
+    - NaN pixel treated as hole and filled
+    - row of zeros within uniform depth → filled
+
+  fill_holes — fill quality:
+    - linear gradient: centre hole filled with interpolated value
+    - multi-pass fills larger holes than single pass
+    - all-zero image stays zero (no valid neighbours)
+    - border hole (edge pixel) is handled without crash
+    - depth range: pixel above d_max treated as hole
+
+  fill_holes — valid pixel preservation:
+    - original valid pixels are never modified
+    - max_passes=1 still fills small holes
+"""
+
+import sys
+import os
+import math
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+
+from saltybot_bringup._depth_hole_fill import (
+    fill_holes,
+    valid_mask,
+    _odd_kernel_size,
+)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _uniform(val=2.0, h=64, w=64) -> np.ndarray:
+    return np.full((h, w), val, dtype=np.float32)
+
+
+def _poke_hole(arr, r, c) -> np.ndarray:
+    arr = arr.copy()
+    arr[r, c] = 0.0
+    return arr
+
+
+def _poke_nan(arr, r, c) -> np.ndarray:
+    arr = arr.copy()
+    arr[r, c] = float('nan')
+    return arr
+
+
+# ── valid_mask ────────────────────────────────────────────────────────────────
+
+class TestValidMask:
+
+    def test_valid_pixel_is_true(self):
+        d = np.array([[1.0]], dtype=np.float32)
+        assert valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_zero_is_false(self):
+        d = np.array([[0.0]], dtype=np.float32)
+        assert not valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_below_dmin_is_false(self):
+        d = np.array([[0.05]], dtype=np.float32)
+        assert not valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_nan_is_false(self):
+        d = np.array([[float('nan')]], dtype=np.float32)
+        assert not valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_above_dmax_is_false(self):
+        d = np.array([[15.0]], dtype=np.float32)
+        assert not valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_at_dmin_is_true(self):
+        d = np.array([[0.1]], dtype=np.float32)
+        assert valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_at_dmax_is_true(self):
+        d = np.array([[10.0]], dtype=np.float32)
+        assert valid_mask(d, 0.1, 10.0)[0, 0]
+
+    def test_mixed_array(self):
+        d = np.array([[0.0, 1.0, float('nan'), 5.0, 11.0]], dtype=np.float32)
+        m = valid_mask(d, 0.1, 10.0)
+        np.testing.assert_array_equal(m, [[False, True, False, True, False]])
+
+
+# ── _odd_kernel_size ──────────────────────────────────────────────────────────
+
+class TestOddKernelSize:
+
+    @pytest.mark.parametrize('base,scale', [
+        (5, 1.0), (5, 2.5), (5, 6.0),
+        (3, 1.0), (7, 2.0), (9, 3.0),
+        (4, 1.0),   # even base → must become odd
+    ])
+    def test_result_is_odd(self, base, scale):
+        ks = _odd_kernel_size(base, scale)
+        assert ks % 2 == 1
+
+    @pytest.mark.parametrize('base,scale', [(3, 1.0), (1, 5.0), (2, 0.5)])
+    def test_result_at_least_3(self, base, scale):
+        assert _odd_kernel_size(base, scale) >= 3
+
+    def test_scale_1_returns_base_or_nearby_odd(self):
+        ks = _odd_kernel_size(5, 1.0)
+        assert ks == 5
+
+    def test_large_scale_gives_large_kernel(self):
+        ks = _odd_kernel_size(5, 6.0)
+        assert ks >= 25   # 5 * 6 = 30 → 31
+
+
+# ── fill_holes — output contract ──────────────────────────────────────────────
+
+class TestFillHolesOutputContract:
+
+    def test_output_dtype_float32(self):
+        out = fill_holes(_uniform(2.0))
+        assert out.dtype == np.float32
+
+    def test_output_shape_preserved(self):
+        img = _uniform(2.0, h=48, w=64)
+        out = fill_holes(img)
+        assert out.shape == img.shape
+
+    def test_fully_valid_image_unchanged(self):
+        img = _uniform(2.0)
+        out = fill_holes(img)
+        np.testing.assert_allclose(out, img, atol=1e-6)
+
+    def test_valid_pixels_never_modified(self):
+        """Any pixel valid in the input must be identical in the output."""
+        img = _uniform(3.0, h=32, w=32)
+        img[16, 16] = 0.0   # one hole
+        mask_before = valid_mask(img)
+        out = fill_holes(img)
+        np.testing.assert_allclose(out[mask_before], img[mask_before], atol=1e-6)
+
+
+# ── fill_holes — basic hole filling ──────────────────────────────────────────
+
+class TestFillHolesBasic:
+
+    def test_centre_zero_filled_uniform(self):
+        """Single zero pixel in uniform depth → filled with that depth."""
+        img = _poke_hole(_uniform(2.0, 32, 32), 16, 16)
+        out = fill_holes(img, kernel_size=5, max_passes=1)
+        assert out[16, 16] == pytest.approx(2.0, abs=0.05)
+
+    def test_centre_nan_filled_uniform(self):
+        """Single NaN pixel in uniform depth → filled."""
+        img = _poke_nan(_uniform(2.0, 32, 32), 16, 16)
+        out = fill_holes(img, kernel_size=5, max_passes=1)
+        assert out[16, 16] == pytest.approx(2.0, abs=0.05)
+
+    def test_filled_value_is_positive(self):
+        img = _poke_hole(_uniform(1.5, 32, 32), 16, 16)
+        out = fill_holes(img)
+        assert out[16, 16] > 0.0
+
+    def test_row_of_holes_filled(self):
+        """Entire middle row zeroed → should be filled from neighbours above/below."""
+        img = _uniform(3.0, 32, 32)
+        img[16, :] = 0.0
+        out = fill_holes(img, kernel_size=7, max_passes=1)
+        # All pixels in the row should be non-zero after filling
+        assert (out[16, :] > 0.0).all()
+
+    def test_all_zero_stays_zero(self):
+        """Image with no valid pixels → stays zero (nothing to interpolate from)."""
+        img = np.zeros((32, 32), dtype=np.float32)
+        out = fill_holes(img, d_min=0.1)
+        assert (out == 0.0).all()
+
+    def test_border_hole_no_crash(self):
+        """Holes at image corners must not raise exceptions."""
+        img = _uniform(2.0, 32, 32)
+        img[0, 0] = 0.0
+        img[0, -1] = 0.0
+        img[-1, 0] = 0.0
+        img[-1, -1] = 0.0
+        out = fill_holes(img)   # must not raise
+        assert out.shape == img.shape
+
+    def test_border_holes_filled(self):
+        """Corner holes should be filled from their neighbours."""
+        img = _uniform(2.0, 32, 32)
+        img[0, 0] = 0.0
+        out = fill_holes(img, kernel_size=5, max_passes=1)
+        assert out[0, 0] == pytest.approx(2.0, abs=0.1)
+
+
+# ── fill_holes — fill quality ─────────────────────────────────────────────────
+
+class TestFillHolesQuality:
+
+    def test_linear_gradient_centre_hole_interpolated(self):
+        """
+        Depth linearly increasing from 1.0 (left) to 3.0 (right).
+        Centre hole should be filled near the midpoint (~2.0).
+        """
+        h, w = 32, 32
+        img = np.tile(np.linspace(1.0, 3.0, w, dtype=np.float32), (h, 1))
+        cx = w // 2
+        img[:, cx] = 0.0
+        out = fill_holes(img, kernel_size=5, max_passes=1)
+        mid = out[h // 2, cx]
+        assert 1.5 <= mid <= 2.5, f'interpolated value {mid:.3f} not in [1.5, 2.5]'
+
+    def test_large_hole_filled_with_more_passes(self):
+        """A 9×9 hole in uniform depth: single pass may not fully fill it,
+        but 3 passes should."""
+        img = _uniform(2.0, 64, 64)
+        # Create a 9×9 hole
+        img[28:37, 28:37] = 0.0
+        out1 = fill_holes(img, kernel_size=5, max_passes=1)
+        out3 = fill_holes(img, kernel_size=5, max_passes=3)
+        # More passes → fewer remaining holes
+        holes1 = (out1 == 0.0).sum()
+        holes3 = (out3 == 0.0).sum()
+        assert holes3 <= holes1, f'more passes should reduce holes: {holes3} vs {holes1}'
+
+    def test_3pass_fills_9x9_hole_completely(self):
+        img = _uniform(2.0, 64, 64)
+        img[28:37, 28:37] = 0.0
+        out = fill_holes(img, kernel_size=5, max_passes=3)
+        assert (out[28:37, 28:37] > 0.0).all()
+
+    def test_filled_depth_within_valid_range(self):
+        """Filled pixels should have depth within [d_min, d_max]."""
+        img = _uniform(2.0, 32, 32)
+        img[10:15, 10:15] = 0.0
+        out = fill_holes(img, d_min=0.1, d_max=10.0, max_passes=3)
+        # Only check pixels that were actually filled
+        was_hole = (img == 0.0)
+        filled   = out[was_hole]
+        positive = filled[filled > 0.0]
+        assert (positive >= 0.1).all()
+        assert (positive <= 10.0).all()
+
+    def test_above_dmax_treated_as_hole(self):
+        """Pixels above d_max should be treated as holes and filled."""
+        img = _uniform(2.0, 32, 32)
+        img[16, 16] = 15.0   # out of range
+        out = fill_holes(img, d_max=10.0, max_passes=1)
+        assert out[16, 16] == pytest.approx(2.0, abs=0.1)
+
+    def test_max_passes_1_works(self):
+        img = _poke_hole(_uniform(2.0, 32, 32), 16, 16)
+        out = fill_holes(img, max_passes=1)
+        assert out.shape == img.shape
+        assert out[16, 16] > 0.0
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml

@@ -0,0 +1,5 @@
+wheel_slip_detector:
+  ros__parameters:
+    frequency: 10
+    slip_threshold: 0.1
+    slip_timeout: 0.5

jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("saltybot_wheel_slip_detector")
+    config_file = os.path.join(pkg_dir, "config", "wheel_slip_config.yaml")
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "config_file",
+            default_value=config_file,
+            description="Path to configuration YAML file",
+        ),
+        Node(
+            package="saltybot_wheel_slip_detector",
+            executable="wheel_slip_detector_node",
+            name="wheel_slip_detector",
+            output="screen",
+            parameters=[LaunchConfiguration("config_file")],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+"""Wheel slip detector for SaltyBot.
+
+Detects wheel slip by comparing commanded velocity vs actual encoder velocity.
+Publishes Bool when slip is detected for >0.5s, enabling speed reduction response.
+"""
+
+from typing import Optional
+import math
+
+import rclpy
+from rclpy.node import Node
+from rclpy.timer import Timer
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool
+
+
+class WheelSlipDetectorNode(Node):
+    """ROS2 node for wheel slip detection."""
+
+    def __init__(self):
+        super().__init__("wheel_slip_detector")
+
+        self.declare_parameter("frequency", 10)
+        frequency = self.get_parameter("frequency").value
+        self.declare_parameter("slip_threshold", 0.1)
+        self.declare_parameter("slip_timeout", 0.5)
+
+        self.slip_threshold = self.get_parameter("slip_threshold").value
+        self.slip_timeout = self.get_parameter("slip_timeout").value
+        self.period = 1.0 / frequency
+
+        self.cmd_vel: Optional[Twist] = None
+        self.actual_vel: Optional[Twist] = None
+        self.slip_duration = 0.0
+        self.slip_detected = False
+
+        self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)
+        self.create_subscription(Odometry, "/odom", self._on_odom, 10)
+        self.pub_slip = self.create_publisher(Bool, "/saltybot/wheel_slip_detected", 10)
+        self.timer: Timer = self.create_timer(self.period, self._timer_callback)
+
+        self.get_logger().info(
+            f"Wheel slip detector initialized at {frequency}Hz. "
+            f"Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s"
+        )
+
+    def _on_cmd_vel(self, msg: Twist) -> None:
+        """Update commanded velocity from subscription."""
+        self.cmd_vel = msg
+
+    def _on_odom(self, msg: Odometry) -> None:
+        """Update actual velocity from odometry subscription."""
+        self.actual_vel = msg.twist.twist
+
+    def _timer_callback(self) -> None:
+        """Detect wheel slip and publish detection flag."""
+        if self.cmd_vel is None or self.actual_vel is None:
+            slip_detected = False
+        else:
+            slip_detected = self._check_slip()
+
+        if slip_detected:
+            self.slip_duration += self.period
+        else:
+            self.slip_duration = 0.0
+
+        is_slip = self.slip_duration > self.slip_timeout
+
+        if is_slip != self.slip_detected:
+            self.slip_detected = is_slip
+            if self.slip_detected:
+                self.get_logger().warn(f"WHEEL SLIP DETECTED: {self.slip_duration:.2f}s")
+            else:
+                self.get_logger().info("Wheel slip cleared")
+
+        slip_msg = Bool()
+        slip_msg.data = is_slip
+        self.pub_slip.publish(slip_msg)
+
+    def _check_slip(self) -> bool:
+        """Check if velocity difference indicates slip."""
+        cmd_speed = math.sqrt(self.cmd_vel.linear.x**2 + self.cmd_vel.linear.y**2)
+        actual_speed = math.sqrt(self.actual_vel.linear.x**2 + self.actual_vel.linear.y**2)
+        vel_diff = abs(cmd_speed - actual_speed)
+
+        if cmd_speed < 0.05 and actual_speed < 0.05:
+            return False
+
+        return vel_diff > self.slip_threshold
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WheelSlipDetectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py

@@ -0,0 +1,27 @@
+from setuptools import find_packages, setup
+
+package_name = "saltybot_wheel_slip_detector"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/launch", ["launch/wheel_slip_detector.launch.py"]),
+        ("share/" + package_name + "/config", ["config/wheel_slip_config.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Seb",
+    maintainer_email="seb@vayrette.com",
+    description="Wheel slip detection from velocity command/actual mismatch",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/test_wheel_slip_detector.py

@@ -0,0 +1,343 @@
+"""Unit tests for wheel_slip_detector_node."""
+
+import pytest
+import math
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool
+
+import rclpy
+
+from saltybot_wheel_slip_detector.wheel_slip_detector_node import WheelSlipDetectorNode
+
+
+@pytest.fixture
+def rclpy_fixture():
+    """Initialize and cleanup rclpy."""
+    rclpy.init()
+    yield
+    rclpy.shutdown()
+
+
+@pytest.fixture
+def node(rclpy_fixture):
+    """Create a wheel slip detector node instance."""
+    node = WheelSlipDetectorNode()
+    yield node
+    node.destroy_node()
+
+
+class TestNodeInitialization:
+    """Test suite for node initialization."""
+
+    def test_node_initialization(self, node):
+        """Test that node initializes with correct defaults."""
+        assert node.cmd_vel is None
+        assert node.actual_vel is None
+        assert node.slip_threshold == 0.1
+        assert node.slip_timeout == 0.5
+        assert node.slip_duration == 0.0
+        assert node.slip_detected is False
+
+    def test_frequency_parameter(self, node):
+        """Test frequency parameter is set correctly."""
+        frequency = node.get_parameter("frequency").value
+        assert frequency == 10
+
+    def test_slip_threshold_parameter(self, node):
+        """Test slip threshold parameter is set correctly."""
+        threshold = node.get_parameter("slip_threshold").value
+        assert threshold == 0.1
+
+    def test_slip_timeout_parameter(self, node):
+        """Test slip timeout parameter is set correctly."""
+        timeout = node.get_parameter("slip_timeout").value
+        assert timeout == 0.5
+
+    def test_period_calculation(self, node):
+        """Test that time period is correctly calculated from frequency."""
+        assert node.period == pytest.approx(0.1)
+
+
+class TestSubscriptions:
+    """Test suite for subscription handling."""
+
+    def test_cmd_vel_subscription(self, node):
+        """Test that cmd_vel subscription updates node state."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        cmd.linear.y = 0.5
+        node._on_cmd_vel(cmd)
+        assert node.cmd_vel is cmd
+        assert node.cmd_vel.linear.x == 1.0
+
+    def test_odom_subscription(self, node):
+        """Test that odometry subscription updates actual velocity."""
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.95
+        odom.twist.twist.linear.y = 0.48
+        node._on_odom(odom)
+        assert node.actual_vel is odom.twist.twist
+        assert node.actual_vel.linear.x == 0.95
+
+
+class TestSlipDetection:
+    """Test suite for slip detection logic."""
+
+    def test_no_slip_perfect_match(self, node):
+        """Test no slip when commanded equals actual."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 1.0
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is False
+
+    def test_no_slip_small_difference(self, node):
+        """Test no slip when difference is below threshold."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.95
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is False
+
+    def test_slip_exceeds_threshold(self, node):
+        """Test slip detection when difference exceeds threshold."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.85
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is True
+
+    def test_slip_large_difference(self, node):
+        """Test slip detection with large velocity difference."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is True
+
+    def test_no_slip_both_zero(self, node):
+        """Test no slip when both commanded and actual are zero."""
+        cmd = Twist()
+        cmd.linear.x = 0.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.0
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is False
+
+    def test_no_slip_both_near_zero(self, node):
+        """Test no slip when both are near zero (tolerance)."""
+        cmd = Twist()
+        cmd.linear.x = 0.01
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.02
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is False
+
+    def test_slip_2d_velocity(self, node):
+        """Test slip detection with 2D velocity (x and y)."""
+        cmd = Twist()
+        cmd.linear.x = 0.7
+        cmd.linear.y = 0.7
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        odom.twist.twist.linear.y = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        assert node._check_slip() is True
+
+
+class TestSlipPersistence:
+    """Test suite for slip persistence timing."""
+
+    def test_slip_not_triggered_immediately(self, node):
+        """Test that slip is not triggered immediately but requires timeout."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        node._timer_callback()
+        assert node.slip_duration > 0.0
+        assert node.slip_detected is False
+
+    def test_slip_declared_after_timeout(self, node):
+        """Test that slip is declared after timeout period."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        for _ in range(6):
+            node._timer_callback()
+        assert node.slip_detected is True
+
+    def test_slip_recovery_resets_duration(self, node):
+        """Test that slip duration resets when condition clears."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom1 = Odometry()
+        odom1.twist.twist.linear.x = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom1)
+        for _ in range(3):
+            node._timer_callback()
+        odom2 = Odometry()
+        odom2.twist.twist.linear.x = 1.0
+        node._on_odom(odom2)
+        node._timer_callback()
+        assert node.slip_duration == pytest.approx(0.0)
+        assert node.slip_detected is False
+
+    def test_slip_cumulative_time(self, node):
+        """Test that slip duration accumulates across callbacks."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        node._on_cmd_vel(cmd)
+        node._on_odom(odom)
+        for _ in range(3):
+            node._timer_callback()
+        assert node.slip_duration == pytest.approx(0.3)
+        assert node.slip_detected is False
+        for _ in range(3):
+            node._timer_callback()
+        assert node.slip_duration == pytest.approx(0.6)
+        assert node.slip_detected is True
+
+
+class TestNoDataConditions:
+    """Test suite for behavior when sensor data is unavailable."""
+
+    def test_no_slip_without_cmd_vel(self, node):
+        """Test no slip declared when cmd_vel not received."""
+        node.cmd_vel = None
+        odom = Odometry()
+        odom.twist.twist.linear.x = 0.5
+        node._on_odom(odom)
+        node._timer_callback()
+        assert node.slip_detected is False
+
+    def test_no_slip_without_odometry(self, node):
+        """Test no slip declared when odometry not received."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        node._on_cmd_vel(cmd)
+        node.actual_vel = None
+        node._timer_callback()
+        assert node.slip_detected is False
+
+
+class TestScenarios:
+    """Integration-style tests for realistic scenarios."""
+
+    def test_scenario_normal_motion_no_slip(self, node):
+        """Scenario: Normal motion with good wheel traction."""
+        cmd = Twist()
+        cmd.linear.x = 0.5
+        for i in range(10):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 0.5 + (i * 0.001)
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is False
+
+    def test_scenario_ice_slip_persistent(self, node):
+        """Scenario: Ice causes persistent wheel slip."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        for _ in range(10):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 0.7
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is True
+
+    def test_scenario_sandy_surface_intermittent_slip(self, node):
+        """Scenario: Sandy surface causes intermittent slip."""
+        cmd = Twist()
+        cmd.linear.x = 0.8
+        speeds = [0.7, 0.8, 0.6, 0.8, 0.7, 0.8]
+        for speed in speeds:
+            odom = Odometry()
+            odom.twist.twist.linear.x = speed
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is False
+
+    def test_scenario_sudden_obstacle_slip(self, node):
+        """Scenario: Robot hits obstacle and wheels slip."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        for _ in range(3):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 1.0
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        for _ in range(8):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 0.2
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is True
+
+    def test_scenario_wet_surface_recovery(self, node):
+        """Scenario: Wet surface slip, then wheel regains traction."""
+        cmd = Twist()
+        cmd.linear.x = 1.0
+        for _ in range(6):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 0.8
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is True
+        for _ in range(3):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 1.0
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is False
+
+    def test_scenario_backward_motion(self, node):
+        """Scenario: Backward motion with slip."""
+        cmd = Twist()
+        cmd.linear.x = -0.8
+        for _ in range(6):
+            odom = Odometry()
+            odom.twist.twist.linear.x = -0.4
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is True
+
+    def test_scenario_diagonal_motion_slip(self, node):
+        """Scenario: Diagonal motion with slip."""
+        cmd = Twist()
+        cmd.linear.x = 0.7
+        cmd.linear.y = 0.7
+        for _ in range(6):
+            odom = Odometry()
+            odom.twist.twist.linear.x = 0.5
+            odom.twist.twist.linear.y = 0.5
+            node._on_cmd_vel(cmd)
+            node._on_odom(odom)
+            node._timer_callback()
+        assert node.slip_detected is True

ui/social-bot/src/App.jsx

@@ -58,6 +58,12 @@ import JoystickTeleop from './components/JoystickTeleop.jsx';
 // Network diagnostics (issue #222)
 import { NetworkPanel } from './components/NetworkPanel.jsx';
 
+// Waypoint editor (issue #261)
+import { WaypointEditor } from './components/WaypointEditor.jsx';
+
+// Status header (issue #269)
+import { StatusHeader } from './components/StatusHeader.jsx';
+
 const TAB_GROUPS = [
   {
     label: 'SOCIAL',
@@ -85,6 +91,13 @@ const TAB_GROUPS = [
       { id: 'cameras', label: 'Cameras', },
     ],
   },
+  {
+    label: 'NAVIGATION',
+    color: 'text-teal-600',
+    tabs: [
+      { id: 'waypoints', label: 'Waypoints' },
+    ],
+  },
   {
     label: 'FLEET',
     color: 'text-green-600',
@@ -187,6 +200,9 @@ export default function App() {
         )}
       </header>
 
+      {/* ── Status Header ── */}
+      <StatusHeader subscribe={subscribe} />
+
       {/* ── Tab Navigation ── */}
       <nav className="bg-[#070712] border-b border-cyan-950 shrink-0 overflow-x-auto">
         <div className="flex min-w-max">
@@ -244,8 +260,10 @@ export default function App() {
             </div>
           </div>
         )}
-        {activeTab === 'health'  && <SystemHealth subscribe={subscribe} />}
-        {activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
+        {activeTab === 'health'   && <SystemHealth subscribe={subscribe} />}
+        {activeTab === 'cameras'  && <CameraViewer subscribe={subscribe} />}
+
+        {activeTab === 'waypoints' && <WaypointEditor subscribe={subscribe} publish={publishFn} callService={callService} />}
 
         {activeTab === 'fleet'    && <FleetPanel />}
         {activeTab === 'missions' && <MissionPlanner />}

ui/social-bot/src/components/StatusHeader.jsx

@@ -0,0 +1,260 @@
+/**
+ * StatusHeader.jsx — Persistent status bar with robot health indicators
+ *
+ * Features:
+ *   - Battery percentage and status indicator
+ *   - WiFi signal strength (RSSI)
+ *   - Motor status (running/stopped/error)
+ *   - Emergency state indicator (active/clear)
+ *   - System uptime
+ *   - Current operational mode (idle/navigation/social/docking)
+ *   - Real-time updates from ROS topics
+ *   - Always visible at top of dashboard
+ */
+
+import { useEffect, useState } from 'react';
+
+function StatusHeader({ subscribe }) {
+  const [batteryPercent, setBatteryPercent] = useState(null);
+  const [batteryVoltage, setBatteryVoltage] = useState(null);
+  const [wifiRssi, setWifiRssi] = useState(null);
+  const [wifiQuality, setWifiQuality] = useState('unknown');
+  const [motorStatus, setMotorStatus] = useState('idle');
+  const [motorCurrent, setMotorCurrent] = useState(null);
+  const [emergencyActive, setEmergencyActive] = useState(false);
+  const [uptime, setUptime] = useState(0);
+  const [currentMode, setCurrentMode] = useState('idle');
+  const [connected, setConnected] = useState(true);
+
+  // Battery subscriber
+  useEffect(() => {
+    const unsubBattery = subscribe(
+      '/saltybot/battery',
+      'sensor_msgs/BatteryState',
+      (msg) => {
+        try {
+          setBatteryPercent(Math.round(msg.percentage * 100));
+          setBatteryVoltage(msg.voltage?.toFixed(1));
+        } catch (e) {
+          console.error('Error parsing battery data:', e);
+        }
+      }
+    );
+    return unsubBattery;
+  }, [subscribe]);
+
+  // WiFi RSSI subscriber
+  useEffect(() => {
+    const unsubWifi = subscribe(
+      '/saltybot/wifi_rssi',
+      'std_msgs/Float32',
+      (msg) => {
+        try {
+          const rssi = Math.round(msg.data);
+          setWifiRssi(rssi);
+
+          if (rssi > -50) setWifiQuality('excellent');
+          else if (rssi > -60) setWifiQuality('good');
+          else if (rssi > -70) setWifiQuality('fair');
+          else if (rssi > -80) setWifiQuality('weak');
+          else setWifiQuality('poor');
+        } catch (e) {
+          console.error('Error parsing WiFi data:', e);
+        }
+      }
+    );
+    return unsubWifi;
+  }, [subscribe]);
+
+  // Motor status subscriber
+  useEffect(() => {
+    const unsubMotor = subscribe(
+      '/saltybot/motor_status',
+      'std_msgs/String',
+      (msg) => {
+        try {
+          const status = msg.data?.toLowerCase() || 'unknown';
+          setMotorStatus(status);
+        } catch (e) {
+          console.error('Error parsing motor status:', e);
+        }
+      }
+    );
+    return unsubMotor;
+  }, [subscribe]);
+
+  // Motor current subscriber
+  useEffect(() => {
+    const unsubCurrent = subscribe(
+      '/saltybot/motor_current',
+      'std_msgs/Float32',
+      (msg) => {
+        try {
+          setMotorCurrent(Math.round(msg.data * 100) / 100);
+        } catch (e) {
+          console.error('Error parsing motor current:', e);
+        }
+      }
+    );
+    return unsubCurrent;
+  }, [subscribe]);
+
+  // Emergency subscriber
+  useEffect(() => {
+    const unsubEmergency = subscribe(
+      '/saltybot/emergency',
+      'std_msgs/Bool',
+      (msg) => {
+        try {
+          setEmergencyActive(msg.data === true);
+        } catch (e) {
+          console.error('Error parsing emergency status:', e);
+        }
+      }
+    );
+    return unsubEmergency;
+  }, [subscribe]);
+
+  // Uptime tracking
+  useEffect(() => {
+    const startTime = Date.now();
+    const interval = setInterval(() => {
+      const elapsed = Math.floor((Date.now() - startTime) / 1000);
+      const hours = Math.floor(elapsed / 3600);
+      const minutes = Math.floor((elapsed % 3600) / 60);
+      setUptime(`${hours}h ${minutes}m`);
+    }, 1000);
+
+    return () => clearInterval(interval);
+  }, []);
+
+  // Current mode subscriber
+  useEffect(() => {
+    const unsubMode = subscribe(
+      '/saltybot/current_mode',
+      'std_msgs/String',
+      (msg) => {
+        try {
+          const mode = msg.data?.toLowerCase() || 'idle';
+          setCurrentMode(mode);
+        } catch (e) {
+          console.error('Error parsing mode:', e);
+        }
+      }
+    );
+    return unsubMode;
+  }, [subscribe]);
+
+  // Connection status
+  useEffect(() => {
+    const timer = setTimeout(() => {
+      setConnected(batteryPercent !== null);
+    }, 2000);
+    return () => clearTimeout(timer);
+  }, [batteryPercent]);
+
+  const getBatteryColor = () => {
+    if (batteryPercent === null) return 'text-gray-600';
+    if (batteryPercent > 60) return 'text-green-400';
+    if (batteryPercent > 30) return 'text-amber-400';
+    return 'text-red-400';
+  };
+
+  const getWifiColor = () => {
+    if (wifiRssi === null) return 'text-gray-600';
+    if (wifiQuality === 'excellent' || wifiQuality === 'good') return 'text-green-400';
+    if (wifiQuality === 'fair') return 'text-amber-400';
+    return 'text-red-400';
+  };
+
+  const getMotorColor = () => {
+    if (motorStatus === 'running') return 'text-green-400';
+    if (motorStatus === 'idle') return 'text-gray-500';
+    return 'text-red-400';
+  };
+
+  const getModeColor = () => {
+    switch (currentMode) {
+      case 'navigation':
+        return 'text-cyan-400';
+      case 'social':
+        return 'text-purple-400';
+      case 'docking':
+        return 'text-blue-400';
+      default:
+        return 'text-gray-500';
+    }
+  };
+
+  return (
+    <div className="flex items-center justify-between px-4 py-2 bg-[#0a0a0f] border-b border-cyan-950/50 h-14 shrink-0 gap-4">
+      {/* Connection status */}
+      <div className="flex items-center gap-2">
+        <div className={`w-2 h-2 rounded-full ${connected ? 'bg-green-400' : 'bg-red-500'}`} />
+        <span className="text-xs text-gray-600">
+          {connected ? 'CONNECTED' : 'DISCONNECTED'}
+        </span>
+      </div>
+
+      {/* Battery */}
+      <div className="flex items-center gap-1.5 px-2 py-1 rounded bg-gray-900 border border-gray-800">
+        <span className={`text-xs font-bold ${getBatteryColor()}`}>🔋</span>
+        <span className={`text-xs font-mono ${getBatteryColor()}`}>
+          {batteryPercent !== null ? `${batteryPercent}%` : '—'}
+        </span>
+        {batteryVoltage && (
+          <span className="text-xs text-gray-600">{batteryVoltage}V</span>
+        )}
+      </div>
+
+      {/* WiFi */}
+      <div className="flex items-center gap-1.5 px-2 py-1 rounded bg-gray-900 border border-gray-800">
+        <span className={`text-xs font-bold ${getWifiColor()}`}>📡</span>
+        <span className={`text-xs font-mono ${getWifiColor()}`}>
+          {wifiRssi !== null ? `${wifiRssi}dBm` : '—'}
+        </span>
+        <span className="text-xs text-gray-600 capitalize">{wifiQuality}</span>
+      </div>
+
+      {/* Motors */}
+      <div className="flex items-center gap-1.5 px-2 py-1 rounded bg-gray-900 border border-gray-800">
+        <span className={`text-xs font-bold ${getMotorColor()}`}>⚙️</span>
+        <span className={`text-xs font-mono capitalize ${getMotorColor()}`}>
+          {motorStatus}
+        </span>
+        {motorCurrent !== null && (
+          <span className="text-xs text-gray-600">{motorCurrent}A</span>
+        )}
+      </div>
+
+      {/* Emergency */}
+      <div
+        className={`flex items-center gap-1.5 px-2 py-1 rounded border ${
+          emergencyActive
+            ? 'bg-red-950 border-red-700'
+            : 'bg-gray-900 border-gray-800'
+        }`}
+      >
+        <span className={emergencyActive ? 'text-red-400 text-xs' : 'text-gray-600 text-xs'}>
+          {emergencyActive ? '🚨 EMERGENCY' : '✓ Safe'}
+        </span>
+      </div>
+
+      {/* Uptime */}
+      <div className="flex items-center gap-1.5 px-2 py-1 rounded bg-gray-900 border border-gray-800">
+        <span className="text-xs text-gray-600">⏱️</span>
+        <span className="text-xs font-mono text-gray-500">{uptime}</span>
+      </div>
+
+      {/* Current Mode */}
+      <div className="flex items-center gap-1.5 px-2 py-1 rounded bg-gray-900 border border-gray-800">
+        <span className="text-xs text-gray-600">Mode:</span>
+        <span className={`text-xs font-bold capitalize ${getModeColor()}`}>
+          {currentMode}
+        </span>
+      </div>
+    </div>
+  );
+}
+
+export { StatusHeader };

ui/social-bot/src/components/WaypointEditor.jsx

@@ -0,0 +1,401 @@
+/**
+ * WaypointEditor.jsx — Interactive waypoint navigation editor with click-to-place and drag-to-reorder
+ *
+ * Features:
+ *   - Click on map canvas to place waypoints
+ *   - Drag waypoints to reorder navigation sequence
+ *   - Right-click to delete waypoints
+ *   - Real-time waypoint list with labels and coordinates
+ *   - Send Nav2 goal to /navigate_to_pose action
+ *   - Execute waypoint sequence with automatic progression
+ *   - Clear all waypoints button
+ *   - Visual feedback for active waypoint (executing)
+ */
+
+import { useEffect, useRef, useState } from 'react';
+
+function WaypointEditor({ subscribe, publish, callService }) {
+  const [waypoints, setWaypoints] = useState([]);
+  const [selectedWaypoint, setSelectedWaypoint] = useState(null);
+  const [isDragging, setIsDragging] = useState(false);
+  const [dragIndex, setDragIndex] = useState(null);
+  const [activeWaypoint, setActiveWaypoint] = useState(null);
+  const [executing, setExecuting] = useState(false);
+
+  const [mapData, setMapData] = useState(null);
+  const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 });
+
+  const containerRef = useRef(null);
+
+  const mapDataRef = useRef(null);
+  const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 });
+  const waypointsRef = useRef([]);
+
+  // Subscribe to map data
+  useEffect(() => {
+    const unsubMap = subscribe(
+      '/map',
+      'nav_msgs/OccupancyGrid',
+      (msg) => {
+        try {
+          const mapInfo = {
+            width: msg.info.width,
+            height: msg.info.height,
+            resolution: msg.info.resolution,
+            origin: msg.info.origin,
+          };
+          setMapData(mapInfo);
+          mapDataRef.current = mapInfo;
+        } catch (e) {
+          console.error('Error parsing map data:', e);
+        }
+      }
+    );
+    return unsubMap;
+  }, [subscribe]);
+
+  // Subscribe to robot odometry
+  useEffect(() => {
+    const unsubOdom = subscribe(
+      '/odom',
+      'nav_msgs/Odometry',
+      (msg) => {
+        try {
+          const pos = msg.pose.pose.position;
+          const ori = msg.pose.pose.orientation;
+
+          const siny_cosp = 2 * (ori.w * ori.z + ori.x * ori.y);
+          const cosy_cosp = 1 - 2 * (ori.y * ori.y + ori.z * ori.z);
+          const theta = Math.atan2(siny_cosp, cosy_cosp);
+
+          const newPose = { x: pos.x, y: pos.y, theta };
+          setRobotPose(newPose);
+          robotPoseRef.current = newPose;
+        } catch (e) {
+          console.error('Error parsing odometry data:', e);
+        }
+      }
+    );
+    return unsubOdom;
+  }, [subscribe]);
+
+  const handleCanvasClick = (e) => {
+    if (!mapDataRef.current || !containerRef.current) return;
+
+    const rect = containerRef.current.getBoundingClientRect();
+    const clickX = e.clientX - rect.left;
+    const clickY = e.clientY - rect.top;
+
+    const robot = robotPoseRef.current;
+    const zoom = 1;
+
+    const centerX = containerRef.current.clientWidth / 2;
+    const centerY = containerRef.current.clientHeight / 2;
+
+    const worldX = robot.x + (clickX - centerX) / zoom;
+    const worldY = robot.y - (clickY - centerY) / zoom;
+
+    const newWaypoint = {
+      id: Date.now(),
+      x: parseFloat(worldX.toFixed(2)),
+      y: parseFloat(worldY.toFixed(2)),
+      label: `WP-${waypoints.length + 1}`,
+    };
+
+    setWaypoints((prev) => [...prev, newWaypoint]);
+    waypointsRef.current = [...waypointsRef.current, newWaypoint];
+  };
+
+  const handleDeleteWaypoint = (id) => {
+    setWaypoints((prev) => prev.filter((wp) => wp.id !== id));
+    waypointsRef.current = waypointsRef.current.filter((wp) => wp.id !== id);
+    if (selectedWaypoint === id) setSelectedWaypoint(null);
+  };
+
+  const handleWaypointSelect = (id) => {
+    setSelectedWaypoint(selectedWaypoint === id ? null : id);
+  };
+
+  const handleWaypointDragStart = (e, index) => {
+    setIsDragging(true);
+    setDragIndex(index);
+  };
+
+  const handleWaypointDragOver = (e, targetIndex) => {
+    if (!isDragging || dragIndex === null || dragIndex === targetIndex) return;
+
+    const newWaypoints = [...waypoints];
+    const draggedWaypoint = newWaypoints[dragIndex];
+    newWaypoints.splice(dragIndex, 1);
+    newWaypoints.splice(targetIndex, 0, draggedWaypoint);
+
+    setWaypoints(newWaypoints);
+    waypointsRef.current = newWaypoints;
+    setDragIndex(targetIndex);
+  };
+
+  const handleWaypointDragEnd = () => {
+    setIsDragging(false);
+    setDragIndex(null);
+  };
+
+  const sendNavGoal = async (waypoint) => {
+    if (!callService) return;
+
+    try {
+      const heading = waypoint.theta || 0;
+      const halfHeading = heading / 2;
+
+      const goal = {
+        pose: {
+          position: {
+            x: waypoint.x,
+            y: waypoint.y,
+            z: 0,
+          },
+          orientation: {
+            x: 0,
+            y: 0,
+            z: Math.sin(halfHeading),
+            w: Math.cos(halfHeading),
+          },
+        },
+      };
+
+      await callService(
+        '/navigate_to_pose',
+        'nav2_msgs/NavigateToPose',
+        { pose: goal.pose }
+      );
+
+      setActiveWaypoint(waypoint.id);
+      return true;
+    } catch (e) {
+      console.error('Error sending nav goal:', e);
+      return false;
+    }
+  };
+
+  const executeWaypoints = async () => {
+    if (waypoints.length === 0) return;
+
+    setExecuting(true);
+    for (const waypoint of waypoints) {
+      const success = await sendNavGoal(waypoint);
+      if (!success) break;
+      await new Promise((resolve) => setTimeout(resolve, 500));
+    }
+    setExecuting(false);
+    setActiveWaypoint(null);
+  };
+
+  const clearWaypoints = () => {
+    setWaypoints([]);
+    waypointsRef.current = [];
+    setSelectedWaypoint(null);
+    setActiveWaypoint(null);
+  };
+
+  const sendSingleGoal = async () => {
+    if (selectedWaypoint === null) return;
+
+    const wp = waypoints.find((w) => w.id === selectedWaypoint);
+    if (wp) {
+      await sendNavGoal(wp);
+    }
+  };
+
+  return (
+    <div className="flex h-full gap-3">
+      {/* Map area */}
+      <div className="flex-1 flex flex-col space-y-3">
+        <div className="flex-1 bg-gray-900 rounded-lg border border-cyan-950 overflow-hidden relative cursor-crosshair">
+          <div
+            ref={containerRef}
+            className="w-full h-full"
+            onClick={handleCanvasClick}
+            onContextMenu={(e) => e.preventDefault()}
+          >
+            <svg className="absolute inset-0 w-full h-full pointer-events-none" id="waypoint-overlay">
+              {waypoints.map((wp, idx) => {
+                if (!mapDataRef.current) return null;
+
+                const robot = robotPoseRef.current;
+                const zoom = 1;
+                const centerX = containerRef.current?.clientWidth / 2 || 400;
+                const centerY = containerRef.current?.clientHeight / 2 || 300;
+
+                const canvasX = centerX + (wp.x - robot.x) * zoom;
+                const canvasY = centerY - (wp.y - robot.y) * zoom;
+
+                const isActive = wp.id === activeWaypoint;
+                const isSelected = wp.id === selectedWaypoint;
+
+                return (
+                  <g key={wp.id}>
+                    <circle
+                      cx={canvasX}
+                      cy={canvasY}
+                      r="10"
+                      fill={isActive ? '#ef4444' : isSelected ? '#fbbf24' : '#06b6d4'}
+                      opacity="0.8"
+                    />
+                    <text
+                      x={canvasX}
+                      y={canvasY}
+                      textAnchor="middle"
+                      dominantBaseline="middle"
+                      fill="white"
+                      fontSize="10"
+                      fontWeight="bold"
+                      pointerEvents="none"
+                    >
+                      {idx + 1}
+                    </text>
+                    {idx < waypoints.length - 1 && (
+                      <line
+                        x1={canvasX}
+                        y1={canvasY}
+                        x2={centerX + (waypoints[idx + 1].x - robot.x) * zoom}
+                        y2={centerY - (waypoints[idx + 1].y - robot.y) * zoom}
+                        stroke="#10b981"
+                        strokeWidth="2"
+                        opacity="0.6"
+                      />
+                    )}
+                  </g>
+                );
+              })}
+              <circle
+                cx={containerRef.current?.clientWidth / 2 || 400}
+                cy={containerRef.current?.clientHeight / 2 || 300}
+                r="8"
+                fill="#8b5cf6"
+                opacity="1"
+              />
+            </svg>
+
+            <div className="absolute inset-0 flex items-center justify-center pointer-events-none text-gray-600 text-sm">
+              {waypoints.length === 0 && (
+                <div className="text-center">
+                  <div>Click to place waypoints</div>
+                  <div className="text-xs text-gray-700">Right-click to delete</div>
+                </div>
+              )}
+            </div>
+          </div>
+        </div>
+
+        {/* Info panel */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 text-xs text-gray-600 space-y-1">
+          <div className="flex justify-between">
+            <span>Waypoints:</span>
+            <span className="text-cyan-400">{waypoints.length}</span>
+          </div>
+          <div className="flex justify-between">
+            <span>Robot Position:</span>
+            <span className="text-cyan-400">
+              ({robotPose.x.toFixed(2)}, {robotPose.y.toFixed(2)})
+            </span>
+          </div>
+        </div>
+      </div>
+
+      {/* Waypoint list sidebar */}
+      <div className="w-64 flex flex-col bg-gray-950 rounded-lg border border-cyan-950 space-y-3 p-3">
+        <div className="flex items-center justify-between">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">WAYPOINTS</div>
+          <div className="text-gray-600 text-xs">{waypoints.length}</div>
+        </div>
+
+        {/* Waypoint list */}
+        <div className="flex-1 overflow-y-auto space-y-1">
+          {waypoints.length === 0 ? (
+            <div className="text-center text-gray-700 text-xs py-4">Click map to add waypoints</div>
+          ) : (
+            waypoints.map((wp, idx) => (
+              <div
+                key={wp.id}
+                draggable
+                onDragStart={(e) => handleWaypointDragStart(e, idx)}
+                onDragOver={(e) => {
+                  e.preventDefault();
+                  handleWaypointDragOver(e, idx);
+                }}
+                onDragEnd={handleWaypointDragEnd}
+                onClick={() => handleWaypointSelect(wp.id)}
+                onContextMenu={(e) => {
+                  e.preventDefault();
+                  handleDeleteWaypoint(wp.id);
+                }}
+                className={`p-2 rounded border text-xs cursor-move transition-colors ${
+                  wp.id === activeWaypoint
+                    ? 'bg-red-950 border-red-700 text-red-300'
+                    : wp.id === selectedWaypoint
+                    ? 'bg-amber-950 border-amber-700 text-amber-300'
+                    : 'bg-gray-900 border-gray-700 text-gray-400 hover:border-gray-600'
+                }`}
+              >
+                <div className="flex justify-between items-start gap-2">
+                  <div className="font-bold">#{idx + 1}</div>
+                  <div className="text-right flex-1">
+                    <div className="text-gray-500">{wp.label}</div>
+                    <div className="text-gray-600">
+                      {wp.x.toFixed(2)}, {wp.y.toFixed(2)}
+                    </div>
+                  </div>
+                </div>
+              </div>
+            ))
+          )}
+        </div>
+
+        {/* Control buttons */}
+        <div className="space-y-2 border-t border-gray-800 pt-3">
+          <button
+            onClick={sendSingleGoal}
+            disabled={selectedWaypoint === null || executing}
+            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-cyan-800 bg-cyan-950 text-cyan-400 hover:bg-cyan-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
+          >
+            SEND GOAL
+          </button>
+
+          <button
+            onClick={executeWaypoints}
+            disabled={waypoints.length === 0 || executing}
+            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-green-800 bg-green-950 text-green-400 hover:bg-green-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
+          >
+            {executing ? 'EXECUTING...' : 'EXECUTE ALL'}
+          </button>
+
+          <button
+            onClick={clearWaypoints}
+            disabled={waypoints.length === 0}
+            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-red-800 bg-red-950 text-red-400 hover:bg-red-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
+          >
+            CLEAR ALL
+          </button>
+        </div>
+
+        {/* Instructions */}
+        <div className="text-xs text-gray-600 space-y-1 border-t border-gray-800 pt-3">
+          <div className="font-bold text-gray-500">CONTROLS:</div>
+          <div>• Click: Place waypoint</div>
+          <div>• Right-click: Delete waypoint</div>
+          <div>• Drag: Reorder waypoints</div>
+          <div>• Click list: Select waypoint</div>
+        </div>
+
+        {/* Topic info */}
+        <div className="text-xs text-gray-600 border-t border-gray-800 pt-3">
+          <div className="flex justify-between">
+            <span>Service:</span>
+            <span className="text-gray-500">/navigate_to_pose</span>
+          </div>
+        </div>
+      </div>
+    </div>
+  );
+}
+
+export { WaypointEditor };