Merge pull request #31 from legop3/REFACTOR-docking-undocking-serverside

Refactor docking undocking serverside

server/src/services/commandService/index.js

@@ -85,7 +85,7 @@ function normalizeOutboundCommandPayload(payload = {}) {
   };
 }
 
-function issueCommand(roverId, payload) {
+function issueCommand(roverId, payload, options = {}) {
   const record = roverManager.rovers.get(roverId);
   if (!record || !record.ws) {
     throw new Error('Rover offline');
@@ -93,6 +93,11 @@ function issueCommand(roverId, payload) {
   if (payload?.type === 'laser' && isLaserCommandBlockedByRoomLightLock(payload)) {
     throw new Error('Laser disabled while room lights are locked on');
   }
+  if (options.dockingOwned && payload.driveDirect && getCommandMotionMagnitude('drive', payload) > 0
+    && Date.now() < (driveCooldowns.get(roverId) || 0)) {
+    throw new Error('Drive blocked: safety cooldown');
+  }
+  payload = require('../dockingService').protectCommand(record, payload, options);
   const id = uuidv4();
   const normalizedPayload = normalizeOutboundCommandPayload(payload);
   const message = { ...normalizedPayload, id };
@@ -124,7 +129,14 @@ overcurrentProtectionService.configureCommandIssuer((roverId, payload) => {
     */
     return false;
   }
-  issueCommand(roverId, payload);
+  const record = roverManager.rovers.get(roverId);
+  const reverseAdjustment = payload.driveDirect
+    && (payload.driveDirect.left < 0 || payload.driveDirect.right < 0)
+    && payload.driveDirect.left <= 0 && payload.driveDirect.right <= 0
+    && require('../dockingService').canAdjustReverse(record);
+  // Only protection's adjustments to the active reverse inherit controller
+  // ownership. Zero remains an ordinary stop and cancels the sequence.
+  issueCommand(roverId, payload, { dockingOwned: Boolean(reverseAdjustment) });
   return true;
 });
 
@@ -315,6 +327,22 @@ function submitCommand(socket, { roverId, type, data } = {}, cb) {
     if (!isSongCommand && !isRebootCommand && !isUpdateCommand && !roverManager.canDrive(roverId, socket)) {
       throw new Error('Not your turn or no control');
     }
+    const record = roverManager.rovers.get(roverId);
+    if (!record?.ws) throw new Error('Rover offline');
+    const docking = require('../dockingService');
+    // The controller owns wheel output during undocking. Discard all ordinary
+    // drive input, including neutral, before it can alter protection's intent.
+    if (docking.shouldIgnoreDriveInput(record, payload)) {
+      reply({ ignored: true, reason: 'undocking' });
+      return;
+    }
+    // Reject before recording held motor intent: otherwise a rejected input
+    // could be replayed by protection as soon as the docking lock is released.
+    if (!docking.intents.has(type)) {
+      const guarded = docking.protectCommand(record, { ...payload, type });
+      type = guarded.type;
+      payload = guarded;
+    }
     const driveDirect = payload?.driveDirect;
     if (type === 'drive' && driveDirect) {
       if (!isAdminSocket) {
@@ -361,7 +389,14 @@ function submitCommand(socket, { roverId, type, data } = {}, cb) {
         bypassed: isAdminSocket,
       });
     }
-    const id = issueCommand(roverId, { type, ...payload });
+    const isDockingIntent = docking.intents.has(type);
+    const id = isDockingIntent
+      ? docking.issueIntent(roverManager.rovers.get(roverId), type, payload, socket)
+      : issueCommand(roverId, { ...payload, type });
+    if (isDockingIntent) {
+      // The server acknowledges accepting the intent; telemetry reports its completion.
+      io.emit('commandAck', { roverId, id, status: 'ok' });
+    }
     commandEvents.emit('observation', {
       ts: Date.now(),
       roverId: String(roverId),

server/src/services/dockingService/index.js

@@ -0,0 +1,302 @@
+// Server-owned docking operations.
+const { v4: uuidv4 } = require('uuid');
+const io = require('../../globals/io');
+const logger = require('../../globals/logger').child('dockingService');
+const states = new WeakMap();
+const intents = new Set(['enterDocking', 'cancelDocking', 'undock']);
+const ASSIST_SPEED = 60;
+const SENSOR_TIMEOUT_MS = 1500;
+const UNDOCK_TIMEOUT_MS = 4000;
+const REVERSE_TIMEOUT_MS = 1000;
+// Preserve the working OI preparation sequence; only backing away is sensor-driven.
+const MODE_COMMAND_DELAY_MS = 100;
+const FULL_MODE_SETTLE_MS = 300;
+
+function stateFor(record) {
+  if (!states.has(record)) {
+    states.set(record, {
+      contact: null, charging: false, latched: false, assist: false,
+      operation: null, error: null, seenAt: 0, timer: null, contactDockPending: false,
+    });
+  }
+  return states.get(record);
+}
+
+function snapshot(record) {
+  const state = stateFor(record);
+  return {
+    phase: state.operation ? 'undocking' : state.contact ? (state.charging ? 'charging' : 'docked') : state.assist ? 'assisting' : 'idle',
+    movementLocked: Boolean(state.operation || state.latched || state.error || state.contact == null),
+    cameraLocked: state.assist,
+    error: state.error,
+  };
+}
+
+function publish(record) {
+  if (!record.lastSensor?.decoded) return;
+  record.lastSensor.decoded = { ...record.lastSensor.decoded, docking: snapshot(record) };
+  // State transitions use the same telemetry event, including failures when the
+  // physical sensor stream has stalled. Never manufacture a new sensor timestamp.
+  io.to(record.room).emit('sensorFrame', {
+    roverId: record.id, frame: record.lastSensor.raw, sensors: record.lastSensor.decoded,
+    overcurrentProtection: record.lastOvercurrentProtection || null,
+  });
+}
+
+function send(record, payload) {
+  return require('../commandService').issueCommand(record.id, payload, { dockingOwned: true });
+}
+function stop(record) {
+  // Forget the previous held intent so protection cannot replay it after unlock.
+  require('../overcurrentProtectionService').protectCommand(record.id, 'drive', { driveDirect: { left: 0, right: 0 } });
+  send(record, { type: 'drive', driveDirect: { left: 0, right: 0 } });
+}
+function stopAllMotion(record) {
+  stop(record);
+  const payload = { motorPwm: { main: 0, side: 0, vacuum: 0 } };
+  require('../overcurrentProtectionService').protectCommand(record.id, 'motors', payload);
+  send(record, { type: 'motors', ...payload });
+}
+function mode(record, opcode) {
+  send(record, { type: 'raw', raw: Buffer.from([opcode]).toString('base64') });
+}
+function camera(record, angle) {
+  if (record.meta?.cameraServo?.enabled) send(record, { type: 'servo', servo: { angle } });
+}
+function song(record, notes, slot = 0, duration = 10) {
+  send(record, { type: 'song', song: { slot, notes: notes.map((note) => ({ note, duration })) } });
+}
+function finishAssist(record, { playExitSong = true } = {}) {
+  const state = stateFor(record);
+  if (!state.assist) return;
+  state.assist = false;
+  camera(record, 0);
+  if (playExitSong) song(record, [83, 76]);
+}
+function clearOperation(state) {
+  clearTimeout(state.timer);
+  state.timer = null;
+  state.operation = null;
+}
+function fail(record, code, message) {
+  const state = stateFor(record);
+  clearOperation(state);
+  state.error = { code, message };
+  state.latched = true;
+  try { stop(record); } catch (error) {
+    logger.warn('Unable to stop docking operation', { roverId: record.id, error: error.message });
+  }
+  publish(record);
+}
+function armDeadline(record) {
+  const state = stateFor(record);
+  clearTimeout(state.timer);
+  if (!state.operation) return;
+  const deadline = Math.min(
+    state.seenAt + SENSOR_TIMEOUT_MS,
+    state.operation.startedAt + UNDOCK_TIMEOUT_MS,
+    state.operation.reversingAt ? state.operation.reversingAt + REVERSE_TIMEOUT_MS : Infinity,
+  );
+  state.timer = setTimeout(() => {
+    const stale = Date.now() - state.seenAt >= SENSOR_TIMEOUT_MS;
+    fail(record, stale ? 'telemetryUnavailable' : 'undockTimeout', stale ? 'Rover telemetry stopped during undocking.' : 'Unable to leave the dock.');
+  }, Math.max(1, deadline - Date.now()));
+  state.timer.unref?.();
+}
+
+function handleIntent(record, type, payload, socket) {
+  const state = stateFor(record);
+  if (type === 'cancelDocking') {
+    if (state.operation) fail(record, 'undockCancelled', 'Undocking stopped.');
+    stop(record);
+    finishAssist(record);
+    publish(record);
+    return;
+  }
+  state.error = null;
+  state.contactDockPending = false;
+  require('../roverManager').stopDockGuard(record.id);
+  stop(record);
+  if (type === 'enterDocking') {
+    state.latched = false;
+    if (!state.assist) {
+      state.assist = true;
+      camera(record, record.meta?.cameraServo?.minAngle ?? -45);
+      song(record, [76, 83]);
+    }
+  } else {
+    finishAssist(record);
+    camera(record, 0);
+    state.operation = {
+      startedAt: Date.now(), socket, backoff: payload.backoff !== false, reversing: false,
+      stage: 'start', nextStepAt: Date.now() + MODE_COMMAND_DELAY_MS,
+    };
+    mode(record, 128);
+    armDeadline(record);
+  }
+  publish(record);
+}
+
+function processTelemetry(record, sensors) {
+  const state = stateFor(record);
+  if (!sensors?.chargingSources || !sensors?.oiMode) return snapshot(record);
+  const previousContact = state.contact;
+  state.seenAt = Date.now();
+  state.contact = Boolean(sensors.chargingSources.homeBase);
+  state.charging = [1, 2, 3, 4].includes(sensors.chargingState?.code);
+  if (previousContact == null) state.latched = state.contact;
+  try {
+    const operation = state.operation;
+    if (operation) {
+      const manager = require('../roverManager');
+      if (!operation.socket?.connected || !manager.canDrive(record.id, operation.socket)) {
+        fail(record, 'controlLost', 'Undocking stopped because control changed.');
+      } else if (Date.now() - operation.startedAt >= UNDOCK_TIMEOUT_MS
+        || (operation.reversingAt && Date.now() - operation.reversingAt >= REVERSE_TIMEOUT_MS)) {
+        fail(record, 'undockTimeout', 'Unable to leave the dock.');
+      } else if (Date.now() < operation.nextStepAt) {
+        // Advance only on fresh telemetry, after the previous OI command settles.
+      } else if (operation.stage === 'start') {
+        mode(record, 143);
+        operation.stage = 'dock';
+        operation.nextStepAt = Date.now() + MODE_COMMAND_DELAY_MS;
+      } else if (operation.stage === 'dock') {
+        mode(record, 132);
+        operation.stage = 'full';
+        operation.nextStepAt = Date.now() + FULL_MODE_SETTLE_MS;
+      } else if (sensors.oiMode.label === 'full') {
+        if (!state.contact || !operation.backoff) {
+          stop(record);
+          clearOperation(state);
+          state.latched = state.contact;
+        } else if (!operation.reversing) {
+          operation.reversing = true;
+          operation.reversingAt = Date.now();
+          const speed = Math.max(1, Math.min(500, Number(record.meta?.maxWheelSpeed) || 500));
+          // Track the controller's own intent in existing protection so stalls
+          // still stop it and subsequent adjustments refer to this reverse only.
+          const drive = require('../overcurrentProtectionService').protectCommand(record.id, 'drive', {
+            driveDirect: { left: -speed, right: -speed },
+          }, { bypassed: require('../roleService').isAdmin(operation.socket) });
+          if (!drive.driveDirect.left && !drive.driveDirect.right) throw new Error('Undocking blocked by wheel protection');
+          send(record, { ...drive, type: 'drive' });
+        }
+      } else if (operation.reversing) {
+        fail(record, 'modeChanged', 'Undocking stopped because the Roomba mode changed.');
+      }
+      armDeadline(record);
+    } else if (state.contact) {
+      state.latched = true;
+      if (previousContact !== true) {
+        stopAllMotion(record);
+        state.contactDockPending = true;
+        if (state.assist) song(record, [84], 1, 6);
+      } else if (state.contactDockPending && !state.charging) {
+        const speeds = sensors.wheelSpeedsMmPerSecond;
+        // A stop request does not prove the wheels stopped on the contacts.
+        // Require a subsequent frame with contact and measured stationary wheels.
+        if (speeds?.left === 0 && speeds?.right === 0) {
+          mode(record, 143);
+          state.contactDockPending = false;
+        }
+      }
+      if (state.charging) {
+        state.contactDockPending = false;
+        // An immediate exit song would replace the contact beep on the Roomba.
+        finishAssist(record, { playExitSong: false });
+      }
+    } else {
+      state.contactDockPending = false;
+      if (previousContact === true && state.assist) song(record, [72], 1, 6);
+    }
+  } catch (error) {
+    fail(record, 'commandFailed', error.message);
+  }
+  return snapshot(record);
+}
+
+function shouldIgnoreDriveInput(record, payload) {
+  if (!stateFor(record).operation) return false;
+  if (payload.driveDirect) return true;
+  const raw = payload.raw ? Buffer.from(payload.raw, 'base64') : null;
+  return Boolean(raw && [137, 145].includes(raw[0]));
+}
+
+function protectCommand(record, payload, { dockingOwned = false } = {}) {
+  if (dockingOwned) return payload;
+  const state = stateFor(record);
+  // Raw OI drive packets must obey the same movement lock. Other raw commands,
+  // including mode changes, remain available and can interrupt the operation.
+  const raw = payload.raw ? Buffer.from(payload.raw, 'base64') : null;
+  if (state.operation && raw && [128, 131, 132, 133, 134, 135, 136, 143].includes(raw[0])) {
+    // Explicit mode commands remain available and take precedence over an
+    // automatic sequence, including while it is still waiting for full mode.
+    fail(record, 'modeChanged', 'Undocking stopped by a Roomba mode command.');
+  }
+  if (raw && [137, 145].includes(raw[0])) {
+    if (raw.length !== 5) throw new Error('Invalid raw drive command');
+    const first = raw.readInt16BE(1);
+    const second = raw.readInt16BE(3);
+    if (raw[0] === 137 && first !== 0 && state.assist && !snapshot(record).movementLocked) {
+      const limited = Buffer.from(raw);
+      limited.writeInt16BE(Math.max(-ASSIST_SPEED, Math.min(ASSIST_SPEED, first)), 1);
+      return { ...payload, raw: limited.toString('base64') };
+    }
+    if (raw[0] === 145) {
+      return protectCommand(record, { type: 'drive', driveDirect: { left: second, right: first } });
+    }
+    if (first === 0 && state.operation) fail(record, 'undockStopped', 'Undocking stopped by a stop command.');
+    if (first !== 0 && snapshot(record).movementLocked) throw new Error('Drive blocked while docked or undocking');
+  }
+  // User drive input is discarded at submitCommand before reaching here.
+  // Internal safety stops still interrupt the controller through this path.
+  if (payload.driveDirect) {
+    const { left, right } = payload.driveDirect;
+    if (!Number(left) && !Number(right) && state.operation) {
+      fail(record, 'undockStopped', 'Undocking stopped by a stop command.');
+    }
+    if ((Number(left) || Number(right)) && snapshot(record).movementLocked) throw new Error('Drive blocked while docked or undocking');
+    if (state.assist) {
+      const scale = Math.min(1, ASSIST_SPEED / Math.max(Math.abs(left), Math.abs(right), 1));
+      return { ...payload, driveDirect: { left: Math.round(left * scale), right: Math.round(right * scale) } };
+    }
+  }
+  if (payload.servo && state.assist) throw new Error('Camera locked during dock assist');
+  return payload;
+}
+
+function disconnect(record) {
+  const state = stateFor(record);
+  if (state.operation) fail(record, 'roverDisconnected', 'Rover disconnected during undocking.');
+  clearOperation(state);
+  state.seenAt = 0;
+  state.contact = null;
+  state.assist = false;
+}
+
+function canAdjustReverse(record) {
+  return Boolean(record && stateFor(record).operation?.reversing);
+}
+
+function issueIntent(record, type, payload, socket) {
+  const state = stateFor(record);
+  // Validate before mutating, so duplicate/rejected requests cannot disturb an
+  // operation that is already in progress.
+  if (type !== 'cancelDocking') {
+    if (state.operation) throw new Error('Undocking is already in progress');
+    if (Date.now() - state.seenAt >= SENSOR_TIMEOUT_MS || state.contact == null) {
+      throw new Error('Waiting for current rover telemetry');
+    }
+    // Already at the requested destination; acknowledge without restarting assist.
+    if (type === 'enterDocking' && state.contact) return uuidv4();
+  }
+  const id = uuidv4();
+  try {
+    handleIntent(record, type, payload, socket);
+  } catch (error) {
+    fail(record, 'commandFailed', error.message);
+    throw error;
+  }
+  return id;
+}
+module.exports = { shouldIgnoreDriveInput, canAdjustReverse, snapshot, intents, issueIntent, processTelemetry, protectCommand, disconnect };

server/src/services/idleService/index.js

@@ -4,12 +4,15 @@
 const logger = require('../../globals/logger').child('idleService');
 const io = require('../../globals/io');
 const { getRole, roleEvents } = require('../roleService');
+const { getMode, MODES, modeEvents } = require('../modeManager');
+const { isDeterred, verificationEvents } = require('../verificationService');
 const { IDLE_TIMEOUT_MS } = require('./constants');
 const { runtime } = require('./state');
 const { runIdleActions } = require('./actions');
 const activityControls = require('../homeAssistantActivitiesService');
 
 function getActivitySnapshot() {
+  const mode = getMode();
   let onlineUsers = 0;
   let onlineAdmins = 0;
   let onlineSpectators = 0;
@@ -29,12 +32,16 @@ function getActivitySnapshot() {
       return;
     }
 
-    /*
-      Lockdown admins are counted with regular admins because both represent a
-      person with operator-level access who may be supervising the room without
-      actively driving a rover. Plain users also count even before they request
-      control, which is the behavior this service now needs.
-    */
+
+    if (
+      isDeterred(socket)
+      || (mode === MODES.ADMIN && role !== 'admin' && role !== 'lockdown')
+      || (mode === MODES.LOCKDOWN && role !== 'lockdown')
+    ) {
+      onlineIgnored += 1;
+      return;
+    }
+
     if (role === 'admin' || role === 'lockdown') {
       onlineAdmins += 1;
       return;
@@ -90,7 +97,7 @@ function scheduleIdleTimer() {
     runtime.deadlineAt = null;
     const activity = getActivitySnapshot();
     if (activity.totalActive > 0) {
-      logger.info('Idle automation skipped; user or admin online', activity);
+      logger.info('Idle automation skipped; eligible operator online', activity);
       return;
     }
     runtime.lastTriggeredAt = Date.now();
@@ -114,18 +121,12 @@ function scheduleIdleTimer() {
 function refreshIdleState() {
   const activity = getActivitySnapshot();
   logger.info('Idle state refresh', activity);
-  // Share the exact idle presence policy; Activity Controls must not count spectators or invent a second role filter.
   activityControls.setOperatorsOnline(activity.totalActive > 0);
   if (activity.totalActive > 0) {
     clearIdleTimer();
     if (runtime.idleActionsCompleted) {
       logger.info('Idle action one-shot reset; operator is online again', activity);
     }
-    /*
-      A user/admin coming online starts a new activity window. When the room
-      later becomes idle again, the cleanup pipeline should be allowed to run
-      once for that new idle period.
-    */
     runtime.idleActionsCompleted = false;
     return;
   }
@@ -133,17 +134,13 @@ function refreshIdleState() {
 }
 
 io.on('connection', (socket) => {
-  /*
-    A user/admin can be online without ever touching rover controls, so socket
-    presence has to be a first-class idle signal. The disconnect hook is just as
-    important: it is what starts the idle timeout after the last non-spectator
-    leaves, even if no driving event happens around that departure.
-  */
   refreshIdleState();
   socket.on('disconnect', refreshIdleState);
 });
 
 roleEvents.on('change', refreshIdleState);
+modeEvents.on('change', refreshIdleState);
+verificationEvents.on('change', refreshIdleState);
 
 refreshIdleState();
 

server/src/services/roverManager/index.js

@@ -274,6 +274,7 @@ odometerService.odometerEvents.on('update', ({ roverId, odometer }) => {
 setInterval(tickPrivateAutoClose, PRIVATE_AUTO_CLOSE_TICK_MS);
 
 module.exports = {
+  stopDockGuard,
   upsertRover,
   removeRover,
   lockRover,

server/src/services/roverManager/rosterLifecycle.js

@@ -117,6 +117,7 @@ function createRosterLifecycle(deps) {
       offline, while removeRoverDrivers still receives the captured record so
       it can clean the reverse membership index and Socket.IO room membership.
     */
+    require('../dockingService').disconnect(record);
     rovers.delete(id);
     removeRoverDrivers(id, record);
     stopDockGuard(id);

server/src/services/roverManager/sensorPipeline.js

@@ -389,6 +389,10 @@ function createSensorPipeline(deps) {
 
   function evaluateDockGuard(record, sensors) {
     if (!record || !sensors) return;
+    if (sensors.docking?.phase === 'undocking' || sensors.docking?.phase === 'assisting') {
+      stopDockGuard(record.id);
+      return;
+    }
     const state = getDockGuardState(record.id);
     const now = Date.now();
     const docked = Boolean(sensors?.chargingSources?.homeBase);
@@ -564,6 +568,10 @@ function createSensorPipeline(deps) {
       };
       record.lastSensor = { raw: frame, decoded };
     }
+    if (decoded) {
+      decoded = { ...decoded, docking: require('../dockingService').processTelemetry(record, decoded) };
+      record.lastSensor = { raw: frame, decoded };
+    }
     /*
       Rover manager remains responsible only for decoding and routing sensor
       frames. The dedicated service receives the completed sensor object after
@@ -577,6 +585,7 @@ function createSensorPipeline(deps) {
     const overcurrentProtection = decoded && typeof processOvercurrentTelemetry === 'function'
       ? processOvercurrentTelemetry(roverId, decoded)
       : null;
+    record.lastOvercurrentProtection = overcurrentProtection;
     updateMovement(record, decoded);
     const hasDockInfo = decoded?.chargingSources != null;
     if (hasDockInfo) {
@@ -589,6 +598,10 @@ function createSensorPipeline(deps) {
     if (bumps?.bumpLeft || bumps?.bumpRight) record.lastBumpAt = Date.now();
     handlePrivateButtonHold(record, decoded);
     evaluatePrivateSafety(record, decoded);
+    if (decoded) {
+      decoded = { ...decoded, docking: require('../dockingService').snapshot(record) };
+      record.lastSensor = { raw: frame, decoded };
+    }
     io.to(record.room).volatile.emit('sensorFrame', {
       roverId,
       frame,