roverdometer

This commit is contained in:
legop3
2026-06-19 16:37:47 -04:00
parent 39bb53104e
commit 3bad454e0d
13 changed files with 726 additions and 32 deletions
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
+2 -2
View File
@@ -78,8 +78,8 @@
<script defer src="https://analytics.otter.land/script.js" data-website-id="82dd56a5-db44-4279-bd1e-a4d9fee39af7" data-domains="rover.otter.land"></script>
<script defer src="https://analytics.otter.land/recorder.js" data-website-id="82dd56a5-db44-4279-bd1e-a4d9fee39af7" data-domains="rover.otter.land" data-sample-rate="0.15" data-mask-level="moderate" data-max-duration="300000"></script>
<title>Roomba Rover</title>
<script type="module" crossorigin src="/assets/index-BEB1f3bD.js"></script>
<link rel="stylesheet" crossorigin href="/assets/index-THYGwIt0.css">
<script type="module" crossorigin src="/assets/index-DXjwvKfx.js"></script>
<link rel="stylesheet" crossorigin href="/assets/index-BcP4x_Qk.css">
</head>
<body>
<div id="root"></div>
+2 -2
View File
@@ -54,8 +54,8 @@ const GROUP100_LAYOUT = [
{ id: 40, key: 'requestedRadius', bytes: 2, parser: parseInt },
{ id: 41, key: 'requestedRightVelocity', bytes: 2, parser: parseInt },
{ id: 42, key: 'requestedLeftVelocity', bytes: 2, parser: parseInt },
{ id: 43, key: 'encoderCountsLeft', bytes: 2, parser: parseUInt },
{ id: 44, key: 'encoderCountsRight', bytes: 2, parser: parseUInt },
{ id: 43, key: 'encoderCountsLeft', bytes: 2, parser: parseInt },
{ id: 44, key: 'encoderCountsRight', bytes: 2, parser: parseInt },
{ id: 45, key: 'lightBumper', bytes: 1, parser: parseLightBumper },
{ id: 46, key: 'lightBumpLeftSignal', bytes: 2, parser: parseUInt },
{ id: 47, key: 'lightBumpFrontLeftSignal', bytes: 2, parser: parseUInt },
@@ -0,0 +1,420 @@
// odometer Service
// Purpose: Converts Roomba wheel encoder samples into persistent rover distance totals.
// Scope: Owns encoder rollover handling, sanity filtering, per-rover odometer state, and disk persistence.
const fs = require('fs');
const path = require('path');
const EventEmitter = require('events');
const { resolveDataPath } = require('../../helpers/dataPaths');
const logger = require('../../globals/logger').child('odometerService');
const STORE_PATH = resolveDataPath('rover-odometers.json');
const ENCODER_MODULUS = 65536;
const ENCODER_HALF_RANGE = ENCODER_MODULUS / 2;
const DEFAULT_WHEEL_DIAMETER_MM = 72.0;
const DEFAULT_COUNTS_PER_REVOLUTION = 508.8;
const DEFAULT_MM_PER_COUNT = (Math.PI * DEFAULT_WHEEL_DIAMETER_MM) / DEFAULT_COUNTS_PER_REVOLUTION;
const DEFAULT_CALIBRATION_MULTIPLIER = 1;
const MAX_REASONABLE_SPEED_MM_PER_SECOND = 1200;
const MAX_REASONABLE_DELTA_FLOOR_MM = 250;
const SAVE_DEBOUNCE_MS = 2500;
const MIN_SAVE_INTERVAL_MS = 10000;
const EMIT_THROTTLE_MS = 500;
const odometerEvents = new EventEmitter();
const states = new Map();
let saveTimer = null;
let lastSaveAt = 0;
let loaded = false;
function nowMs() {
return Date.now();
}
function safeNumber(value, fallback = 0) {
const numeric = Number(value);
return Number.isFinite(numeric) ? numeric : fallback;
}
function createInitialComparison() {
return {
sampleCount: 0,
missingDistanceSamples: 0,
ignoredSamples: 0,
encoderSessionMm: 0,
distancePacketSessionMm: 0,
signedEncoderSessionMm: 0,
signedDistancePacketSessionMm: 0,
last: null,
};
}
function normalizeStoredEntry(entry = {}) {
const totalMm = Math.max(0, safeNumber(entry.totalMm, 0));
const calibrationMultiplier = Math.max(0.01, safeNumber(entry.calibrationMultiplier, DEFAULT_CALIBRATION_MULTIPLIER));
return {
totalMm,
calibrationMultiplier,
updatedAt: safeNumber(entry.updatedAt, null),
};
}
function ensureLoaded() {
if (loaded) return;
loaded = true;
try {
const raw = JSON.parse(fs.readFileSync(STORE_PATH, 'utf8'));
const rovers = raw && typeof raw === 'object' && raw.rovers && typeof raw.rovers === 'object'
? raw.rovers
: raw;
Object.entries(rovers || {}).forEach(([roverId, entry]) => {
const normalized = normalizeStoredEntry(entry);
states.set(String(roverId), {
roverId: String(roverId),
totalMm: normalized.totalMm,
sessionMm: 0,
calibrationMultiplier: normalized.calibrationMultiplier,
lastLeftCount: null,
lastRightCount: null,
lastSampleAt: null,
lastIntegratedAt: null,
lastDelta: null,
rolloverEvents: 0,
ignoredSamples: 0,
status: 'waiting',
statusReason: 'waiting for encoder sample',
comparison: createInitialComparison(),
lastEmittedAt: 0,
lastEmittedStatus: null,
updatedAt: normalized.updatedAt,
});
});
} catch (err) {
if (err.code !== 'ENOENT') {
logger.warn('Failed to load rover odometers', { path: STORE_PATH, error: err.message });
}
}
}
function ensureState(roverId) {
ensureLoaded();
const id = String(roverId || '').trim();
if (!id) return null;
if (!states.has(id)) {
states.set(id, {
roverId: id,
totalMm: 0,
sessionMm: 0,
calibrationMultiplier: DEFAULT_CALIBRATION_MULTIPLIER,
lastLeftCount: null,
lastRightCount: null,
lastSampleAt: null,
lastIntegratedAt: null,
lastDelta: null,
rolloverEvents: 0,
ignoredSamples: 0,
status: 'waiting',
statusReason: 'waiting for encoder sample',
comparison: createInitialComparison(),
lastEmittedAt: 0,
lastEmittedStatus: null,
updatedAt: null,
});
}
return states.get(id);
}
function signedEncoderDelta(previous, current) {
let delta = current - previous;
if (delta > ENCODER_HALF_RANGE) {
delta -= ENCODER_MODULUS;
} else if (delta < -ENCODER_HALF_RANGE) {
delta += ENCODER_MODULUS;
}
return delta;
}
function crossedRollover(previous, current, delta) {
// The unwrapped delta is intentionally compared with the raw subtraction.
// If they differ, the encoder crossed the signed 16-bit boundary between
// samples and the modular correction above was required.
return current - previous !== delta;
}
function maxReasonableDeltaMm(elapsedMs) {
const elapsedSeconds = Math.max(0.05, safeNumber(elapsedMs, 0) / 1000);
return Math.max(MAX_REASONABLE_DELTA_FLOOR_MM, elapsedSeconds * MAX_REASONABLE_SPEED_MM_PER_SECOND);
}
function persistSoon() {
const elapsed = nowMs() - lastSaveAt;
if (elapsed >= MIN_SAVE_INTERVAL_MS) {
saveNow();
return;
}
if (saveTimer) return;
saveTimer = setTimeout(() => {
saveTimer = null;
saveNow();
}, Math.max(SAVE_DEBOUNCE_MS, MIN_SAVE_INTERVAL_MS - elapsed));
}
function saveNow() {
ensureLoaded();
const rovers = {};
for (const [roverId, state] of states.entries()) {
rovers[roverId] = {
totalMm: Math.round(state.totalMm * 1000) / 1000,
calibrationMultiplier: state.calibrationMultiplier,
updatedAt: state.updatedAt || null,
};
}
const payload = {
version: 1,
unit: 'millimeters',
source: 'roomba wheel encoders',
mmPerCount: DEFAULT_MM_PER_COUNT,
rovers,
};
try {
fs.mkdirSync(path.dirname(STORE_PATH), { recursive: true });
const tempPath = `${STORE_PATH}.tmp`;
fs.writeFileSync(tempPath, `${JSON.stringify(payload, null, 2)}\n`, 'utf8');
fs.renameSync(tempPath, STORE_PATH);
lastSaveAt = nowMs();
} catch (err) {
logger.warn('Failed to save rover odometers', { path: STORE_PATH, error: err.message });
}
}
function percentDifference(differenceMm, referenceMm) {
const reference = Math.abs(safeNumber(referenceMm, 0));
if (reference < 1) return null;
return Math.round((Math.abs(differenceMm) / reference) * 1000) / 10;
}
function snapshotComparison(comparison = createInitialComparison()) {
const sessionDifferenceMm = comparison.distancePacketSessionMm - comparison.encoderSessionMm;
return {
sampleCount: comparison.sampleCount,
missingDistanceSamples: comparison.missingDistanceSamples,
ignoredSamples: comparison.ignoredSamples,
encoderSessionMm: Math.round(comparison.encoderSessionMm),
distancePacketSessionMm: Math.round(comparison.distancePacketSessionMm),
signedEncoderSessionMm: Math.round(comparison.signedEncoderSessionMm),
signedDistancePacketSessionMm: Math.round(comparison.signedDistancePacketSessionMm),
sessionDifferenceMm: Math.round(sessionDifferenceMm),
sessionDifferencePct: percentDifference(sessionDifferenceMm, comparison.encoderSessionMm),
last: comparison.last,
};
}
function snapshotState(state) {
if (!state) return null;
return {
roverId: state.roverId,
totalMm: Math.round(state.totalMm),
sessionMm: Math.round(state.sessionMm),
calibrationMultiplier: state.calibrationMultiplier,
mmPerCount: DEFAULT_MM_PER_COUNT * state.calibrationMultiplier,
rawMmPerCount: DEFAULT_MM_PER_COUNT,
countsPerRevolution: DEFAULT_COUNTS_PER_REVOLUTION,
wheelDiameterMm: DEFAULT_WHEEL_DIAMETER_MM,
lastLeftCount: state.lastLeftCount,
lastRightCount: state.lastRightCount,
lastSampleAt: state.lastSampleAt,
lastIntegratedAt: state.lastIntegratedAt,
lastDelta: state.lastDelta,
rolloverEvents: state.rolloverEvents,
ignoredSamples: state.ignoredSamples,
comparison: snapshotComparison(state.comparison),
status: state.status,
statusReason: state.statusReason,
updatedAt: state.updatedAt,
};
}
function updateDistancePacketComparison(state, sensors, encoderCenterMm, encoderDistanceMm, options = {}) {
const comparison = state.comparison || createInitialComparison();
state.comparison = comparison;
const packetMm = Number(sensors?.distanceMm);
const ignored = Boolean(options.ignored);
if (!Number.isFinite(packetMm)) {
comparison.missingDistanceSamples += 1;
return;
}
const packetDistanceMm = Math.abs(packetMm);
const signedDifferenceMm = packetMm - encoderCenterMm;
const distanceDifferenceMm = packetDistanceMm - encoderDistanceMm;
comparison.sampleCount += 1;
if (ignored) {
// Ignored encoder deltas are still recorded as a point-in-time diagnostic,
// but they are not folded into the session totals. That keeps reconnect
// edges from making the distance-packet comparison look worse than the
// odometer's own accepted movement stream.
comparison.ignoredSamples += 1;
} else {
comparison.encoderSessionMm += encoderDistanceMm;
comparison.distancePacketSessionMm += packetDistanceMm;
comparison.signedEncoderSessionMm += encoderCenterMm;
comparison.signedDistancePacketSessionMm += packetMm;
}
comparison.last = {
encoderCenterMm: Math.round(encoderCenterMm),
encoderDistanceMm: Math.round(encoderDistanceMm),
distancePacketMm: Math.round(packetMm),
distancePacketAbsMm: Math.round(packetDistanceMm),
signedDifferenceMm: Math.round(signedDifferenceMm),
distanceDifferenceMm: Math.round(distanceDifferenceMm),
signedDifferencePct: percentDifference(signedDifferenceMm, encoderCenterMm),
distanceDifferencePct: percentDifference(distanceDifferenceMm, encoderDistanceMm),
ignored,
};
}
function emitSnapshot(state, snapshot, options = {}) {
const force = Boolean(options.force);
const emittedRecently = nowMs() - safeNumber(state.lastEmittedAt, 0) < EMIT_THROTTLE_MS;
const statusChanged = state.lastEmittedStatus !== state.status;
if (!force && emittedRecently && !statusChanged) return;
// Odometer updates are presentation data, not control-loop data. Throttling
// here keeps browser traffic proportional to what humans can read while the
// integration math above still processes every sensor frame.
state.lastEmittedAt = nowMs();
state.lastEmittedStatus = state.status;
odometerEvents.emit('update', { roverId: state.roverId, odometer: snapshot });
}
function getSnapshot(roverId) {
return snapshotState(ensureState(roverId));
}
function getSnapshots(roverIds = null) {
ensureLoaded();
const ids = Array.isArray(roverIds) ? roverIds.map((id) => String(id)) : Array.from(states.keys());
return ids.map((id) => snapshotState(ensureState(id))).filter(Boolean);
}
function processSensorFrame(roverId, sensors = {}) {
const state = ensureState(roverId);
if (!state) return null;
const left = Number(sensors?.encoderCountsLeft);
const right = Number(sensors?.encoderCountsRight);
const sampleAt = nowMs();
if (!Number.isInteger(left) || !Number.isInteger(right)) {
state.status = 'waiting';
state.statusReason = 'encoder counts missing';
return snapshotState(state);
}
if (state.lastLeftCount == null || state.lastRightCount == null) {
// The first valid frame becomes the baseline because encoder packets are
// cumulative counters inside the Roomba, not distance-since-last-poll
// packets. Adding the first absolute value would invent mileage whenever
// the server or rover reconnects.
state.lastLeftCount = left;
state.lastRightCount = right;
state.lastSampleAt = sampleAt;
state.status = 'tracking';
state.statusReason = 'baseline ready';
state.updatedAt = sampleAt;
const snapshot = snapshotState(state);
emitSnapshot(state, snapshot, { force: true });
return snapshot;
}
const leftCounts = signedEncoderDelta(state.lastLeftCount, left);
const rightCounts = signedEncoderDelta(state.lastRightCount, right);
const elapsedMs = state.lastSampleAt ? sampleAt - state.lastSampleAt : 0;
const mmPerCount = DEFAULT_MM_PER_COUNT * state.calibrationMultiplier;
const leftMm = leftCounts * mmPerCount;
const rightMm = rightCounts * mmPerCount;
const centerMm = (leftMm + rightMm) / 2;
const distanceMm = Math.abs(centerMm);
const reasonableLimit = maxReasonableDeltaMm(elapsedMs);
const leftRolled = crossedRollover(state.lastLeftCount, left, leftCounts);
const rightRolled = crossedRollover(state.lastRightCount, right, rightCounts);
state.lastLeftCount = left;
state.lastRightCount = right;
state.lastSampleAt = sampleAt;
if (leftRolled) state.rolloverEvents += 1;
if (rightRolled) state.rolloverEvents += 1;
if (distanceMm > reasonableLimit) {
// A single impossible jump is much more likely to be stale serial data,
// a reconnect edge, or corrupt parsing than real movement. The baseline is
// still advanced so the next good frame can continue from the new counter.
state.ignoredSamples += 1;
state.status = 'ignored';
state.statusReason = `ignored ${Math.round(distanceMm)} mm jump`;
state.lastDelta = {
leftCounts,
rightCounts,
leftMm: Math.round(leftMm),
rightMm: Math.round(rightMm),
centerMm: Math.round(centerMm),
distanceMm: 0,
elapsedMs,
ignored: true,
};
updateDistancePacketComparison(state, sensors, centerMm, distanceMm, { ignored: true });
state.updatedAt = sampleAt;
const snapshot = snapshotState(state);
emitSnapshot(state, snapshot);
return snapshot;
}
state.totalMm += distanceMm;
state.sessionMm += distanceMm;
updateDistancePacketComparison(state, sensors, centerMm, distanceMm);
state.lastIntegratedAt = sampleAt;
state.status = 'tracking';
state.statusReason = distanceMm > 0 ? 'integrated encoder delta' : 'no movement';
state.lastDelta = {
leftCounts,
rightCounts,
leftMm: Math.round(leftMm),
rightMm: Math.round(rightMm),
centerMm: Math.round(centerMm),
distanceMm: Math.round(distanceMm),
elapsedMs,
ignored: false,
};
state.updatedAt = sampleAt;
persistSoon();
const snapshot = snapshotState(state);
emitSnapshot(state, snapshot);
return snapshot;
}
function resetSession(roverId) {
const state = ensureState(roverId);
if (!state) return null;
state.sessionMm = 0;
state.comparison = createInitialComparison();
state.updatedAt = nowMs();
const snapshot = snapshotState(state);
emitSnapshot(state, snapshot, { force: true });
return snapshot;
}
process.on('exit', () => {
if (saveTimer) {
clearTimeout(saveTimer);
saveTimer = null;
}
saveNow();
});
module.exports = {
ENCODER_MODULUS,
DEFAULT_MM_PER_COUNT,
odometerEvents,
getSnapshot,
getSnapshots,
processSensorFrame,
resetSession,
saveNow,
};
+18
View File
@@ -5,6 +5,7 @@ const io = require('../../globals/io');
const logger = require('../../globals/logger').child('roverManager');
const { sendAlert } = require('../alertService');
const { parseSensorFrame } = require('../../helpers/sensorDecoder');
const odometerService = require('../odometerService');
const { MODES, getMode } = require('../modeManager');
const { isAdmin, isLockdownAdmin, roleEvents } = require('../roleService');
const { publishEvent } = require('../eventBus');
@@ -174,6 +175,7 @@ const sensorPipeline = createSensorPipeline({
ALERT_COLOR,
sendAlert,
publishEvent,
processOdometerFrame: odometerService.processSensorFrame,
isPrivateRecord,
isPrivateOpen,
getPrivateSafety,
@@ -197,6 +199,11 @@ function canReplayRoverId(roverId, socket = null) {
return socket ? isRoverVisibleToSocket(record, socket) : false;
}
function getOdometersForSocket(socket) {
const visibleIds = getRosterForSocket(socket).map((entry) => entry.id);
return odometerService.getSnapshots(visibleIds);
}
roleEvents.on('change', ({ socket, role }) => {
if (role === 'spectator') {
enableSpectator(socket);
@@ -231,6 +238,16 @@ registerSocketHandlers({
setPrivateOpen,
lockRover,
setPrivateSafety,
getOdometersForSocket,
});
odometerService.odometerEvents.on('update', ({ roverId, odometer }) => {
io.sockets.sockets.forEach((socket) => {
const record = rovers.get(String(roverId));
if (isRoverVisibleToSocket(record, socket)) {
socket.volatile.emit('odometer:update', { roverId, odometer });
}
});
});
setInterval(tickPrivateAutoClose, PRIVATE_AUTO_CLOSE_TICK_MS);
@@ -262,4 +279,5 @@ module.exports = {
canRequestControl,
applyPrivateDriveSafety,
canReplayRoverId,
getOdometersForSocket,
};
@@ -29,6 +29,7 @@ function createSensorPipeline(deps) {
ALERT_COLOR,
sendAlert,
publishEvent,
processOdometerFrame,
isPrivateRecord,
isPrivateOpen,
getPrivateSafety,
@@ -364,6 +365,9 @@ function createSensorPipeline(deps) {
const decoded = parseSensorFrame(frame.data);
record.lastSensor = { raw: frame, decoded };
record.batteryState = computeBatteryState(record, decoded);
if (typeof processOdometerFrame === 'function') {
processOdometerFrame(roverId, decoded);
}
updateMovement(record, decoded);
const hasDockInfo = decoded?.chargingSources != null;
if (hasDockInfo) {
@@ -28,6 +28,7 @@ function registerSocketHandlers(deps) {
setPrivateOpen,
lockRover,
setPrivateSafety,
getOdometersForSocket,
} = deps;
io.on('connection', (socket) => {
@@ -177,6 +178,14 @@ function registerSocketHandlers(deps) {
cb({ success: true });
}
function handleOdometerSubscribe(_, cb = () => {}) {
try {
cb({ success: true, odometers: getOdometersForSocket(socket) });
} catch (err) {
cb({ error: err.message });
}
}
socket.on('requestControl', handleRequestControl);
socket.on('session:requestControl', handleRequestControl);
socket.on('releaseControl', handleReleaseControl);
@@ -187,6 +196,7 @@ function registerSocketHandlers(deps) {
socket.on('session:privateSafety:set', handlePrivateSafetySet);
socket.on('subscribeAll', handleSubscribeAll);
socket.on('session:subscribeAll', handleSubscribeAll);
socket.on('odometer:subscribe', handleOdometerSubscribe);
socket.on('disconnecting', () => {
logger.info('Socket disconnecting', socket.id);
@@ -94,6 +94,7 @@ function buildSession(socket) {
mode: getMode(),
isLocalNetwork: isLocalNetwork(getSocketIp(socket)),
roster,
odometers: roverManager.getOdometersForSocket(socket),
assignment: {
...assignment,
roverId: assignmentRoverId,