diff --git a/ui/map_panel.css b/ui/map_panel.css new file mode 100644 index 0000000..30be7f6 --- /dev/null +++ b/ui/map_panel.css @@ -0,0 +1,236 @@ +/* map_panel.css — Saltybot 2D Map View (Issue #587) */ + +*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; } + +:root { + --bg0: #050510; + --bg1: #070712; + --bg2: #0a0a1a; + --bd: #0c2a3a; + --bd2: #1e3a5f; + --dim: #374151; + --mid: #6b7280; + --base: #9ca3af; + --hi: #d1d5db; + --cyan: #06b6d4; + --green: #22c55e; + --amber: #f59e0b; + --red: #ef4444; +} + +body { + font-family: 'Courier New', Courier, monospace; + font-size: 12px; + background: var(--bg0); + color: var(--base); + height: 100dvh; + display: flex; + flex-direction: column; + overflow: hidden; +} + +/* ── Header ── */ +#header { + display: flex; + align-items: center; + padding: 5px 12px; + background: var(--bg1); + border-bottom: 1px solid var(--bd); + flex-shrink: 0; + gap: 8px; + flex-wrap: wrap; +} +.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; } + +#conn-dot { + width: 8px; height: 8px; border-radius: 50%; + background: var(--dim); flex-shrink: 0; transition: background .3s; +} +#conn-dot.connected { background: var(--green); } +#conn-dot.error { background: var(--red); animation: blink 1s infinite; } + +@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} } + +#ws-input { + background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px; + color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px; width: 185px; +} +#ws-input:focus { outline: none; border-color: var(--cyan); } + +.hbtn { + padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2); + background: var(--bg2); color: #67e8f9; font-family: monospace; + font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s; + white-space: nowrap; +} +.hbtn:hover { background: #0e4f69; } +.hbtn.on { background: #0e4f69; border-color: var(--cyan); color: #fff; } +.hbtn.warn-on { background: #3d1a00; border-color: #92400e; color: #fbbf24; } + +/* ── Toolbar ── */ +#toolbar { + display: flex; + align-items: center; + gap: 8px; + padding: 4px 12px; + background: var(--bg1); + border-bottom: 1px solid var(--bd); + flex-shrink: 0; + flex-wrap: wrap; + font-size: 10px; +} +.tsep { width: 1px; height: 16px; background: var(--bd2); } + +#zoom-display { color: var(--mid); min-width: 45px; } + +/* ── Main: map + sidebar ── */ +#main { + flex: 1; + display: grid; + grid-template-columns: 1fr 240px; + min-height: 0; +} + +@media (max-width: 700px) { + #main { grid-template-columns: 1fr; } + #sidebar { display: none; } +} + +/* ── Canvas ── */ +#map-canvas { + display: block; + width: 100%; + height: 100%; + cursor: grab; + touch-action: none; +} +#map-canvas.dragging { cursor: grabbing; } +#map-wrap { + position: relative; + overflow: hidden; + background: #010108; +} + +/* No-signal overlay */ +#no-signal { + position: absolute; + inset: 0; + display: flex; + flex-direction: column; + align-items: center; + justify-content: center; + gap: 8px; + color: var(--dim); + pointer-events: none; +} +#no-signal.hidden { display: none; } +#no-signal .icon { font-size: 48px; } + +/* E-stop overlay */ +#estop-overlay { + position: absolute; + top: 8px; left: 50%; transform: translateX(-50%); + background: rgba(127,0,0,0.85); + border: 2px solid #ef4444; + color: #fca5a5; + padding: 4px 14px; + border-radius: 4px; + font-weight: bold; + font-size: 11px; + letter-spacing: .1em; + pointer-events: none; + display: none; + animation: blink 1s infinite; +} +#estop-overlay.visible { display: block; } + +/* Mouse coords HUD */ +#coords-hud { + position: absolute; + bottom: 6px; left: 8px; + background: rgba(0,0,0,.7); + color: var(--mid); + padding: 2px 6px; + border-radius: 3px; + font-size: 10px; + pointer-events: none; +} + +/* ── Sidebar ── */ +#sidebar { + background: var(--bg1); + border-left: 1px solid var(--bd); + display: flex; + flex-direction: column; + overflow-y: auto; + padding: 8px; + gap: 8px; + font-size: 11px; +} +.sb-card { + background: var(--bg2); + border: 1px solid var(--bd2); + border-radius: 6px; + padding: 8px; +} +.sb-title { + font-size: 9px; font-weight: bold; letter-spacing: .12em; + color: #0891b2; text-transform: uppercase; margin-bottom: 6px; +} +.sb-row { + display: flex; justify-content: space-between; + padding: 2px 0; border-bottom: 1px solid var(--bd); + font-size: 10px; +} +.sb-row:last-child { border-bottom: none; } +.sb-lbl { color: var(--mid); } +.sb-val { color: var(--hi); font-family: monospace; } + +/* Status dots */ +.sdot { + width: 8px; height: 8px; border-radius: 50%; + display: inline-block; margin-right: 4px; +} +.sdot.green { background: var(--green); } +.sdot.amber { background: var(--amber); } +.sdot.red { background: var(--red); animation: blink .8s infinite; } +.sdot.gray { background: var(--dim); } + +/* Legend ── */ +.legend-row { + display: flex; align-items: center; gap: 6px; + font-size: 10px; color: var(--base); padding: 2px 0; +} +.legend-swatch { + width: 20px; height: 3px; border-radius: 2px; flex-shrink: 0; +} + +/* Anchor list */ +#anchor-list .anchor-row { + display: flex; align-items: center; gap: 4px; + padding: 2px 0; font-size: 10px; color: var(--base); + border-bottom: 1px solid var(--bd); +} +#anchor-list .anchor-row:last-child { border-bottom: none; } +#anchor-add { width: 100%; margin-top: 4px; } + +/* Anchor input row */ +.anchor-inputs { + display: grid; grid-template-columns: 1fr 1fr 1fr; + gap: 4px; margin-top: 4px; +} +.anchor-inputs input { + background: var(--bg0); border: 1px solid var(--bd2); + border-radius: 3px; color: var(--hi); padding: 2px 4px; + font-family: monospace; font-size: 10px; text-align: center; + width: 100%; +} +.anchor-inputs input:focus { outline: none; border-color: var(--cyan); } + +/* ── Footer ── */ +#footer { + background: var(--bg1); border-top: 1px solid var(--bd); + padding: 3px 12px; + display: flex; align-items: center; justify-content: space-between; + flex-shrink: 0; font-size: 10px; color: var(--dim); +} diff --git a/ui/map_panel.html b/ui/map_panel.html new file mode 100644 index 0000000..d4026e2 --- /dev/null +++ b/ui/map_panel.html @@ -0,0 +1,176 @@ + + + + + +Saltybot — Map View + + + + + + + + + +
+ + + 1.00x + +
+ + + + +
+ + + +
+ + +
+
+
+ LIDAR scan +
+
+
+ Danger zone (0.30m) +
+
+
+
+
+ Warn zone (1.00m) +
+
+
+ Trail (100 pts) +
+
+
+
+ UWB anchor +
+
+ + +
+ + +
+ + + +
+
🗺️
+
Connect to rosbridge to view map
+
+ /saltybot/pose/fused · /scan · /saltybot/safety_zone/status +
+
+ + +
🛑 E-STOP ACTIVE
+ + +
(0.00, 0.00) m
+
+ + + +
+ + + + + + + + diff --git a/ui/map_panel.js b/ui/map_panel.js new file mode 100644 index 0000000..9ec1d92 --- /dev/null +++ b/ui/map_panel.js @@ -0,0 +1,607 @@ +/** + * map_panel.js — Saltybot 2D Map View (Issue #587) + * + * Canvas-based 2D map with: + * - Robot position from /saltybot/pose/fused (geometry_msgs/PoseStamped) + * - RPLIDAR scan overlay from /scan (sensor_msgs/LaserScan) + * - Safety zone rings at danger (0.30m) and warn (1.00m) + * - /saltybot/safety_zone/status JSON — e-stop + closest obstacle + * - UWB anchor markers (user-configured or via /saltybot/uwb/anchors) + * - 100-position breadcrumb trail + * - Zoom (wheel) and pan (drag) with touch support + * - Auto-center toggle + * + * World → canvas: cx = origin.x + x * ppm + * cy = origin.y - y * ppm (Y flipped) + */ + +'use strict'; + +// ── Config ──────────────────────────────────────────────────────────────────── + +const DANGER_R = 0.30; // m — matches safety_zone_params.yaml default +const WARN_R = 1.00; // m +const TRAIL_MAX = 100; +const SCAN_THROTTLE = 100; // ms — don't render every scan packet +const GRID_SPACING_M = 1.0; // world metres per grid square +const PIXELS_PER_M = 80; // initial scale +const MIN_PPM = 10; +const MAX_PPM = 600; + +// ── State ───────────────────────────────────────────────────────────────────── + +let ros = null, poseSub = null, scanSub = null, statusSub = null; + +const state = { + // Robot + robot: { x: 0, y: 0, theta: 0 }, // world metres + radians + trail: [], // [{x,y}] + + // Scan + scan: null, // { angle_min, angle_increment, ranges[] } + scanTs: 0, + + // Safety status + estop: false, + fwdZone: 'CLEAR', + closestM: null, + dangerN: 0, + warnN: 0, + + // UWB anchors [{x,y,label}] + anchors: [], + + // View + autoCenter: true, + ppm: PIXELS_PER_M, // pixels per metre + originX: 0, // canvas px where world (0,0) is + originY: 0, +}; + +// ── Canvas ──────────────────────────────────────────────────────────────────── + +const canvas = document.getElementById('map-canvas'); +const ctx = canvas.getContext('2d'); +const wrap = document.getElementById('map-wrap'); + +function resize() { + canvas.width = wrap.clientWidth || 600; + canvas.height = wrap.clientHeight || 400; + if (state.autoCenter) centerOnRobot(); + draw(); +} + +window.addEventListener('resize', resize); + +// ── World ↔ canvas coordinate helpers ──────────────────────────────────────── + +function w2cx(wx) { return state.originX + wx * state.ppm; } +function w2cy(wy) { return state.originY - wy * state.ppm; } +function c2wx(cx) { return (cx - state.originX) / state.ppm; } +function c2wy(cy) { return -(cy - state.originY) / state.ppm; } + +function yawFromQuat(qx, qy, qz, qw) { + return Math.atan2(2*(qw*qz + qx*qy), 1 - 2*(qy*qy + qz*qz)); +} + +// ── Draw ────────────────────────────────────────────────────────────────────── + +function draw() { + const W = canvas.width, H = canvas.height; + ctx.clearRect(0, 0, W, H); + + // Background + ctx.fillStyle = '#010108'; + ctx.fillRect(0, 0, W, H); + + drawGrid(W, H); + drawScan(); + drawSafetyZones(); + drawTrail(); + drawAnchors(); + drawRobot(); + updateHUD(); +} + +// Grid lines +function drawGrid(W, H) { + const ppm = state.ppm; + const step = GRID_SPACING_M * ppm; + const ox = ((state.originX % step) + step) % step; + const oy = ((state.originY % step) + step) % step; + + ctx.strokeStyle = '#0d1a2a'; + ctx.lineWidth = 1; + ctx.beginPath(); + for (let x = ox; x < W; x += step) { ctx.moveTo(x,0); ctx.lineTo(x,H); } + for (let y = oy; y < H; y += step) { ctx.moveTo(0,y); ctx.lineTo(W,y); } + ctx.stroke(); + + // Axis cross at world origin + const ox0 = state.originX, oy0 = state.originY; + if (ox0 > 0 && ox0 < W) { + ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1; + ctx.beginPath(); ctx.moveTo(ox0, 0); ctx.lineTo(ox0, H); ctx.stroke(); + } + if (oy0 > 0 && oy0 < H) { + ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1; + ctx.beginPath(); ctx.moveTo(0, oy0); ctx.lineTo(W, oy0); ctx.stroke(); + } + + // Scale bar (bottom right) + const barM = 1.0; + const barPx = barM * ppm; + const bx = W - 20, by = H - 12; + ctx.strokeStyle = '#374151'; ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(bx - barPx, by); ctx.lineTo(bx, by); + ctx.moveTo(bx - barPx, by - 4); ctx.lineTo(bx - barPx, by + 4); + ctx.moveTo(bx, by - 4); ctx.lineTo(bx, by + 4); + ctx.stroke(); + ctx.fillStyle = '#6b7280'; ctx.font = '9px Courier New'; ctx.textAlign = 'right'; + ctx.fillText(`${barM}m`, bx, by - 6); + ctx.textAlign = 'left'; +} + +// LIDAR scan +function drawScan() { + if (!state.scan) return; + const { angle_min, angle_increment, ranges, range_max } = state.scan; + const rx = state.robot.x, ry = state.robot.y, th = state.robot.theta; + const maxR = range_max || 12; + + ctx.fillStyle = 'rgba(34,197,94,0.75)'; + for (let i = 0; i < ranges.length; i++) { + const r = ranges[i]; + if (!isFinite(r) || r <= 0.02 || r >= maxR) continue; + const a = th + angle_min + i * angle_increment; + const wx = rx + r * Math.cos(a); + const wy = ry + r * Math.sin(a); + const cx_ = w2cx(wx), cy_ = w2cy(wy); + ctx.beginPath(); + ctx.arc(cx_, cy_, 1.5, 0, Math.PI * 2); + ctx.fill(); + } +} + +// Safety zone rings (drawn around robot) +function drawSafetyZones() { + const cx_ = w2cx(state.robot.x), cy_ = w2cy(state.robot.y); + const ppm = state.ppm; + + // WARN ring (amber) + const warnEstop = state.estop; + ctx.strokeStyle = warnEstop ? 'rgba(239,68,68,0.35)' : 'rgba(245,158,11,0.35)'; + ctx.lineWidth = 1; + ctx.setLineDash([4, 4]); + ctx.beginPath(); + ctx.arc(cx_, cy_, WARN_R * ppm, 0, Math.PI * 2); + ctx.stroke(); + + // DANGER ring (red) + ctx.strokeStyle = 'rgba(239,68,68,0.55)'; + ctx.lineWidth = 1.5; + ctx.setLineDash([3, 3]); + ctx.beginPath(); + ctx.arc(cx_, cy_, DANGER_R * ppm, 0, Math.PI * 2); + ctx.stroke(); + + ctx.setLineDash([]); +} + +// Breadcrumb trail +function drawTrail() { + const trail = state.trail; + if (trail.length < 2) return; + ctx.lineWidth = 1.5; + for (let i = 1; i < trail.length; i++) { + const alpha = i / trail.length; + ctx.strokeStyle = `rgba(6,182,212,${alpha * 0.6})`; + ctx.beginPath(); + ctx.moveTo(w2cx(trail[i-1].x), w2cy(trail[i-1].y)); + ctx.lineTo(w2cx(trail[i].x), w2cy(trail[i].y)); + ctx.stroke(); + } + // Trail dots at every 5th point + ctx.fillStyle = 'rgba(6,182,212,0.4)'; + for (let i = 0; i < trail.length; i += 5) { + ctx.beginPath(); + ctx.arc(w2cx(trail[i].x), w2cy(trail[i].y), 2, 0, Math.PI * 2); + ctx.fill(); + } +} + +// UWB anchor markers +function drawAnchors() { + for (const a of state.anchors) { + const cx_ = w2cx(a.x), cy_ = w2cy(a.y); + const r = 7; + // Diamond shape + ctx.strokeStyle = '#f59e0b'; + ctx.lineWidth = 1.5; + ctx.beginPath(); + ctx.moveTo(cx_, cy_ - r); + ctx.lineTo(cx_ + r, cy_); + ctx.lineTo(cx_, cy_ + r); + ctx.lineTo(cx_ - r, cy_); + ctx.closePath(); + ctx.stroke(); + ctx.fillStyle = 'rgba(245,158,11,0.15)'; + ctx.fill(); + // Label + ctx.fillStyle = '#fcd34d'; + ctx.font = 'bold 9px Courier New'; + ctx.textAlign = 'center'; + ctx.fillText(a.label || '⚓', cx_, cy_ - r - 3); + ctx.textAlign = 'left'; + } +} + +// Robot marker +function drawRobot() { + const cx_ = w2cx(state.robot.x), cy_ = w2cy(state.robot.y); + const th = state.robot.theta; + const r = 10; + + ctx.save(); + ctx.translate(cx_, cy_); + ctx.rotate(-th); // canvas Y is flipped so negate + + // Body circle + const isEstop = state.estop; + ctx.strokeStyle = isEstop ? '#ef4444' : '#22d3ee'; + ctx.fillStyle = isEstop ? 'rgba(239,68,68,0.2)' : 'rgba(34,211,238,0.15)'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.arc(0, 0, r, 0, Math.PI * 2); + ctx.fill(); + ctx.stroke(); + + // Forward arrow + ctx.strokeStyle = isEstop ? '#f87171' : '#67e8f9'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(0, 0); + ctx.lineTo(r + 4, 0); + ctx.stroke(); + // Arrowhead + ctx.fillStyle = isEstop ? '#f87171' : '#67e8f9'; + ctx.beginPath(); + ctx.moveTo(r + 4, 0); + ctx.lineTo(r + 1, -3); + ctx.lineTo(r + 1, 3); + ctx.closePath(); + ctx.fill(); + + ctx.restore(); +} + +// ── HUD ─────────────────────────────────────────────────────────────────────── + +function updateHUD() { + document.getElementById('zoom-display').textContent = + (state.ppm / PIXELS_PER_M).toFixed(2) + 'x'; + updateSidebar(); +} + +function updateSidebar() { + setText('sb-pos', `${state.robot.x.toFixed(2)}, ${state.robot.y.toFixed(2)} m`); + setText('sb-hdg', (state.robot.theta * 180 / Math.PI).toFixed(1) + '°'); + setText('sb-trail', state.trail.length + ' pts'); + setText('sb-closest', + state.closestM != null ? state.closestM.toFixed(2) + ' m' : '—'); + setText('sb-fwd', state.fwdZone); + + // Zone status dot + const dot = document.getElementById('sb-zone-dot'); + if (dot) { + dot.className = 'sdot ' + ( + state.estop ? 'red' : + state.fwdZone === 'DANGER' ? 'red' : + state.fwdZone === 'WARN' ? 'amber' : 'green' + ); + } + + // E-stop overlay + const ov = document.getElementById('estop-overlay'); + if (ov) ov.classList.toggle('visible', state.estop); + + // ESTOP badge in sidebar + setText('sb-estop', state.estop ? 'ACTIVE' : 'clear'); + const estopEl = document.getElementById('sb-estop'); + if (estopEl) estopEl.style.color = state.estop ? '#ef4444' : '#22c55e'; +} + +function setText(id, val) { + const el = document.getElementById(id); + if (el) el.textContent = val ?? '—'; +} + +// Coords HUD on mouse move +canvas.addEventListener('mousemove', (e) => { + const rect = canvas.getBoundingClientRect(); + const scaleX = canvas.width / rect.width; + const scaleY = canvas.height / rect.height; + const cx_ = (e.clientX - rect.left) * scaleX; + const cy_ = (e.clientY - rect.top) * scaleY; + const wx = c2wx(cx_).toFixed(2); + const wy = c2wy(cy_).toFixed(2); + document.getElementById('coords-hud').textContent = `(${wx}, ${wy}) m`; +}); + +// ── Zoom & pan ──────────────────────────────────────────────────────────────── + +let dragging = false, dragStart = { cx: 0, cy: 0, ox: 0, oy: 0 }; +let pinchDist = null; + +canvas.addEventListener('wheel', (e) => { + e.preventDefault(); + const rect = canvas.getBoundingClientRect(); + const mx = (e.clientX - rect.left) * (canvas.width / rect.width); + const my = (e.clientY - rect.top) * (canvas.height / rect.height); + const factor = e.deltaY < 0 ? 1.12 : 0.89; + const newPpm = Math.max(MIN_PPM, Math.min(MAX_PPM, state.ppm * factor)); + + // Zoom around cursor position + state.originX = mx - (mx - state.originX) * (newPpm / state.ppm); + state.originY = my - (my - state.originY) * (newPpm / state.ppm); + state.ppm = newPpm; + state.autoCenter = false; + document.getElementById('btn-center').classList.remove('on'); + draw(); +}, { passive: false }); + +canvas.addEventListener('pointerdown', (e) => { + if (e.pointerType === 'touch') return; + dragging = true; + canvas.classList.add('dragging'); + canvas.setPointerCapture(e.pointerId); + dragStart = { cx: e.clientX, cy: e.clientY, ox: state.originX, oy: state.originY }; +}); +canvas.addEventListener('pointermove', (e) => { + if (!dragging) return; + const dx = e.clientX - dragStart.cx; + const dy = e.clientY - dragStart.cy; + state.originX = dragStart.ox + dx; + state.originY = dragStart.oy + dy; + state.autoCenter = false; + document.getElementById('btn-center').classList.remove('on'); + draw(); +}); +canvas.addEventListener('pointerup', () => { + dragging = false; + canvas.classList.remove('dragging'); +}); +canvas.addEventListener('pointercancel', () => { + dragging = false; + canvas.classList.remove('dragging'); +}); + +// Touch pinch-to-zoom +canvas.addEventListener('touchstart', (e) => { + if (e.touches.length === 2) { + pinchDist = touchDist(e.touches); + } +}, { passive: true }); +canvas.addEventListener('touchmove', (e) => { + if (e.touches.length === 2 && pinchDist != null) { + const d = touchDist(e.touches); + const factor = d / pinchDist; + pinchDist = d; + const newPpm = Math.max(MIN_PPM, Math.min(MAX_PPM, state.ppm * factor)); + const mx = (e.touches[0].clientX + e.touches[1].clientX) / 2; + const my = (e.touches[0].clientY + e.touches[1].clientY) / 2; + const rect = canvas.getBoundingClientRect(); + const cx_ = (mx - rect.left) * (canvas.width / rect.width); + const cy_ = (my - rect.top) * (canvas.height / rect.height); + state.originX = cx_ - (cx_ - state.originX) * (newPpm / state.ppm); + state.originY = cy_ - (cy_ - state.originY) * (newPpm / state.ppm); + state.ppm = newPpm; + draw(); + } +}, { passive: true }); +canvas.addEventListener('touchend', () => { pinchDist = null; }, { passive: true }); + +function touchDist(touches) { + const dx = touches[0].clientX - touches[1].clientX; + const dy = touches[0].clientY - touches[1].clientY; + return Math.sqrt(dx*dx + dy*dy); +} + +// ── Auto-center ─────────────────────────────────────────────────────────────── + +function centerOnRobot() { + state.originX = canvas.width / 2 - state.robot.x * state.ppm; + state.originY = canvas.height / 2 + state.robot.y * state.ppm; +} + +document.getElementById('btn-center').addEventListener('click', () => { + state.autoCenter = !state.autoCenter; + document.getElementById('btn-center').classList.toggle('on', state.autoCenter); + if (state.autoCenter) { centerOnRobot(); draw(); } +}); + +document.getElementById('btn-zoom-in').addEventListener('click', () => { + state.ppm = Math.min(MAX_PPM, state.ppm * 1.4); + if (state.autoCenter) centerOnRobot(); + draw(); +}); +document.getElementById('btn-zoom-out').addEventListener('click', () => { + state.ppm = Math.max(MIN_PPM, state.ppm / 1.4); + if (state.autoCenter) centerOnRobot(); + draw(); +}); +document.getElementById('btn-reset').addEventListener('click', () => { + state.ppm = PIXELS_PER_M; + state.autoCenter = true; + document.getElementById('btn-center').classList.add('on'); + centerOnRobot(); draw(); +}); + +// ── ROS connection ──────────────────────────────────────────────────────────── + +function connect() { + const url = document.getElementById('ws-input').value.trim(); + if (!url) return; + if (ros) ros.close(); + + ros = new ROSLIB.Ros({ url }); + ros.on('connection', () => { + document.getElementById('conn-dot').className = 'connected'; + document.getElementById('conn-label').textContent = url; + document.getElementById('btn-connect').textContent = 'RECONNECT'; + document.getElementById('no-signal').classList.add('hidden'); + setupSubs(); + }); + ros.on('error', (err) => { + document.getElementById('conn-dot').className = 'error'; + document.getElementById('conn-label').textContent = 'ERR: ' + (err?.message || err); + teardown(); + }); + ros.on('close', () => { + document.getElementById('conn-dot').className = ''; + document.getElementById('conn-label').textContent = 'Disconnected'; + teardown(); + }); +} + +function setupSubs() { + // Fused pose + poseSub = new ROSLIB.Topic({ + ros, name: '/saltybot/pose/fused', + messageType: 'geometry_msgs/PoseStamped', + throttle_rate: 50, + }); + poseSub.subscribe((msg) => { + const p = msg.pose.position; + const q = msg.pose.orientation; + const th = yawFromQuat(q.x, q.y, q.z, q.w); + state.robot = { x: p.x, y: p.y, theta: th }; + + state.trail.push({ x: p.x, y: p.y }); + if (state.trail.length > TRAIL_MAX) state.trail.shift(); + + if (state.autoCenter) centerOnRobot(); + requestDraw(); + }); + + // RPLIDAR scan + scanSub = new ROSLIB.Topic({ + ros, name: '/scan', + messageType: 'sensor_msgs/LaserScan', + throttle_rate: SCAN_THROTTLE, + compression: 'cbor', + }); + scanSub.subscribe((msg) => { + state.scan = { + angle_min: msg.angle_min, + angle_increment: msg.angle_increment, + ranges: msg.ranges, + range_max: msg.range_max, + }; + requestDraw(); + }); + + // Safety zone status + statusSub = new ROSLIB.Topic({ + ros, name: '/saltybot/safety_zone/status', + messageType: 'std_msgs/String', + throttle_rate: 200, + }); + statusSub.subscribe((msg) => { + try { + const d = JSON.parse(msg.data); + state.estop = d.estop_active ?? false; + state.fwdZone = d.forward_zone ?? 'CLEAR'; + state.closestM = d.closest_obstacle_m ?? null; + state.dangerN = d.danger_sector_count ?? 0; + state.warnN = d.warn_sector_count ?? 0; + } catch (_) {} + requestDraw(); + }); +} + +function teardown() { + if (poseSub) { poseSub.unsubscribe(); poseSub = null; } + if (scanSub) { scanSub.unsubscribe(); scanSub = null; } + if (statusSub) { statusSub.unsubscribe(); statusSub = null; } + document.getElementById('no-signal').classList.remove('hidden'); +} + +// Batch draw calls +let drawPending = false; +function requestDraw() { + if (drawPending) return; + drawPending = true; + requestAnimationFrame(() => { drawPending = false; draw(); }); +} + +// ── UWB Anchors ─────────────────────────────────────────────────────────────── + +function renderAnchorList() { + const list = document.getElementById('anchor-list'); + if (!list) return; + list.innerHTML = state.anchors.map((a, i) => + `
+ ◆ + ${a.label} (${a.x.toFixed(1)}, ${a.y.toFixed(1)}) + +
` + ).join('') || '
No anchors
'; +} + +window.removeAnchor = function(i) { + state.anchors.splice(i, 1); + saveAnchors(); + renderAnchorList(); + draw(); +}; + +document.getElementById('btn-add-anchor').addEventListener('click', () => { + const x = parseFloat(document.getElementById('anc-x').value); + const y = parseFloat(document.getElementById('anc-y').value); + const lbl = document.getElementById('anc-lbl').value.trim() || `A${state.anchors.length}`; + if (isNaN(x) || isNaN(y)) return; + state.anchors.push({ x, y, label: lbl }); + document.getElementById('anc-x').value = ''; + document.getElementById('anc-y').value = ''; + document.getElementById('anc-lbl').value = ''; + saveAnchors(); + renderAnchorList(); + draw(); +}); + +function saveAnchors() { + localStorage.setItem('map_anchors', JSON.stringify(state.anchors)); +} +function loadAnchors() { + try { + const saved = JSON.parse(localStorage.getItem('map_anchors') || '[]'); + state.anchors = saved.filter(a => typeof a.x === 'number' && typeof a.y === 'number'); + } catch (_) { state.anchors = []; } + renderAnchorList(); +} + +// ── Init ────────────────────────────────────────────────────────────────────── + +document.getElementById('btn-connect').addEventListener('click', connect); +document.getElementById('ws-input').addEventListener('keydown', (e) => { + if (e.key === 'Enter') connect(); +}); + +const stored = localStorage.getItem('map_ws_url'); +if (stored) document.getElementById('ws-input').value = stored; +document.getElementById('ws-input').addEventListener('change', (e) => { + localStorage.setItem('map_ws_url', e.target.value); +}); + +// Clear trail button +document.getElementById('btn-clear-trail').addEventListener('click', () => { + state.trail.length = 0; draw(); +}); + +// Init: center at origin, set btn state +state.autoCenter = true; +document.getElementById('btn-center').classList.add('on'); +loadAnchors(); +resize(); // sets canvas size and draws initial blank map