saltylab-firmware/ui/gimbal_panel.js

/**
 * gimbal_panel.js — Saltybot Gimbal Control Panel (Issue #551)
 *
 * Connects to rosbridge via WebSocket (roslib.js).
 * Publishes pan/tilt commands, subscribes to gimbal state + camera.
 *
 * ROS topics:
 *   /gimbal/cmd              geometry_msgs/Vector3   publish  (x=pan°, y=tilt°)
 *   /gimbal/state            geometry_msgs/Vector3   subscribe (x=pan°, y=tilt°)
 *   /gimbal/tracking_enabled std_msgs/Bool           publish
 *   /camera/front/image_raw/compressed  sensor_msgs/CompressedImage  subscribe
 */

'use strict';

// ── Config ────────────────────────────────────────────────────────────────────

const PAN_MIN   = -180;  // degrees
const PAN_MAX   =  180;
const TILT_MIN  =  -45;  // degrees
const TILT_MAX  =   90;
const CMD_TOPIC      = '/gimbal/cmd';
const STATE_TOPIC    = '/gimbal/state';
const TRACKING_TOPIC = '/gimbal/tracking_enabled';
const CAM_TOPIC      = '/camera/front/image_raw/compressed';
const CAM_TOPICS = {
  front: '/camera/front/image_raw/compressed',
  rear:  '/camera/rear/image_raw/compressed',
  left:  '/camera/left/image_raw/compressed',
  right: '/camera/right/image_raw/compressed',
};

const PRESETS = [
  { name: 'CENTER', pan:   0, tilt:  0 },
  { name: 'LEFT',   pan: -45, tilt:  0 },
  { name: 'RIGHT',  pan:  45, tilt:  0 },
  { name: 'UP',     pan:   0, tilt: 45 },
  { name: 'DOWN',   pan:   0, tilt:-30 },
  { name: 'HOME',   pan:   0, tilt:  0 },
];

// ── State ─────────────────────────────────────────────────────────────────────

let ros           = null;
let cmdTopic      = null;
let trackingTopic = null;
let stateSub      = null;
let camSub        = null;

let currentPan    = 0;
let currentTilt   = 0;
let targetPan     = 0;
let targetTilt    = 0;
let trackingOn    = false;
let selectedCam   = 'front';

// Camera FPS tracking
let fpsCount = 0;
let fpsTime  = Date.now();
let fps      = 0;

// Pad dragging state
let padDragging = false;

// ── DOM refs ──────────────────────────────────────────────────────────────────

const connDot    = document.getElementById('conn-dot');
const connStatus = document.getElementById('conn-status');
const wsInput    = document.getElementById('ws-input');
const padCanvas  = document.getElementById('gimbal-pad');
const panVal     = document.getElementById('pan-val');
const tiltVal    = document.getElementById('tilt-val');
const camImg     = document.getElementById('camera-img');
const noSignal   = document.getElementById('no-signal');
const fpsBadge   = document.getElementById('fps-badge');
const angleOvl   = document.getElementById('angle-overlay');

// ── Connection ────────────────────────────────────────────────────────────────

function connect() {
  const url = wsInput.value.trim();
  if (!url) return;
  if (ros) { ros.close(); }

  ros = new ROSLIB.Ros({ url });

  ros.on('connection', () => {
    connDot.className = 'connected';
    connStatus.textContent = url;
    document.getElementById('btn-connect').textContent = 'RECONNECT';
    setupTopics();
  });

  ros.on('error', (err) => {
    connDot.className = 'error';
    connStatus.textContent = 'ERROR: ' + (err?.message || err);
  });

  ros.on('close', () => {
    connDot.className = '';
    connStatus.textContent = 'DISCONNECTED';
    teardownTopics();
  });
}

function setupTopics() {
  // Publish: /gimbal/cmd
  cmdTopic = new ROSLIB.Topic({
    ros, name: CMD_TOPIC,
    messageType: 'geometry_msgs/Vector3',
  });

  // Publish: /gimbal/tracking_enabled
  trackingTopic = new ROSLIB.Topic({
    ros, name: TRACKING_TOPIC,
    messageType: 'std_msgs/Bool',
  });

  // Subscribe: /gimbal/state
  stateSub = new ROSLIB.Topic({
    ros, name: STATE_TOPIC,
    messageType: 'geometry_msgs/Vector3',
    throttle_rate: 100,
  });
  stateSub.subscribe((msg) => {
    currentPan  = msg.x ?? 0;
    currentTilt = msg.y ?? 0;
    updateAngleDisplay();
    drawPad();
  });

  // Subscribe: camera
  subscribeCamera(selectedCam);
}

function teardownTopics() {
  if (stateSub)  { stateSub.unsubscribe();  stateSub  = null; }
  if (camSub)    { camSub.unsubscribe();    camSub    = null; }
  cmdTopic = trackingTopic = null;
}

function subscribeCamera(camId) {
  if (camSub) { camSub.unsubscribe(); camSub = null; }
  const topic = CAM_TOPICS[camId] ?? CAM_TOPICS.front;
  if (!ros) return;
  camSub = new ROSLIB.Topic({
    ros, name: topic,
    messageType: 'sensor_msgs/CompressedImage',
    throttle_rate: 50,
  });
  camSub.subscribe((msg) => {
    const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
    camImg.src = `data:image/${fmt};base64,${msg.data}`;
    camImg.classList.add('visible');
    noSignal.style.display = 'none';
    fpsBadge.classList.add('visible');
    angleOvl.classList.add('visible');

    // FPS
    fpsCount++;
    const now = Date.now();
    if (now - fpsTime >= 1000) {
      fps = Math.round(fpsCount * 1000 / (now - fpsTime));
      fpsCount = 0; fpsTime = now;
      fpsBadge.textContent = fps + ' fps';
    }
  });
}

// ── Publish helpers ───────────────────────────────────────────────────────────

function publishCmd(pan, tilt) {
  if (!cmdTopic) return;
  cmdTopic.publish(new ROSLIB.Message({ x: pan, y: tilt, z: 0.0 }));
}

function publishTracking(enabled) {
  if (!trackingTopic) return;
  trackingTopic.publish(new ROSLIB.Message({ data: enabled }));
}

// ── Gimbal pad drawing ────────────────────────────────────────────────────────

function drawPad() {
  const canvas = padCanvas;
  const ctx = canvas.getContext('2d');
  const W = canvas.width;
  const H = canvas.height;

  ctx.clearRect(0, 0, W, H);

  // Background
  ctx.fillStyle = '#0a0a1a';
  ctx.fillRect(0, 0, W, H);

  // Grid lines
  ctx.strokeStyle = '#1e3a5f';
  ctx.lineWidth = 1;
  // Vertical lines
  for (let i = 1; i < 4; i++) {
    ctx.beginPath();
    ctx.moveTo(W * i / 4, 0); ctx.lineTo(W * i / 4, H);
    ctx.stroke();
  }
  // Horizontal lines
  for (let i = 1; i < 4; i++) {
    ctx.beginPath();
    ctx.moveTo(0, H * i / 4); ctx.lineTo(W, H * i / 4);
    ctx.stroke();
  }

  // Center cross
  ctx.strokeStyle = '#374151';
  ctx.lineWidth = 1;
  ctx.beginPath();
  ctx.moveTo(W / 2, 0); ctx.lineTo(W / 2, H);
  ctx.moveTo(0, H / 2); ctx.lineTo(W, H / 2);
  ctx.stroke();

  // Helpers to map degrees ↔ canvas coords
  const panToX  = (p) => ((p - PAN_MIN)  / (PAN_MAX  - PAN_MIN))  * W;
  const tiltToY = (t) => (1 - (t - TILT_MIN) / (TILT_MAX - TILT_MIN)) * H;

  // Target position (what we're commanding)
  const tx = panToX(targetPan);
  const ty = tiltToY(targetTilt);

  // Line from center to target
  ctx.strokeStyle = 'rgba(6,182,212,0.3)';
  ctx.lineWidth = 1;
  ctx.beginPath();
  ctx.moveTo(W / 2, H / 2);
  ctx.lineTo(tx, ty);
  ctx.stroke();

  // Target ring
  ctx.strokeStyle = '#0891b2';
  ctx.lineWidth = 1.5;
  ctx.beginPath();
  ctx.arc(tx, ty, 10, 0, Math.PI * 2);
  ctx.stroke();

  // Current position (feedback)
  const cx_ = panToX(currentPan);
  const cy_ = tiltToY(currentTilt);
  ctx.fillStyle = '#22d3ee';
  ctx.beginPath();
  ctx.arc(cx_, cy_, 6, 0, Math.PI * 2);
  ctx.fill();

  // Labels: axis limits
  ctx.fillStyle = '#374151';
  ctx.font = '9px Courier New';
  ctx.fillText(`${PAN_MIN}°`, 2, H / 2 - 3);
  ctx.fillText(`${PAN_MAX}°`, W - 24, H / 2 - 3);
  ctx.fillText(`${TILT_MAX}°`, W / 2 + 3, 11);
  ctx.fillText(`${TILT_MIN}°`, W / 2 + 3, H - 3);

  // Current angle text
  ctx.fillStyle = '#0891b2';
  ctx.font = 'bold 9px Courier New';
  ctx.fillText(`PAN ${targetPan.toFixed(0)}°  TILT ${targetTilt.toFixed(0)}°`, 4, H - 3);
}

// ── Pad interaction ───────────────────────────────────────────────────────────

function padCoordsToAngles(clientX, clientY) {
  const rect = padCanvas.getBoundingClientRect();
  const rx = (clientX - rect.left) / rect.width;
  const ry = (clientY - rect.top)  / rect.height;
  const pan  = PAN_MIN  + rx * (PAN_MAX  - PAN_MIN);
  const tilt = TILT_MAX - ry * (TILT_MAX - TILT_MIN);
  return {
    pan:  Math.max(PAN_MIN,  Math.min(PAN_MAX,  pan)),
    tilt: Math.max(TILT_MIN, Math.min(TILT_MAX, tilt)),
  };
}

function padPointerDown(e) {
  padDragging = true;
  padCanvas.setPointerCapture(e.pointerId);
  const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
  setTarget(pan, tilt);
}

function padPointerMove(e) {
  if (!padDragging) return;
  const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
  setTarget(pan, tilt);
}

function padPointerUp() {
  padDragging = false;
}

padCanvas.addEventListener('pointerdown',   padPointerDown);
padCanvas.addEventListener('pointermove',   padPointerMove);
padCanvas.addEventListener('pointerup',     padPointerUp);
padCanvas.addEventListener('pointercancel', padPointerUp);

// Touch passthrough
padCanvas.addEventListener('touchstart',  (e) => e.preventDefault(), { passive: false });
padCanvas.addEventListener('touchmove',   (e) => e.preventDefault(), { passive: false });

// ── Target update ─────────────────────────────────────────────────────────────

function setTarget(pan, tilt) {
  targetPan  = Math.round(pan  * 10) / 10;
  targetTilt = Math.round(tilt * 10) / 10;
  updateAngleDisplay();
  drawPad();
  publishCmd(targetPan, targetTilt);
}

function updateAngleDisplay() {
  panVal.textContent  = currentPan.toFixed(1);
  tiltVal.textContent = currentTilt.toFixed(1);
  angleOvl.textContent = `PAN ${currentPan.toFixed(1)}°  TILT ${currentTilt.toFixed(1)}°`;
}

// ── Presets ───────────────────────────────────────────────────────────────────

function applyPreset(pan, tilt) {
  setTarget(pan, tilt);
}

function goHome() {
  setTarget(0, 0);
}

// ── Tracking toggle ───────────────────────────────────────────────────────────

document.getElementById('tracking-toggle').addEventListener('change', (e) => {
  trackingOn = e.target.checked;
  publishTracking(trackingOn);
});

// ── Camera selector ───────────────────────────────────────────────────────────

document.querySelectorAll('.cam-btn').forEach((btn) => {
  btn.addEventListener('click', () => {
    selectedCam = btn.dataset.cam;
    document.querySelectorAll('.cam-btn').forEach((b) => b.classList.remove('active'));
    btn.classList.add('active');
    subscribeCamera(selectedCam);
    // Reset fps display
    camImg.classList.remove('visible');
    noSignal.style.display = '';
    fpsBadge.classList.remove('visible');
    angleOvl.classList.remove('visible');
    fpsCount = 0; fpsTime = Date.now();
  });
});

// ── Connect button ────────────────────────────────────────────────────────────

document.getElementById('btn-connect').addEventListener('click', connect);
wsInput.addEventListener('keydown', (e) => { if (e.key === 'Enter') connect(); });

// ── Preset buttons ────────────────────────────────────────────────────────────

document.querySelectorAll('[data-preset]').forEach((btn) => {
  const idx = parseInt(btn.dataset.preset, 10);
  const p   = PRESETS[idx];
  btn.addEventListener('click', () => applyPreset(p.pan, p.tilt));
});

document.getElementById('btn-home').addEventListener('click', goHome);

// ── Init ──────────────────────────────────────────────────────────────────────

// Size canvas to fill its container initially
function resizePad() {
  const w = padCanvas.offsetWidth || 200;
  padCanvas.width  = w;
  padCanvas.height = Math.round(w * 0.75);   // 4:3 aspect
  drawPad();
}

window.addEventListener('resize', resizePad);
resizePad();

// Auto-connect on load using stored URL or default
const storedUrl = localStorage.getItem('gimbal_ws_url');
if (storedUrl) wsInput.value = storedUrl;
wsInput.addEventListener('change', () => {
  localStorage.setItem('gimbal_ws_url', wsInput.value);
});

// Initial draw
drawPad();
updateAngleDisplay();