feat(webui): waypoint editor with click-to-navigate (Issue #261)
This commit is contained in:
parent
94a6f0787e
commit
d6ee5a16b7
@ -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() {
|
||||
</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 />}
|
||||
|
||||
449
ui/social-bot/src/components/WaypointEditor.jsx
Normal file
449
ui/social-bot/src/components/WaypointEditor.jsx
Normal file
@ -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 (
|
||||
<div className="flex h-full gap-3">
|
||||
{/* Map area with click handlers */}
|
||||
<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={handleCanvasContextMenu}
|
||||
>
|
||||
{/* Virtual map display - waypoints overlaid */}
|
||||
<svg
|
||||
className="absolute inset-0 w-full h-full pointer-events-none"
|
||||
id="waypoint-overlay"
|
||||
>
|
||||
{/* 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 (
|
||||
<g key={wp.id}>
|
||||
{/* Waypoint circle */}
|
||||
<circle
|
||||
cx={canvasX}
|
||||
cy={canvasY}
|
||||
r="10"
|
||||
fill={isActive ? '#ef4444' : isSelected ? '#fbbf24' : '#06b6d4'}
|
||||
opacity="0.8"
|
||||
/>
|
||||
{/* Waypoint number */}
|
||||
<text
|
||||
x={canvasX}
|
||||
y={canvasY}
|
||||
textAnchor="middle"
|
||||
dominantBaseline="middle"
|
||||
fill="white"
|
||||
fontSize="10"
|
||||
fontWeight="bold"
|
||||
pointerEvents="none"
|
||||
>
|
||||
{idx + 1}
|
||||
</text>
|
||||
{/* Line to next waypoint */}
|
||||
{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>
|
||||
);
|
||||
})}
|
||||
|
||||
{/* Robot position marker */}
|
||||
<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 };
|
||||
Loading…
x
Reference in New Issue
Block a user