277 lines
10 KiB
JavaScript
277 lines
10 KiB
JavaScript
/* Navigation - odometry, heading and travelled path.
|
|
|
|
This firmware does not expose the documented SLAM command interface
|
|
(/integrated_command, /relocalization_pose, GetStoredMapByName are all absent
|
|
from the live graph), so no mapping controls are offered - a button that
|
|
silently does nothing is worse than no button. What the robot does publish is
|
|
leg odometry, and that is what this tab shows.
|
|
*/
|
|
|
|
import { store, num, RAD2DEG } from '../core.js';
|
|
import { el, card, pageHead, button, note, badge, stat, kv, emptyState } from '../ui.js';
|
|
import { compass, attitude } from '../charts.js';
|
|
|
|
export default {
|
|
id: 'navigation',
|
|
label: 'Navigation',
|
|
icon: 'nav',
|
|
|
|
async render() {
|
|
const root = el('div.stack');
|
|
const trail = [];
|
|
let following = true;
|
|
|
|
root.appendChild(pageHead(
|
|
'Navigation',
|
|
'Where the robot has been, from leg odometry. Values advance while it walks and hold '
|
|
+ 'steady when it is parked.',
|
|
[
|
|
button('Clear path', () => { trail.length = 0; drawTrail(); }, { size: 'sm', style: 'ghost' }),
|
|
],
|
|
));
|
|
|
|
/* -- Tiles ------------------------------------------------------------ */
|
|
|
|
const tiles = el('div.grid.cols-4');
|
|
const refs = {};
|
|
for (const [key, label] of [
|
|
['x', 'Position X'],
|
|
['y', 'Position Y'],
|
|
['heading', 'Heading'],
|
|
['speed', 'Ground speed'],
|
|
['distance', 'Distance travelled'],
|
|
['points', 'Path points'],
|
|
['mode', 'Motion mode'],
|
|
['status', 'Odometry'],
|
|
]) {
|
|
const node = stat(label, '—', { sub: ' ' });
|
|
refs[key] = node;
|
|
tiles.appendChild(node);
|
|
}
|
|
root.appendChild(tiles);
|
|
|
|
function setTile(key, value, sub, tone = 'default', unit = '') {
|
|
const node = refs[key];
|
|
if (!node) return;
|
|
node.dataset.tone = tone;
|
|
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 || '';
|
|
}
|
|
|
|
/* -- Heading and attitude --------------------------------------------- */
|
|
|
|
const compassHost = el('div.attitude');
|
|
const attitudeHost = el('div.attitude');
|
|
const readoutHost = el('div');
|
|
const trailCanvas = el('canvas', {
|
|
width: 960, height: 620,
|
|
style: { width: '100%', height: 'auto', display: 'block', borderRadius: 'var(--radius-sm)' },
|
|
});
|
|
|
|
root.appendChild(el('div.grid.split', {},
|
|
card('Heading and attitude', { sub: 'Odometry yaw · chest IMU' },
|
|
el('div.stack', { style: { alignItems: 'center' } },
|
|
el('div.row', { style: { justifyContent: 'center', gap: '14px', flexWrap: 'wrap' } },
|
|
compassHost, attitudeHost,
|
|
),
|
|
readoutHost,
|
|
),
|
|
),
|
|
card('Travelled path', {
|
|
sub: 'Top-down, 1 m grid',
|
|
actions: [
|
|
(() => {
|
|
const box = el('input', { type: 'checkbox', checked: true });
|
|
box.addEventListener('change', () => { following = box.checked; drawTrail(); });
|
|
return el('label.switch', {}, box, el('span.switch-track'),
|
|
el('span.switch-label', { text: 'Follow robot' }));
|
|
})(),
|
|
],
|
|
flush: true,
|
|
}, trailCanvas),
|
|
));
|
|
|
|
root.appendChild(note(
|
|
'SLAM mapping and relocalization are not offered here: this firmware does not advertise the '
|
|
+ '/integrated_command topic or the GetStoredMapByName service, so there is nothing for the '
|
|
+ 'dashboard to call. Use the AGIBOT mobile app for map building on this unit.',
|
|
'info',
|
|
));
|
|
|
|
/* -- Trail rendering --------------------------------------------------- */
|
|
|
|
const context = trailCanvas.getContext('2d');
|
|
|
|
function drawTrail() {
|
|
const width = trailCanvas.width;
|
|
const height = trailCanvas.height;
|
|
const styles = getComputedStyle(document.documentElement);
|
|
const surface = styles.getPropertyValue('--surface-2').trim() || '#1b1e22';
|
|
const gridColor = styles.getPropertyValue('--grid').trim() || '#22262c';
|
|
const axis = styles.getPropertyValue('--axis').trim() || '#333941';
|
|
const line = styles.getPropertyValue('--series-1').trim() || '#3987e5';
|
|
const marker = styles.getPropertyValue('--series-2').trim() || '#d95926';
|
|
const muted = styles.getPropertyValue('--text-3').trim() || '#6d7681';
|
|
|
|
context.fillStyle = surface;
|
|
context.fillRect(0, 0, width, height);
|
|
|
|
if (!trail.length) {
|
|
context.fillStyle = muted;
|
|
context.font = '15px system-ui, sans-serif';
|
|
context.textAlign = 'center';
|
|
context.fillText('Waiting for odometry — walk the robot to draw a path',
|
|
width / 2, height / 2);
|
|
return;
|
|
}
|
|
|
|
const xs = trail.map((p) => p.x);
|
|
const ys = trail.map((p) => p.y);
|
|
const last = trail.at(-1);
|
|
|
|
const spanX = Math.max(2, Math.max(...xs) - Math.min(...xs));
|
|
const spanY = Math.max(2, Math.max(...ys) - Math.min(...ys));
|
|
const pad = 44;
|
|
const scale = Math.min((width - pad * 2) / spanX, (height - pad * 2) / spanY, 110);
|
|
|
|
const centreX = following ? last.x : (Math.min(...xs) + Math.max(...xs)) / 2;
|
|
const centreY = following ? last.y : (Math.min(...ys) + Math.max(...ys)) / 2;
|
|
|
|
// Robot X is forward and Y is left; canvas y grows downward, so flip it.
|
|
const X = (x) => width / 2 + (x - centreX) * scale;
|
|
const Y = (y) => height / 2 - (y - centreY) * scale;
|
|
|
|
const step = scale >= 60 ? 1 : scale >= 25 ? 2 : 5;
|
|
context.lineWidth = 1;
|
|
const firstX = Math.floor(centreX - width / 2 / scale) - 1;
|
|
const lastX = Math.ceil(centreX + width / 2 / scale) + 1;
|
|
const firstY = Math.floor(centreY - height / 2 / scale) - 1;
|
|
const lastY = Math.ceil(centreY + height / 2 / scale) + 1;
|
|
|
|
for (let m = Math.ceil(firstX / step) * step; m <= lastX; m += step) {
|
|
context.strokeStyle = m === 0 ? axis : gridColor;
|
|
context.beginPath(); context.moveTo(X(m), 0); context.lineTo(X(m), height); context.stroke();
|
|
}
|
|
for (let m = Math.ceil(firstY / step) * step; m <= lastY; m += step) {
|
|
context.strokeStyle = m === 0 ? axis : gridColor;
|
|
context.beginPath(); context.moveTo(0, Y(m)); context.lineTo(width, Y(m)); context.stroke();
|
|
}
|
|
|
|
// Scale key.
|
|
context.fillStyle = muted;
|
|
context.font = '11px system-ui, sans-serif';
|
|
context.textAlign = 'left';
|
|
context.fillText(`${step} m grid`, 12, height - 12);
|
|
|
|
if (trail.length > 1) {
|
|
context.strokeStyle = line;
|
|
context.lineWidth = 2;
|
|
context.lineJoin = 'round';
|
|
context.lineCap = 'round';
|
|
context.beginPath();
|
|
trail.forEach((point, index) => {
|
|
const px = X(point.x), py = Y(point.y);
|
|
if (index === 0) context.moveTo(px, py); else context.lineTo(px, py);
|
|
});
|
|
context.stroke();
|
|
|
|
// Start marker with a surface ring so it stays legible over the line.
|
|
const start = trail[0];
|
|
context.beginPath();
|
|
context.arc(X(start.x), Y(start.y), 5, 0, Math.PI * 2);
|
|
context.fillStyle = surface;
|
|
context.fill();
|
|
context.strokeStyle = line;
|
|
context.lineWidth = 2;
|
|
context.stroke();
|
|
}
|
|
|
|
// Robot marker, pointing along its heading.
|
|
context.save();
|
|
context.translate(X(last.x), Y(last.y));
|
|
context.rotate(-last.yaw);
|
|
context.fillStyle = marker;
|
|
context.strokeStyle = surface;
|
|
context.lineWidth = 2.5;
|
|
context.beginPath();
|
|
context.moveTo(14, 0); context.lineTo(-9, 8); context.lineTo(-4, 0); context.lineTo(-9, -8);
|
|
context.closePath();
|
|
context.fill();
|
|
context.stroke();
|
|
context.restore();
|
|
}
|
|
|
|
/* -- Painting ---------------------------------------------------------- */
|
|
|
|
let distance = 0;
|
|
|
|
function paint() {
|
|
const s = store.state;
|
|
if (!s) return;
|
|
|
|
const odom = s.odom || { x: 0, y: 0, yaw: 0 };
|
|
const vel = s.velocity || {};
|
|
const chest = s.imu?.chest || {};
|
|
|
|
const previous = trail.at(-1);
|
|
const moved = !previous || Math.hypot(odom.x - previous.x, odom.y - previous.y) > 0.015;
|
|
if (moved) {
|
|
if (previous) distance += Math.hypot(odom.x - previous.x, odom.y - previous.y);
|
|
trail.push({ x: odom.x, y: odom.y, yaw: odom.yaw });
|
|
if (trail.length > 4000) trail.shift();
|
|
} else if (previous) {
|
|
previous.yaw = odom.yaw;
|
|
}
|
|
|
|
const speed = Math.hypot(vel.forward || 0, vel.lateral || 0);
|
|
const yawDeg = ((odom.yaw || 0) * RAD2DEG + 360) % 360;
|
|
|
|
setTile('x', num(odom.x, 2), 'forward positive', 'default', ' m');
|
|
setTile('y', num(odom.y, 2), 'left positive', 'default', ' m');
|
|
setTile('heading', num(yawDeg, 0), 'counter-clockwise positive', 'default', '°');
|
|
setTile('speed', num(speed, 2), `yaw ${num(vel.angular, 2)} rad/s`, 'default', ' m/s');
|
|
setTile('distance', num(distance, 1), 'since this tab opened', 'default', ' m');
|
|
setTile('points', trail.length, 'samples held');
|
|
|
|
const modeSpec = store.spec?.modes?.find((m) => m.id === s.mode);
|
|
setTile('mode', modeSpec?.label || s.mode || '—', modeSpec?.group || '',
|
|
modeSpec?.danger ? 'critical' : 'default');
|
|
|
|
const stat = s.topic_stats?.['/aima/mc/leg_odometry'];
|
|
const age = stat ? (Date.now() / 1000) - stat.last : null;
|
|
const live = age !== null && age < 2;
|
|
setTile('status', live ? 'Live' : 'Idle',
|
|
live ? `${num(stat.hz, 0)} Hz` : 'no recent messages',
|
|
live ? 'good' : 'default');
|
|
|
|
compassHost.replaceChildren(compass(odom.yaw || 0, { size: 150 }));
|
|
attitudeHost.replaceChildren(attitude(chest.roll || 0, chest.pitch || 0, { size: 150 }));
|
|
|
|
readoutHost.replaceChildren(kv([
|
|
['Roll', `${num((chest.roll || 0) * RAD2DEG, 2)}°`],
|
|
['Pitch', `${num((chest.pitch || 0) * RAD2DEG, 2)}°`],
|
|
['Yaw (odometry)', `${num(yawDeg, 1)}°`],
|
|
['Forward', `${num(vel.forward, 3)} m/s`],
|
|
['Lateral', `${num(vel.lateral, 3)} m/s`],
|
|
['Yaw rate', `${num(vel.angular, 3)} rad/s`],
|
|
]));
|
|
|
|
drawTrail();
|
|
}
|
|
|
|
paint();
|
|
|
|
let last = 0;
|
|
const unsubscribe = store.on('state', () => {
|
|
const now = Date.now();
|
|
if (now - last < 250) return;
|
|
last = now;
|
|
paint();
|
|
});
|
|
|
|
return { node: root, dispose: unsubscribe };
|
|
},
|
|
};
|