447 lines
14 KiB
JavaScript
447 lines
14 KiB
JavaScript
/**
|
||
* MapViewer.jsx — SLAM occupancy grid visualization with robot position overlay
|
||
*
|
||
* Features:
|
||
* - Subscribes to /map (nav_msgs/OccupancyGrid) for SLAM occupancy data
|
||
* - Real-time robot position from /odom (nav_msgs/Odometry)
|
||
* - Canvas-based rendering with efficient grid visualization
|
||
* - Pan/zoom controls with mouse/touch support
|
||
* - Costmap color coding: white=free, gray=unknown, black=occupied
|
||
* - Robot position overlay with heading indicator
|
||
* - Trail visualization of recent robot positions
|
||
* - Grid statistics (resolution, size, robot location)
|
||
*/
|
||
|
||
import { useEffect, useRef, useState } from 'react';
|
||
|
||
// Zoom and pan constants
|
||
const MIN_ZOOM = 0.5;
|
||
const MAX_ZOOM = 10;
|
||
const ZOOM_SPEED = 0.1;
|
||
const MAX_TRAIL_LENGTH = 300; // ~30 seconds at 10Hz
|
||
|
||
function MapViewer({ subscribe }) {
|
||
const canvasRef = useRef(null);
|
||
const containerRef = useRef(null);
|
||
|
||
// Map data
|
||
const [mapData, setMapData] = useState(null);
|
||
const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 });
|
||
const [robotTrail, setRobotTrail] = useState([]);
|
||
|
||
// View controls
|
||
const [zoom, setZoom] = useState(1);
|
||
const [panX, setPanX] = useState(0);
|
||
const [panY, setPanY] = useState(0);
|
||
const [isDragging, setIsDragging] = useState(false);
|
||
const dragStartRef = useRef({ x: 0, y: 0 });
|
||
|
||
// Refs for tracking state
|
||
const mapDataRef = useRef(null);
|
||
const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 });
|
||
const robotTrailRef = useRef([]);
|
||
|
||
// Subscribe to occupancy grid (SLAM map)
|
||
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,
|
||
data: msg.data || new Uint8Array(msg.info.width * msg.info.height),
|
||
};
|
||
setMapData(mapInfo);
|
||
mapDataRef.current = mapInfo;
|
||
} catch (e) {
|
||
console.error('Error parsing map data:', e);
|
||
}
|
||
}
|
||
);
|
||
return unsubMap;
|
||
}, [subscribe]);
|
||
|
||
// Subscribe to robot odometry (pose)
|
||
useEffect(() => {
|
||
const unsubOdom = subscribe(
|
||
'/odom',
|
||
'nav_msgs/Odometry',
|
||
(msg) => {
|
||
try {
|
||
const pos = msg.pose.pose.position;
|
||
const ori = msg.pose.pose.orientation;
|
||
|
||
// Convert quaternion to yaw angle
|
||
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;
|
||
|
||
// Add to trail
|
||
setRobotTrail((prev) => {
|
||
const updated = [...prev, { x: pos.x, y: pos.y }];
|
||
if (updated.length > MAX_TRAIL_LENGTH) {
|
||
updated.shift();
|
||
}
|
||
robotTrailRef.current = updated;
|
||
return updated;
|
||
});
|
||
} catch (e) {
|
||
console.error('Error parsing odometry data:', e);
|
||
}
|
||
}
|
||
);
|
||
return unsubOdom;
|
||
}, [subscribe]);
|
||
|
||
// Canvas rendering
|
||
useEffect(() => {
|
||
const canvas = canvasRef.current;
|
||
if (!canvas || !mapDataRef.current) return;
|
||
|
||
const ctx = canvas.getContext('2d');
|
||
const width = canvas.width;
|
||
const height = canvas.height;
|
||
|
||
// Clear canvas
|
||
ctx.fillStyle = '#1f2937';
|
||
ctx.fillRect(0, 0, width, height);
|
||
|
||
const map = mapDataRef.current;
|
||
const cellSize = Math.max(1, map.resolution * zoom);
|
||
|
||
// Calculate offset for centering and panning
|
||
const originX = map.origin.position.x;
|
||
const originY = map.origin.position.y;
|
||
|
||
const centerX = width / 2 + panX;
|
||
const centerY = height / 2 + panY;
|
||
|
||
// Draw occupancy grid
|
||
for (let y = 0; y < map.height; y++) {
|
||
for (let x = 0; x < map.width; x++) {
|
||
const idx = y * map.width + x;
|
||
const cell = map.data[idx] ?? -1;
|
||
|
||
// Convert grid coordinates to world coordinates
|
||
const worldX = originX + x * map.resolution;
|
||
const worldY = originY + y * map.resolution;
|
||
|
||
// Project to canvas
|
||
const canvasX = centerX + (worldX - robotPoseRef.current.x) * zoom;
|
||
const canvasY = centerY - (worldY - robotPoseRef.current.y) * zoom;
|
||
|
||
// Only draw cells on screen
|
||
if (canvasX >= -cellSize && canvasX <= width &&
|
||
canvasY >= -cellSize && canvasY <= height) {
|
||
|
||
// Color based on occupancy
|
||
if (cell >= 0 && cell < 25) {
|
||
ctx.fillStyle = '#f5f5f5'; // Free space (white)
|
||
} else if (cell >= 25 && cell < 65) {
|
||
ctx.fillStyle = '#a0a0a0'; // Unknown/gray
|
||
} else if (cell >= 65) {
|
||
ctx.fillStyle = '#000000'; // Occupied (black)
|
||
} else {
|
||
ctx.fillStyle = '#4b5563'; // Unknown (-1)
|
||
}
|
||
|
||
ctx.fillRect(canvasX, canvasY, cellSize, cellSize);
|
||
}
|
||
}
|
||
}
|
||
|
||
// Draw robot trail
|
||
if (robotTrailRef.current.length > 1) {
|
||
ctx.strokeStyle = '#10b981';
|
||
ctx.lineWidth = 2;
|
||
ctx.globalAlpha = 0.6;
|
||
ctx.beginPath();
|
||
|
||
const startPoint = robotTrailRef.current[0];
|
||
const sx = centerX + (startPoint.x - robotPoseRef.current.x) * zoom;
|
||
const sy = centerY - (startPoint.y - robotPoseRef.current.y) * zoom;
|
||
ctx.moveTo(sx, sy);
|
||
|
||
for (let i = 1; i < robotTrailRef.current.length; i++) {
|
||
const point = robotTrailRef.current[i];
|
||
const px = centerX + (point.x - robotPoseRef.current.x) * zoom;
|
||
const py = centerY - (point.y - robotPoseRef.current.y) * zoom;
|
||
ctx.lineTo(px, py);
|
||
}
|
||
ctx.stroke();
|
||
ctx.globalAlpha = 1.0;
|
||
}
|
||
|
||
// Draw robot position (at center, always)
|
||
const robotRadius = Math.max(8, 0.2 * zoom); // ~0.2m radius at zoom=1
|
||
ctx.fillStyle = '#06b6d4';
|
||
ctx.beginPath();
|
||
ctx.arc(centerX, centerY, robotRadius, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
|
||
// Draw robot heading indicator
|
||
const headingLen = robotRadius * 2.5;
|
||
const headX = centerX + Math.cos(robotPoseRef.current.theta) * headingLen;
|
||
const headY = centerY - Math.sin(robotPoseRef.current.theta) * headingLen;
|
||
|
||
ctx.strokeStyle = '#06b6d4';
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.moveTo(centerX, centerY);
|
||
ctx.lineTo(headX, headY);
|
||
ctx.stroke();
|
||
|
||
// Draw grid overlay (every 1 meter)
|
||
ctx.strokeStyle = '#374151';
|
||
ctx.lineWidth = 0.5;
|
||
ctx.globalAlpha = 0.3;
|
||
|
||
const gridSpacing = 1 * zoom; // 1 meter grid
|
||
for (let i = -100; i <= 100; i++) {
|
||
const x = centerX + i * gridSpacing;
|
||
const y = centerY + i * gridSpacing;
|
||
|
||
// Vertical lines
|
||
if (x > 0 && x < width) {
|
||
ctx.beginPath();
|
||
ctx.moveTo(x, 0);
|
||
ctx.lineTo(x, height);
|
||
ctx.stroke();
|
||
}
|
||
|
||
// Horizontal lines
|
||
if (y > 0 && y < height) {
|
||
ctx.beginPath();
|
||
ctx.moveTo(0, y);
|
||
ctx.lineTo(width, y);
|
||
ctx.stroke();
|
||
}
|
||
}
|
||
ctx.globalAlpha = 1.0;
|
||
}, [zoom, panX, panY]);
|
||
|
||
// Mouse wheel zoom
|
||
const handleWheel = (e) => {
|
||
e.preventDefault();
|
||
const delta = e.deltaY > 0 ? -ZOOM_SPEED : ZOOM_SPEED;
|
||
setZoom((prev) => Math.max(MIN_ZOOM, Math.min(MAX_ZOOM, prev + delta)));
|
||
};
|
||
|
||
// Mouse dragging for pan
|
||
const handleMouseDown = (e) => {
|
||
setIsDragging(true);
|
||
dragStartRef.current = { x: e.clientX, y: e.clientY };
|
||
};
|
||
|
||
const handleMouseMove = (e) => {
|
||
if (!isDragging) return;
|
||
|
||
const dx = e.clientX - dragStartRef.current.x;
|
||
const dy = e.clientY - dragStartRef.current.y;
|
||
|
||
setPanX((prev) => prev + dx);
|
||
setPanY((prev) => prev + dy);
|
||
|
||
dragStartRef.current = { x: e.clientX, y: e.clientY };
|
||
};
|
||
|
||
const handleMouseUp = () => {
|
||
setIsDragging(false);
|
||
};
|
||
|
||
// Touch support for mobile
|
||
const handleTouchStart = (e) => {
|
||
if (e.touches.length === 1) {
|
||
setIsDragging(true);
|
||
dragStartRef.current = { x: e.touches[0].clientX, y: e.touches[0].clientY };
|
||
}
|
||
};
|
||
|
||
const handleTouchMove = (e) => {
|
||
if (!isDragging || e.touches.length !== 1) return;
|
||
|
||
const dx = e.touches[0].clientX - dragStartRef.current.x;
|
||
const dy = e.touches[0].clientY - dragStartRef.current.y;
|
||
|
||
setPanX((prev) => prev + dx);
|
||
setPanY((prev) => prev + dy);
|
||
|
||
dragStartRef.current = { x: e.touches[0].clientX, y: e.touches[0].clientY };
|
||
};
|
||
|
||
const handleTouchEnd = () => {
|
||
setIsDragging(false);
|
||
};
|
||
|
||
// Setup event listeners
|
||
useEffect(() => {
|
||
const canvas = canvasRef.current;
|
||
if (!canvas) return;
|
||
|
||
canvas.addEventListener('wheel', handleWheel, { passive: false });
|
||
canvas.addEventListener('mousedown', handleMouseDown);
|
||
canvas.addEventListener('touchstart', handleTouchStart);
|
||
canvas.addEventListener('touchmove', handleTouchMove);
|
||
canvas.addEventListener('touchend', handleTouchEnd);
|
||
|
||
window.addEventListener('mousemove', handleMouseMove);
|
||
window.addEventListener('mouseup', handleMouseUp);
|
||
|
||
return () => {
|
||
canvas.removeEventListener('wheel', handleWheel);
|
||
canvas.removeEventListener('mousedown', handleMouseDown);
|
||
canvas.removeEventListener('touchstart', handleTouchStart);
|
||
canvas.removeEventListener('touchmove', handleTouchMove);
|
||
canvas.removeEventListener('touchend', handleTouchEnd);
|
||
window.removeEventListener('mousemove', handleMouseMove);
|
||
window.removeEventListener('mouseup', handleMouseUp);
|
||
};
|
||
}, [isDragging]);
|
||
|
||
const resetView = () => {
|
||
setZoom(1);
|
||
setPanX(0);
|
||
setPanY(0);
|
||
};
|
||
|
||
const clearTrail = () => {
|
||
setRobotTrail([]);
|
||
robotTrailRef.current = [];
|
||
};
|
||
|
||
const mapInfo = mapData ? {
|
||
width: mapData.width,
|
||
height: mapData.height,
|
||
area: (mapData.width * mapData.height * mapData.resolution * mapData.resolution).toFixed(1),
|
||
resolution: mapData.resolution.toFixed(3),
|
||
} : null;
|
||
|
||
const robotInfo = robotPose ? {
|
||
x: robotPose.x.toFixed(2),
|
||
y: robotPose.y.toFixed(2),
|
||
theta: (robotPose.theta * 180 / Math.PI).toFixed(1),
|
||
} : null;
|
||
|
||
return (
|
||
<div className="flex flex-col h-full space-y-3">
|
||
{/* Controls */}
|
||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-3">
|
||
<div className="flex justify-between items-center flex-wrap gap-2">
|
||
<div className="text-cyan-700 text-xs font-bold tracking-widest">
|
||
MAP VIEWER (SLAM)
|
||
</div>
|
||
<div className="text-gray-600 text-xs">
|
||
Zoom: {zoom.toFixed(2)}× | Trail: {robotTrail.length} points
|
||
</div>
|
||
</div>
|
||
|
||
{/* Control buttons */}
|
||
<div className="flex gap-2 flex-wrap">
|
||
<button
|
||
onClick={resetView}
|
||
className="px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-cyan-800 bg-cyan-950 text-cyan-400 hover:bg-cyan-900 transition-colors"
|
||
>
|
||
RESET VIEW
|
||
</button>
|
||
<button
|
||
onClick={clearTrail}
|
||
className="px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-green-800 bg-green-950 text-green-400 hover:bg-green-900 transition-colors"
|
||
>
|
||
CLEAR TRAIL
|
||
</button>
|
||
<div className="text-gray-600 text-xs flex items-center">
|
||
Scroll: Zoom | Drag: Pan
|
||
</div>
|
||
</div>
|
||
</div>
|
||
|
||
{/* Canvas */}
|
||
<div className="flex-1 bg-gray-900 rounded-lg border border-cyan-950 overflow-hidden">
|
||
<canvas
|
||
ref={canvasRef}
|
||
width={800}
|
||
height={600}
|
||
className="w-full h-full cursor-grab active:cursor-grabbing"
|
||
onWheel={handleWheel}
|
||
style={{ userSelect: 'none' }}
|
||
/>
|
||
</div>
|
||
|
||
{/* Info panels */}
|
||
<div className="grid grid-cols-2 gap-2 text-xs text-gray-600">
|
||
{/* Map info */}
|
||
<div className="bg-gray-950 rounded border border-gray-800 p-2 space-y-1">
|
||
<div className="text-cyan-700 font-bold">MAP</div>
|
||
{mapInfo ? (
|
||
<>
|
||
<div className="flex justify-between">
|
||
<span>Size:</span>
|
||
<span className="text-gray-400">{mapInfo.width} × {mapInfo.height} cells</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Resolution:</span>
|
||
<span className="text-gray-400">{mapInfo.resolution} m/cell</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Area:</span>
|
||
<span className="text-gray-400">{mapInfo.area} m²</span>
|
||
</div>
|
||
</>
|
||
) : (
|
||
<div className="text-gray-700">Waiting for map...</div>
|
||
)}
|
||
</div>
|
||
|
||
{/* Robot info */}
|
||
<div className="bg-gray-950 rounded border border-gray-800 p-2 space-y-1">
|
||
<div className="text-cyan-700 font-bold">ROBOT POSE</div>
|
||
{robotInfo ? (
|
||
<>
|
||
<div className="flex justify-between">
|
||
<span>X:</span>
|
||
<span className="text-cyan-400">{robotInfo.x} m</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Y:</span>
|
||
<span className="text-cyan-400">{robotInfo.y} m</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Heading:</span>
|
||
<span className="text-cyan-400">{robotInfo.theta}°</span>
|
||
</div>
|
||
</>
|
||
) : (
|
||
<div className="text-gray-700">Waiting for odometry...</div>
|
||
)}
|
||
</div>
|
||
</div>
|
||
|
||
{/* Topic info */}
|
||
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
|
||
<div className="flex justify-between">
|
||
<span>Map Topic:</span>
|
||
<span className="text-gray-500">/map (nav_msgs/OccupancyGrid)</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Odometry Topic:</span>
|
||
<span className="text-gray-500">/odom (nav_msgs/Odometry)</span>
|
||
</div>
|
||
<div className="flex justify-between">
|
||
<span>Legend:</span>
|
||
<span className="text-gray-500">⬜ Free | 🟦 Unknown | ⬛ Occupied | 🔵 Robot</span>
|
||
</div>
|
||
</div>
|
||
</div>
|
||
);
|
||
}
|
||
|
||
export { MapViewer };
|