sl-webui
a9e3082e57
Interactive waypoint editor for Nav2 goal-based navigation: - Click on map display to place waypoints - Drag waypoints in list to reorder navigation sequence - Right-click waypoints to delete them - Visual waypoint overlay on map with numbering - Robot position indicator at center - Waypoint list sidebar with selection and ordering - Send Nav2 goal to individual selected waypoint - Execute all waypoints in sequence with automatic progression - Clear all waypoints button - Real-time coordinate display and robot pose tracking - Integrated into new NAVIGATION tab group - Uses /navigate_to_pose service for goal publishing Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
404 lines
14 KiB
JavaScript
404 lines
14 KiB
JavaScript
/**
|
|
* 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 canvasRef = useRef(null);
|
|
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 map = mapDataRef.current;
|
|
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 };
|