From 173d6fffa7a970d50c2e470f636fd5bf50ce916c Mon Sep 17 00:00:00 2001 From: legop3 Date: Thu, 8 Jan 2026 21:35:47 -0500 Subject: [PATCH] dock bump prevention --- server/src/services/commandService.js | 45 +++++++++++++++ server/src/services/roverManager.js | 80 +++++++++++++++++++++++++++ 2 files changed, 125 insertions(+) diff --git a/server/src/services/commandService.js b/server/src/services/commandService.js index f6237af6..18eccda0 100644 --- a/server/src/services/commandService.js +++ b/server/src/services/commandService.js @@ -1,9 +1,12 @@ const { v4: uuidv4 } = require('uuid'); const io = require('../globals/io'); const roverManager = require('./roverManager'); +const { isAdmin } = require('./roleService'); const logger = require('../globals/logger').child('commandService'); const pendingCommands = new Map(); // id -> { roverId } +const lastDriveActivity = new Map(); // roverId -> { ts, socketId, direction, speed, isAdmin } +const driveCooldowns = new Map(); // roverId -> blockedUntil function issueCommand(roverId, payload) { const record = roverManager.rovers.get(roverId); @@ -31,9 +34,27 @@ function handleAck(msg) { }); } +function getRecentDriveActivity(windowMs, options = {}) { + const now = Date.now(); + const results = []; + for (const [roverId, info] of lastDriveActivity.entries()) { + if (!info || now - info.ts > windowMs) continue; + if (options.excludeAdmins && info.isAdmin) continue; + results.push({ roverId, ...info }); + } + return results; +} + +function setDriveCooldown(roverId, durationMs) { + if (!roverId || !durationMs) return; + driveCooldowns.set(roverId, Date.now() + durationMs); +} + module.exports = { issueCommand, handleAck, + getRecentDriveActivity, + setDriveCooldown, }; io.on('connection', (socket) => { @@ -47,6 +68,30 @@ io.on('connection', (socket) => { throw new Error('Not your turn or no control'); } const payload = data ? { ...data } : {}; + const isAdminSocket = isAdmin(socket); + const driveDirect = payload?.driveDirect; + if (type === 'drive' && driveDirect && !isAdminSocket) { + const left = Number(driveDirect.left); + const right = Number(driveDirect.right); + const speed = Math.max(Math.abs(left), Math.abs(right)); + const isForward = Number.isFinite(left) && Number.isFinite(right) && left > 0 && right > 0; + const blockedUntil = driveCooldowns.get(roverId); + if (blockedUntil && Date.now() < blockedUntil && isForward) { + throw new Error('Drive blocked: dock protection cooldown'); + } + if (speed > 0) { + let direction = 'turn'; + if (left > 0 && right > 0) direction = 'forward'; + if (left < 0 && right < 0) direction = 'backward'; + lastDriveActivity.set(roverId, { + ts: Date.now(), + socketId: socket.id, + direction, + speed, + isAdmin: isAdminSocket, + }); + } + } const id = issueCommand(roverId, { type, ...payload }); logger.info('Queued command', socket.id, roverId, type); try { diff --git a/server/src/services/roverManager.js b/server/src/services/roverManager.js index a4289e3f..745a2484 100644 --- a/server/src/services/roverManager.js +++ b/server/src/services/roverManager.js @@ -13,6 +13,10 @@ const socketToRovers = new Map(); // socketId -> Set(roverId) const spectatorSockets = new Set(); const turnService = require('./turnService'); const managerEvents = new EventEmitter(); +const DOCK_GUARD_WINDOW_MS = 3 * 1000; +const BACKOFF_MS = 500; +const BACKOFF_SPEED = 120; +const backoffTimers = new Map(); // roverId -> Timeout function ensureRecord(id) { if (!rovers.has(id)) { @@ -21,6 +25,8 @@ function ensureRecord(id) { meta: null, ws: null, lastSensor: null, + docked: null, + lastBumpAt: null, drivers: new Set(), locked: false, lockReason: null, @@ -171,6 +177,19 @@ function handleSensorFrame(roverId, frame) { const decoded = parseSensorFrame(frame.data); record.lastSensor = { raw: frame, decoded }; record.batteryState = computeBatteryState(record, decoded); + const hasDockInfo = decoded?.chargingSources != null; + if (hasDockInfo) { + const prevDocked = record.docked; + const docked = Boolean(decoded?.chargingSources?.homeBase); + record.docked = docked; + if (prevDocked === true && docked === false) { + handleIdleUndock(record); + } + } + const bumps = decoded?.bumpsAndWheelDrops; + if (bumps?.bumpLeft || bumps?.bumpRight) { + record.lastBumpAt = Date.now(); + } io.to(record.room).emit('sensorFrame', { roverId, frame, @@ -179,6 +198,67 @@ function handleSensorFrame(roverId, frame) { managerEvents.emit('sensor', { roverId, sensors: decoded, batteryState: record.batteryState }); } +function handleIdleUndock(undockedRecord) { + if (!undockedRecord || undockedRecord.drivers.size > 0) return; + const now = Date.now(); + const { getRecentDriveActivity, setDriveCooldown, issueCommand } = require('./commandService'); + const candidates = getRecentDriveActivity(DOCK_GUARD_WINDOW_MS, { excludeAdmins: true }) + .filter((candidate) => candidate.roverId !== undockedRecord.id); + if (candidates.length === 0) return; + const candidatesWithBump = candidates.filter((candidate) => { + const record = rovers.get(candidate.roverId); + return record?.lastBumpAt && now - record.lastBumpAt <= DOCK_GUARD_WINDOW_MS; + }); + const pool = candidatesWithBump.length > 0 ? candidatesWithBump : candidates; + pool.sort((a, b) => b.ts - a.ts); + const suspect = pool[0]; + if (!suspect) return; + const suspectRecord = rovers.get(suspect.roverId); + if (!suspectRecord) return; + const bumpRecent = + suspectRecord.lastBumpAt && now - suspectRecord.lastBumpAt <= DOCK_GUARD_WINDOW_MS; + sendAlert({ + color: COLORS.warn, + title: 'Dock protection', + message: `${undockedRecord.id} undocked while idle; stopping ${suspect.roverId}.`, + }); + try { + issueCommand(suspect.roverId, { type: 'drive', driveDirect: { left: 0, right: 0 } }); + issueCommand(suspect.roverId, { type: 'motors', motorPwm: { main: 0, side: 0, vacuum: 0 } }); + } catch (err) { + logger.warn('Dock protection stop failed', suspect.roverId, err.message); + } + setDriveCooldown(suspect.roverId, DOCK_GUARD_WINDOW_MS); + if (bumpRecent) { + backoffRover(suspect.roverId); + } +} + +function backoffRover(roverId) { + if (!roverId) return; + const { issueCommand } = require('./commandService'); + clearTimeout(backoffTimers.get(roverId)); + try { + issueCommand(roverId, { + type: 'drive', + driveDirect: { left: -BACKOFF_SPEED, right: -BACKOFF_SPEED }, + }); + } catch (err) { + logger.warn('Dock protection backoff failed', roverId, err.message); + return; + } + backoffTimers.set( + roverId, + setTimeout(() => { + try { + issueCommand(roverId, { type: 'drive', driveDirect: { left: 0, right: 0 } }); + } catch (err) { + logger.warn('Dock protection backoff stop failed', roverId, err.message); + } + }, BACKOFF_MS), + ); +} + function removeSocket(socket) { const joined = socketToRovers.get(socket.id); if (!joined) {