saltylab-firmware/ui/map_panel.js

/**
 * 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) =>
    `<div class="anchor-row">
       <span style="color:#f59e0b">◆</span>
       <span style="flex:1">${a.label} (${a.x.toFixed(1)}, ${a.y.toFixed(1)})</span>
       <button onclick="removeAnchor(${i})" style="background:none;border:none;color:#6b7280;cursor:pointer;font-size:10px">✕</button>
     </div>`
  ).join('') || '<div style="color:#374151;font-size:10px;text-align:center;padding:4px">No anchors</div>';
}

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