431 lines
15 KiB
JavaScript
431 lines
15 KiB
JavaScript
/* LiDAR - live 3D point cloud from the chest sensor.
|
|
|
|
Off until switched on, like the cameras: a scan is 816 KB at 2 Hz and there
|
|
is no reason to carry that when nobody is looking.
|
|
|
|
Rendered with a hand-rolled projection onto a 2D canvas rather than a 3D
|
|
library. Points need no lighting, no materials and no scene graph - just a
|
|
matrix multiply and a fillRect - so a library would be a few hundred KB
|
|
fetched over the same link we are trying not to saturate. The robot has no
|
|
internet either, which rules out a CDN.
|
|
*/
|
|
|
|
import { store, get, post, num, toast } from '../core.js';
|
|
import { el, card, pageHead, button, badge, note, stat, segmented, toggle } from '../ui.js';
|
|
|
|
export default {
|
|
id: 'lidar',
|
|
label: 'LiDAR',
|
|
icon: 'scan',
|
|
|
|
async render() {
|
|
const spec = store.spec?.lidar || {};
|
|
const key = spec.key || 'lidar_chest_front';
|
|
|
|
const root = el('div.stack');
|
|
let active = false;
|
|
let busy = false;
|
|
let timer = null;
|
|
|
|
/* -- View state -------------------------------------------------------- */
|
|
|
|
const view = {
|
|
yaw: -0.6, // radians, orbit around Z (up)
|
|
pitch: 0.45, // radians above the horizon
|
|
distance: 12, // metres from the origin
|
|
colourBy: 'height',
|
|
accumulate: false,
|
|
};
|
|
|
|
let live = []; // newest scan: [x, y, z, intensity]
|
|
const trailScans = []; // recent scans, when accumulating
|
|
const MAX_TRAIL = 12;
|
|
|
|
/* -- Controls ---------------------------------------------------------- */
|
|
|
|
const statusBadge = badge('off');
|
|
const powerBox = el('input', { type: 'checkbox' });
|
|
powerBox.addEventListener('change', () => setActive(powerBox.checked));
|
|
const power = el('label.switch', {}, powerBox, el('span.switch-track'),
|
|
el('span.switch-label', { text: 'Off' }));
|
|
const powerLabel = power.querySelector('.switch-label');
|
|
|
|
root.appendChild(pageHead(
|
|
'LiDAR',
|
|
'Chest LiDAR point cloud, decimated on the robot before it crosses the network. '
|
|
+ 'Drag to orbit, scroll to zoom.',
|
|
[power, statusBadge],
|
|
));
|
|
|
|
const tiles = el('div.grid.cols-4');
|
|
const refs = {};
|
|
for (const [tileKey, label] of [
|
|
['points', 'Points drawn'],
|
|
['scan', 'Points per scan'],
|
|
['rate', 'Scan rate'],
|
|
['range', 'Furthest return'],
|
|
]) {
|
|
const node = stat(label, '—', { sub: ' ' });
|
|
refs[tileKey] = node;
|
|
tiles.appendChild(node);
|
|
}
|
|
root.appendChild(tiles);
|
|
|
|
function setTile(tileKey, value, sub, unit = '') {
|
|
const node = refs[tileKey];
|
|
if (!node) return;
|
|
const valueNode = node.querySelector('.stat-value');
|
|
valueNode.textContent = String(value);
|
|
if (unit) valueNode.appendChild(el('span.unit', { text: unit }));
|
|
node.querySelector('.stat-sub').textContent = sub || '';
|
|
}
|
|
|
|
const canvas = el('canvas', {
|
|
width: 1200, height: 720,
|
|
style: { width: '100%', height: 'auto', display: 'block', cursor: 'grab',
|
|
borderRadius: 'var(--radius-sm)', touchAction: 'none' },
|
|
});
|
|
|
|
root.appendChild(card('Point cloud', {
|
|
sub: spec.topic || '',
|
|
actions: [
|
|
segmented(
|
|
[{ value: 'height', label: 'Height' },
|
|
{ value: 'distance', label: 'Distance' },
|
|
{ value: 'intensity', label: 'Intensity' }],
|
|
view.colourBy,
|
|
(value) => { view.colourBy = value; draw(); },
|
|
),
|
|
toggle('Trail', false, (on) => {
|
|
view.accumulate = on;
|
|
trailScans.length = 0;
|
|
draw();
|
|
}),
|
|
button('Reset view', () => {
|
|
view.yaw = -0.6; view.pitch = 0.45; view.distance = 12;
|
|
draw();
|
|
}, { size: 'sm', style: 'ghost' }),
|
|
],
|
|
flush: true,
|
|
}, canvas));
|
|
|
|
root.appendChild(note(
|
|
'Points are in the sensor frame: X forward, Y left, Z up, metres. The robot decimates '
|
|
+ `each scan to ${spec.max_points || 4000} points — the full scan is ~25 000, which looks `
|
|
+ 'identical on screen and costs ten times the bandwidth.',
|
|
'info',
|
|
));
|
|
|
|
/* -- Switching --------------------------------------------------------- */
|
|
|
|
async function setActive(on) {
|
|
if (busy) return;
|
|
busy = true;
|
|
powerBox.disabled = true;
|
|
statusBadge.textContent = on ? 'starting…' : 'stopping…';
|
|
statusBadge.dataset.tone = 'default';
|
|
|
|
try {
|
|
await post(`/api/streams/${key}`, { active: on });
|
|
active = on;
|
|
powerLabel.textContent = on ? 'On' : 'Off';
|
|
if (on) {
|
|
schedule();
|
|
} else {
|
|
if (timer) clearTimeout(timer);
|
|
timer = null;
|
|
live = [];
|
|
trailScans.length = 0;
|
|
statusBadge.textContent = 'off';
|
|
statusBadge.dataset.tone = 'default';
|
|
setTile('points', '—', '');
|
|
setTile('scan', '—', '');
|
|
setTile('rate', '—', '');
|
|
setTile('range', '—', '');
|
|
draw();
|
|
}
|
|
} catch (err) {
|
|
toast('Could not switch the LiDAR', err.message, 'critical');
|
|
powerBox.checked = active;
|
|
} finally {
|
|
busy = false;
|
|
powerBox.disabled = false;
|
|
}
|
|
}
|
|
|
|
/* -- Polling ----------------------------------------------------------- */
|
|
|
|
// The sensor publishes at 2 Hz, so asking faster than that just re-fetches
|
|
// the same scan. 400 ms keeps the view current without waste.
|
|
const POLL_MS = 400;
|
|
let lastTs = 0;
|
|
let scanTimes = [];
|
|
|
|
function schedule() {
|
|
if (timer) clearTimeout(timer);
|
|
timer = active ? setTimeout(fetchPoints, POLL_MS) : null;
|
|
}
|
|
|
|
async function fetchPoints() {
|
|
if (!active) return;
|
|
try {
|
|
const data = await get('/api/lidar/points');
|
|
if (data.off) {
|
|
// Something else turned it off - follow rather than fight it.
|
|
active = false;
|
|
powerBox.checked = false;
|
|
powerLabel.textContent = 'Off';
|
|
statusBadge.textContent = 'off';
|
|
statusBadge.dataset.tone = 'default';
|
|
return;
|
|
}
|
|
if (!data.ok) {
|
|
statusBadge.textContent = data.message || 'waiting';
|
|
statusBadge.dataset.tone = 'warning';
|
|
return;
|
|
}
|
|
|
|
if (data.ts && data.ts !== lastTs) {
|
|
scanTimes.push(data.ts);
|
|
if (scanTimes.length > 8) scanTimes.shift();
|
|
lastTs = data.ts;
|
|
|
|
if (view.accumulate) {
|
|
trailScans.push(live);
|
|
while (trailScans.length > MAX_TRAIL) trailScans.shift();
|
|
}
|
|
live = data.points || [];
|
|
}
|
|
|
|
statusBadge.textContent = 'live';
|
|
statusBadge.dataset.tone = 'good';
|
|
|
|
const hz = scanTimes.length > 1
|
|
? (scanTimes.length - 1) / (scanTimes.at(-1) - scanTimes[0])
|
|
: 0;
|
|
const drawn = live.length + (view.accumulate
|
|
? trailScans.reduce((sum, scan) => sum + scan.length, 0) : 0);
|
|
let furthest = 0;
|
|
for (const p of live) {
|
|
const d = Math.hypot(p[0], p[1], p[2]);
|
|
if (d > furthest) furthest = d;
|
|
}
|
|
|
|
setTile('points', drawn.toLocaleString(), view.accumulate ? 'including trail' : 'this scan');
|
|
setTile('scan', (data.count || 0).toLocaleString(), 'after decimation');
|
|
setTile('rate', num(hz, 1), 'measured', ' Hz');
|
|
setTile('range', num(furthest, 1), 'from the sensor', ' m');
|
|
|
|
draw();
|
|
} catch {
|
|
statusBadge.textContent = 'no answer';
|
|
statusBadge.dataset.tone = 'warning';
|
|
} finally {
|
|
schedule();
|
|
}
|
|
}
|
|
|
|
/* -- Interaction ------------------------------------------------------- */
|
|
|
|
let dragging = null;
|
|
|
|
canvas.addEventListener('pointerdown', (event) => {
|
|
dragging = { x: event.clientX, y: event.clientY };
|
|
canvas.setPointerCapture(event.pointerId);
|
|
canvas.style.cursor = 'grabbing';
|
|
});
|
|
|
|
canvas.addEventListener('pointermove', (event) => {
|
|
if (!dragging) return;
|
|
view.yaw += (event.clientX - dragging.x) * 0.007;
|
|
// Clamped just short of straight down so the view never flips over.
|
|
view.pitch = Math.max(-1.5, Math.min(1.5, view.pitch + (event.clientY - dragging.y) * 0.007));
|
|
dragging = { x: event.clientX, y: event.clientY };
|
|
draw();
|
|
});
|
|
|
|
for (const type of ['pointerup', 'pointercancel']) {
|
|
canvas.addEventListener(type, () => { dragging = null; canvas.style.cursor = 'grab'; });
|
|
}
|
|
|
|
canvas.addEventListener('wheel', (event) => {
|
|
event.preventDefault();
|
|
view.distance = Math.max(1.5, Math.min(60, view.distance * (1 + Math.sign(event.deltaY) * 0.12)));
|
|
draw();
|
|
}, { passive: false });
|
|
|
|
/* -- Rendering --------------------------------------------------------- */
|
|
|
|
const context = canvas.getContext('2d');
|
|
|
|
// Robot frame is X forward, Y left, Z up. Screen is X right, Y down. The
|
|
// camera orbits the origin looking at it.
|
|
function project(x, y, z, cam) {
|
|
// World -> camera: yaw about Z, then pitch.
|
|
const cy = Math.cos(cam.yaw), sy = Math.sin(cam.yaw);
|
|
const rx = x * cy - y * sy;
|
|
const ry = x * sy + y * cy;
|
|
|
|
const cp = Math.cos(cam.pitch), sp = Math.sin(cam.pitch);
|
|
const depth = ry * cp + z * sp + cam.distance;
|
|
const up = -ry * sp + z * cp;
|
|
|
|
// Behind the camera - drop it rather than projecting it to a mirrored
|
|
// point somewhere silly on screen.
|
|
if (depth <= 0.05) return null;
|
|
|
|
const scale = cam.focal / depth;
|
|
return { sx: cam.cx + rx * scale, sy: cam.cy - up * scale, depth };
|
|
}
|
|
|
|
function draw() {
|
|
const width = canvas.width;
|
|
const height = canvas.height;
|
|
const styles = getComputedStyle(document.documentElement);
|
|
const surface = styles.getPropertyValue('--surface-2').trim() || '#1b1e22';
|
|
const gridColor = styles.getPropertyValue('--grid').trim() || '#22262c';
|
|
const axisColor = styles.getPropertyValue('--axis').trim() || '#333941';
|
|
const muted = styles.getPropertyValue('--text-3').trim() || '#6d7681';
|
|
|
|
context.fillStyle = surface;
|
|
context.fillRect(0, 0, width, height);
|
|
|
|
const cam = {
|
|
yaw: view.yaw, pitch: view.pitch, distance: view.distance,
|
|
focal: height * 0.9, cx: width / 2, cy: height / 2,
|
|
};
|
|
|
|
// Ground grid, 1 m squares, so distances are readable at a glance.
|
|
const half = 10;
|
|
context.lineWidth = 1;
|
|
for (let i = -half; i <= half; i += 1) {
|
|
for (const [a, b] of [[[i, -half], [i, half]], [[-half, i], [half, i]]]) {
|
|
const p1 = project(a[0], a[1], 0, cam);
|
|
const p2 = project(b[0], b[1], 0, cam);
|
|
if (!p1 || !p2) continue;
|
|
context.strokeStyle = i === 0 ? axisColor : gridColor;
|
|
context.beginPath();
|
|
context.moveTo(p1.sx, p1.sy);
|
|
context.lineTo(p2.sx, p2.sy);
|
|
context.stroke();
|
|
}
|
|
}
|
|
|
|
if (!active) {
|
|
context.fillStyle = muted;
|
|
context.font = '16px system-ui, sans-serif';
|
|
context.textAlign = 'center';
|
|
context.fillText('LiDAR is off — switch it on to see the point cloud',
|
|
width / 2, height / 2);
|
|
return;
|
|
}
|
|
|
|
if (!live.length) {
|
|
context.fillStyle = muted;
|
|
context.font = '15px system-ui, sans-serif';
|
|
context.textAlign = 'center';
|
|
context.fillText('Waiting for the first scan…', width / 2, height / 2);
|
|
return;
|
|
}
|
|
|
|
// Older scans first so the newest sits on top.
|
|
const batches = view.accumulate
|
|
? [...trailScans.map((scan, i) => ({ scan, fade: 0.25 + 0.45 * (i / MAX_TRAIL) })),
|
|
{ scan: live, fade: 1 }]
|
|
: [{ scan: live, fade: 1 }];
|
|
|
|
for (const { scan, fade } of batches) {
|
|
for (const point of scan) {
|
|
const projected = project(point[0], point[1], point[2], cam);
|
|
if (!projected) continue;
|
|
if (projected.sx < 0 || projected.sx > width
|
|
|| projected.sy < 0 || projected.sy > height) continue;
|
|
|
|
context.fillStyle = colourFor(point, projected.depth, fade);
|
|
// Nearer points draw larger - the only depth cue a flat point cloud
|
|
// has once perspective alone stops being obvious.
|
|
const size = Math.max(1, Math.min(3.5, 9 / projected.depth));
|
|
context.fillRect(projected.sx, projected.sy, size, size);
|
|
}
|
|
}
|
|
|
|
// Sensor origin, so "where is the robot" is never in doubt.
|
|
const origin = project(0, 0, 0, cam);
|
|
if (origin) {
|
|
context.strokeStyle = styles.getPropertyValue('--series-2').trim() || '#d95926';
|
|
context.lineWidth = 2;
|
|
context.beginPath();
|
|
context.arc(origin.sx, origin.sy, 6, 0, Math.PI * 2);
|
|
context.stroke();
|
|
}
|
|
|
|
context.fillStyle = muted;
|
|
context.font = '11px system-ui, sans-serif';
|
|
context.textAlign = 'left';
|
|
context.fillText('1 m grid · X forward, Y left, Z up', 12, height - 12);
|
|
}
|
|
|
|
function colourFor(point, depth, fade) {
|
|
let t;
|
|
if (view.colourBy === 'height') {
|
|
t = (point[2] + 1.5) / 4.0; // -1.5 m .. 2.5 m
|
|
} else if (view.colourBy === 'intensity') {
|
|
t = (point[3] || 0) / 150;
|
|
} else {
|
|
t = Math.hypot(point[0], point[1], point[2]) / 15;
|
|
}
|
|
t = Math.max(0, Math.min(1, t));
|
|
|
|
// Blue -> cyan -> green -> yellow -> red. Enough steps that a wall at a
|
|
// constant height reads as one colour rather than a gradient.
|
|
const stops = [[59, 130, 246], [34, 211, 238], [74, 222, 128],
|
|
[250, 204, 21], [239, 68, 68]];
|
|
const scaled = t * (stops.length - 1);
|
|
const index = Math.min(stops.length - 2, Math.floor(scaled));
|
|
const frac = scaled - index;
|
|
const a = stops[index], b = stops[index + 1];
|
|
const r = Math.round(a[0] + (b[0] - a[0]) * frac);
|
|
const g = Math.round(a[1] + (b[1] - a[1]) * frac);
|
|
const bl = Math.round(a[2] + (b[2] - a[2]) * frac);
|
|
return `rgba(${r},${g},${bl},${fade})`;
|
|
}
|
|
|
|
/* -- Follow the robot's own state -------------------------------------- */
|
|
|
|
function applyStreams(streams) {
|
|
if (!streams || busy) return;
|
|
const on = !!streams[key]?.active;
|
|
if (on === active) return;
|
|
active = on;
|
|
powerBox.checked = on;
|
|
powerLabel.textContent = on ? 'On' : 'Off';
|
|
if (on) {
|
|
schedule();
|
|
} else {
|
|
if (timer) clearTimeout(timer);
|
|
timer = null;
|
|
live = [];
|
|
trailScans.length = 0;
|
|
statusBadge.textContent = 'off';
|
|
statusBadge.dataset.tone = 'default';
|
|
draw();
|
|
}
|
|
}
|
|
|
|
applyStreams(store.state?.custom?.streams);
|
|
const unsubscribe = store.on('state', () => applyStreams(store.state?.custom?.streams));
|
|
|
|
draw();
|
|
|
|
return {
|
|
node: root,
|
|
dispose: () => {
|
|
unsubscribe();
|
|
if (timer) clearTimeout(timer);
|
|
// Never leave the sensor streaming into a page that has gone away.
|
|
if (active) post(`/api/streams/${key}`, { active: false }).catch(() => {});
|
|
},
|
|
};
|
|
},
|
|
};
|