diff --git a/ui/social-bot/src/components/MapViewer.jsx b/ui/social-bot/src/components/MapViewer.jsx index 3c1fdaa..572e9ba 100644 --- a/ui/social-bot/src/components/MapViewer.jsx +++ b/ui/social-bot/src/components/MapViewer.jsx @@ -1,298 +1,446 @@ /** - * MapViewer.jsx — 2D occupancy grid + robot pose + Nav2 path overlay. + * MapViewer.jsx — SLAM occupancy grid visualization with robot position overlay * - * Topics: - * /map (nav_msgs/OccupancyGrid) — SLAM/static map - * /odom (nav_msgs/Odometry) — robot position & heading - * /outdoor/route (nav_msgs/Path) — Nav2 / OSM route path - * - * NOTE: OccupancyGrid data can be large (384×384 = 150K cells). - * We decode on a worker-free canvas; map refreshes at topic rate - * (typically 0.1–1 Hz from SLAM), odom at ~10 Hz. + * 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, useCallback } from 'react'; +import { useEffect, useRef, useState } from 'react'; -const CELL_COLORS = { - unknown: '#1a1a2e', - free: '#0a1020', - occ: '#00ffff33', - occFull: '#00b8d9', -}; +// 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 quatToYaw(o) { - return Math.atan2(2 * (o.w * o.z + o.x * o.y), 1 - 2 * (o.y * o.y + o.z * o.z)); -} - -export function MapViewer({ subscribe }) { +function MapViewer({ subscribe }) { const canvasRef = useRef(null); - const mapRef = useRef(null); // last OccupancyGrid info - const odomRef = useRef(null); // last robot pose {x,y,yaw} - const pathRef = useRef([]); // [{x,y}] path points in map coords - const [mapInfo, setMapInfo] = useState(null); - const [odomPose, setOdomPose] = useState(null); - const [zoom, setZoom] = useState(1); - const [pan, setPan] = useState({ x: 0, y: 0 }); - const dragging = useRef(null); + const containerRef = useRef(null); - // ── Render ──────────────────────────────────────────────────────────────── - const render = useCallback(() => { + // 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; - const map = mapRef.current; - if (!canvas || !map) return; + if (!canvas || !mapDataRef.current) return; const ctx = canvas.getContext('2d'); - const W = canvas.width, H = canvas.height; + const width = canvas.width; + const height = canvas.height; - ctx.clearRect(0, 0, W, H); - ctx.fillStyle = CELL_COLORS.unknown; - ctx.fillRect(0, 0, W, H); + // Clear canvas + ctx.fillStyle = '#1f2937'; + ctx.fillRect(0, 0, width, height); - const { data, info } = map; - const mW = info.width, mH = info.height; - const res = info.resolution; // m/cell - const cellPx = zoom; // 1 map cell = zoom pixels + const map = mapDataRef.current; + const cellSize = Math.max(1, map.resolution * zoom); - // Canvas centre - const cx = W / 2 + pan.x; - const cy = H / 2 + pan.y; + // Calculate offset for centering and panning + const originX = map.origin.position.x; + const originY = map.origin.position.y; - // Map origin (bottom-left in world) → we flip y for canvas - const ox = info.origin.position.x; - const oy = info.origin.position.y; + const centerX = width / 2 + panX; + const centerY = height / 2 + panY; - // Draw map cells in chunks - const img = ctx.createImageData(W, H); - const imgData = img.data; + // 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; - for (let r = 0; r < mH; r++) { - for (let c = 0; c < mW; c++) { - const val = data[r * mW + c]; - // World coords of this cell centre - const wx = ox + (c + 0.5) * res; - const wy = oy + (r + 0.5) * res; - // Canvas coords (flip y) - const px = Math.round(cx + wx * cellPx / res); - const py = Math.round(cy - wy * cellPx / res); + // Convert grid coordinates to world coordinates + const worldX = originX + x * map.resolution; + const worldY = originY + y * map.resolution; - if (px < 0 || px >= W || py < 0 || py >= H) continue; + // Project to canvas + const canvasX = centerX + (worldX - robotPoseRef.current.x) * zoom; + const canvasY = centerY - (worldY - robotPoseRef.current.y) * zoom; - let ro, go, bo, ao; - if (val < 0) { ro=26; go=26; bo=46; ao=255; } // unknown - else if (val === 0) { ro=10; go=16; bo=32; ao=255; } // free - else if (val < 60) { ro=0; go=100; bo=120; ao=120; } // low occ - else { ro=0; go=184; bo=217; ao=220; } // occupied + // Only draw cells on screen + if (canvasX >= -cellSize && canvasX <= width && + canvasY >= -cellSize && canvasY <= height) { - const i = (py * W + px) * 4; - imgData[i] = ro; imgData[i+1] = go; - imgData[i+2] = bo; imgData[i+3] = ao; + // 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); + } } } - ctx.putImageData(img, 0, 0); - // ── Nav2 path ───────────────────────────────────────────────────────── - const path = pathRef.current; - if (path.length >= 2) { - ctx.strokeStyle = '#f59e0b'; + // Draw robot trail + if (robotTrailRef.current.length > 1) { + ctx.strokeStyle = '#10b981'; ctx.lineWidth = 2; - ctx.setLineDash([4, 4]); + ctx.globalAlpha = 0.6; ctx.beginPath(); - path.forEach(({ x, y }, i) => { - const px = cx + x * cellPx / res; - const py = cy - y * cellPx / res; - i === 0 ? ctx.moveTo(px, py) : ctx.lineTo(px, py); - }); + + 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.setLineDash([]); + ctx.globalAlpha = 1.0; } - // ── Robot ──────────────────────────────────────────────────────────── - const odom = odomRef.current; - if (odom) { - const rx = cx + odom.x * cellPx / res; - const ry = cy - odom.y * cellPx / res; + // 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(); - // Heading arrow - const arrowLen = Math.max(12, cellPx * 1.5); - ctx.strokeStyle = '#f97316'; - ctx.lineWidth = 2; - ctx.beginPath(); - ctx.moveTo(rx, ry); - ctx.lineTo(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen); - ctx.stroke(); + // 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; - // Robot circle - ctx.fillStyle = '#f97316'; - ctx.shadowBlur = 8; - ctx.shadowColor = '#f97316'; - ctx.beginPath(); - ctx.arc(rx, ry, 6, 0, Math.PI * 2); - ctx.fill(); - ctx.shadowBlur = 0; + ctx.strokeStyle = '#06b6d4'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(centerX, centerY); + ctx.lineTo(headX, headY); + ctx.stroke(); - // Heading dot - ctx.fillStyle = '#ffffff'; - ctx.beginPath(); - ctx.arc(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen, 3, 0, Math.PI * 2); - ctx.fill(); + // 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]); - // ── Scale bar ───────────────────────────────────────────────────────── - if (mapInfo) { - const scaleM = 2; // 2-metre scale bar - const scalePx = scaleM * cellPx / res; - const bx = 12, by = H - 12; - ctx.strokeStyle = '#06b6d4'; - ctx.lineWidth = 2; - ctx.beginPath(); - ctx.moveTo(bx, by); ctx.lineTo(bx + scalePx, by); - ctx.moveTo(bx, by - 4); ctx.lineTo(bx, by + 4); - ctx.moveTo(bx + scalePx, by - 4); ctx.lineTo(bx + scalePx, by + 4); - ctx.stroke(); - ctx.fillStyle = '#06b6d4'; - ctx.font = '9px monospace'; - ctx.textAlign = 'left'; - ctx.fillText(`${scaleM}m`, bx, by - 6); + // 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 }; } - }, [zoom, pan, mapInfo]); - - // ── Subscribe /map ──────────────────────────────────────────────────────── - useEffect(() => { - const unsub = subscribe('/map', 'nav_msgs/OccupancyGrid', (msg) => { - mapRef.current = msg; - setMapInfo(msg.info); - render(); - }); - return unsub; - }, [subscribe, render]); - - // ── Subscribe /odom ─────────────────────────────────────────────────────── - useEffect(() => { - const unsub = subscribe('/odom', 'nav_msgs/Odometry', (msg) => { - const p = msg.pose.pose.position; - const o = msg.pose.pose.orientation; - const pose = { x: p.x, y: p.y, yaw: quatToYaw(o) }; - odomRef.current = pose; - setOdomPose(pose); - render(); - }); - return unsub; - }, [subscribe, render]); - - // ── Subscribe /outdoor/route (Nav2 / OSM path) ──────────────────────────── - useEffect(() => { - const unsub = subscribe('/outdoor/route', 'nav_msgs/Path', (msg) => { - pathRef.current = (msg.poses ?? []).map(p => ({ - x: p.pose.position.x, - y: p.pose.position.y, - })); - render(); - }); - return unsub; - }, [subscribe, render]); - - // Re-render when zoom/pan changes - useEffect(() => { render(); }, [zoom, pan, render]); - - // ── Mouse pan ───────────────────────────────────────────────────────────── - const onMouseDown = (e) => { dragging.current = { x: e.clientX - pan.x, y: e.clientY - pan.y }; }; - const onMouseMove = (e) => { - if (!dragging.current) return; - setPan({ x: e.clientX - dragging.current.x, y: e.clientY - dragging.current.y }); }; - const onMouseUp = () => { dragging.current = null; }; - // Touch pan - const touchRef = useRef(null); - const onTouchStart = (e) => { - const t = e.touches[0]; - touchRef.current = { x: t.clientX - pan.x, y: t.clientY - pan.y }; + 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 onTouchMove = (e) => { - if (!touchRef.current) return; - const t = e.touches[0]; - setPan({ x: t.clientX - touchRef.current.x, y: t.clientY - touchRef.current.y }); + + 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 ( -
- {/* Toolbar */} -
-
MAP VIEWER
-
- - {zoom.toFixed(1)}x - - +
+ {/* Controls */} +
+
+
+ MAP VIEWER (SLAM) +
+
+ Zoom: {zoom.toFixed(2)}× | Trail: {robotTrail.length} points +
+
+ + {/* Control buttons */} +
+ + +
+ Scroll: Zoom | Drag: Pan +
{/* Canvas */} -
+
{ touchRef.current = null; }} + width={800} + height={600} + className="w-full h-full cursor-grab active:cursor-grabbing" + onWheel={handleWheel} + style={{ userSelect: 'none' }} />
- {/* Info bar */} -
-
-
MAP SIZE
-
- {mapInfo ? `${mapInfo.width}×${mapInfo.height}` : '—'} -
+ {/* Info panels */} +
+ {/* Map info */} +
+
MAP
+ {mapInfo ? ( + <> +
+ Size: + {mapInfo.width} × {mapInfo.height} cells +
+
+ Resolution: + {mapInfo.resolution} m/cell +
+
+ Area: + {mapInfo.area} m² +
+ + ) : ( +
Waiting for map...
+ )}
-
-
RESOLUTION
-
- {mapInfo ? `${mapInfo.resolution.toFixed(2)}m/cell` : '—'} -
-
-
-
ROBOT POS
-
- {odomPose ? `${odomPose.x.toFixed(2)}, ${odomPose.y.toFixed(2)}` : '—'} -
-
-
-
HEADING
-
- {odomPose ? `${(odomPose.yaw * 180 / Math.PI).toFixed(1)}°` : '—'} -
+ + {/* Robot info */} +
+
ROBOT POSE
+ {robotInfo ? ( + <> +
+ X: + {robotInfo.x} m +
+
+ Y: + {robotInfo.y} m +
+
+ Heading: + {robotInfo.theta}° +
+ + ) : ( +
Waiting for odometry...
+ )}
- {/* Legend */} -
-
-
Unknown + {/* Topic info */} +
+
+ Map Topic: + /map (nav_msgs/OccupancyGrid)
-
-
Free +
+ Odometry Topic: + /odom (nav_msgs/Odometry)
-
-
Occupied -
-
-
Path -
-
-
Robot +
+ Legend: + ⬜ Free | 🟦 Unknown | ⬛ Occupied | 🔵 Robot
); } + +export { MapViewer };