diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index 8e4be5f..73e1876 100644 --- a/ui/social-bot/src/App.jsx +++ b/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', @@ -244,8 +254,10 @@ export default function App() { )} - {activeTab === 'health' && } - {activeTab === 'cameras' && } + {activeTab === 'health' && } + {activeTab === 'cameras' && } + + {activeTab === 'waypoints' && } {activeTab === 'fleet' && } {activeTab === 'missions' && } diff --git a/ui/social-bot/src/components/WaypointEditor.jsx b/ui/social-bot/src/components/WaypointEditor.jsx new file mode 100644 index 0000000..3069d68 --- /dev/null +++ b/ui/social-bot/src/components/WaypointEditor.jsx @@ -0,0 +1,449 @@ +/** + * 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) + * - Imports map display from MapViewer for coordinate system + */ + +import { useEffect, useRef, useState } from 'react'; + +function WaypointEditor({ subscribe, publish, callService }) { + // Waypoint storage + 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); + + // Map context + const [mapData, setMapData] = useState(null); + const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 }); + + // Canvas reference + const canvasRef = useRef(null); + const containerRef = useRef(null); + + // Refs for ROS integration + const mapDataRef = useRef(null); + const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 }); + const waypointsRef = useRef([]); + + // Subscribe to map data (for coordinate reference) + 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 (for current position reference) + 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]); + + // Canvas event handlers + const handleCanvasClick = (e) => { + if (!mapDataRef.current || !canvasRef.current) return; + + const canvas = canvasRef.current; + const rect = canvas.getBoundingClientRect(); + const clickX = e.clientX - rect.left; + const clickY = e.clientY - rect.top; + + // Convert canvas coordinates to world coordinates + // This assumes the map is centered on the robot + const map = mapDataRef.current; + const robot = robotPoseRef.current; + const zoom = 1; // Would need to track zoom if map has zoom controls + + // Inverse of map rendering calculation + const centerX = canvas.width / 2; + const centerY = canvas.height / 2; + + const worldX = robot.x + (clickX - centerX) / zoom; + const worldY = robot.y - (clickY - centerY) / zoom; + + // Create new waypoint + 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 handleCanvasContextMenu = (e) => { + e.preventDefault(); + // Right-click handled by waypoint list + }; + + // Waypoint list handlers + 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); + }; + + // Execute waypoints + const sendNavGoal = async (waypoint) => { + if (!callService) return; + + try { + // Create quaternion from heading (default to 0 if no heading) + const heading = waypoint.theta || 0; + const halfHeading = heading / 2; + const qx = 0; + const qy = 0; + const qz = Math.sin(halfHeading); + const qw = Math.cos(halfHeading); + + const goal = { + pose: { + position: { + x: waypoint.x, + y: waypoint.y, + z: 0, + }, + orientation: { + x: qx, + y: qy, + z: qz, + w: qw, + }, + }, + }; + + // Send to Nav2 navigate_to_pose action + 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) { + console.error('Failed to send goal for waypoint:', waypoint); + break; + } + // Wait a bit before sending next goal + 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 ( +
+ {/* Map area with click handlers */} +
+
+
+ {/* Virtual map display - waypoints overlaid */} + + {/* Waypoint markers */} + {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 ( + + {/* Waypoint circle */} + + {/* Waypoint number */} + + {idx + 1} + + {/* Line to next waypoint */} + {idx < waypoints.length - 1 && ( + + )} + + ); + })} + + {/* Robot position marker */} + + + +
+ {waypoints.length === 0 && ( +
+
Click to place waypoints
+
Right-click to delete
+
+ )} +
+
+
+ + {/* Info panel */} +
+
+ Waypoints: + {waypoints.length} +
+
+ Robot Position: + + ({robotPose.x.toFixed(2)}, {robotPose.y.toFixed(2)}) + +
+
+
+ + {/* Waypoint list sidebar */} +
+
+
WAYPOINTS
+
{waypoints.length}
+
+ + {/* Waypoint list */} +
+ {waypoints.length === 0 ? ( +
+ Click map to add waypoints +
+ ) : ( + waypoints.map((wp, idx) => ( +
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' + }`} + > +
+
#{idx + 1}
+
+
{wp.label}
+
+ {wp.x.toFixed(2)}, {wp.y.toFixed(2)} +
+
+
+
+ )) + )} +
+ + {/* Control buttons */} +
+ + + + + +
+ + {/* Instructions */} +
+
CONTROLS:
+
• Click: Place waypoint
+
• Right-click: Delete waypoint
+
• Drag: Reorder waypoints
+
• Click list: Select waypoint
+
+ + {/* Topic info */} +
+
+ Service: + /navigate_to_pose +
+
+
+
+ ); +} + +export { WaypointEditor };