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,9 @@ 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';
+
 const TAB_GROUPS = [
   {
     label: 'SOCIAL',
@@ -85,6 +88,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',
@@ -247,6 +257,8 @@ export default function App() {
         {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/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 };