Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python

teleop rewritten for MOSS: pure logic (two axes, no lateral input), validated kinematics, deadman brake window; tests reproduce the review findings; remove the internal pre-push script from the tree; build log wording

metroxffd4adc91acc
diff --git a/docs/build_log.md b/docs/build_log.md
index 7dae4ce..30492cb 100644
--- a/docs/build_log.md
+++ b/docs/build_log.md
@@ -23,7 +23,7 @@ The main chassis came off the H2S in black PETG. First fit check on the bench: c
![From print to first fit](../media/images/build_03_first_fit_parts_20260918.jpg)
![Taking shape](../media/images/build_04_taking_shape_20260918.jpg)
-Printing now on the P1S: the drivetrain, eight wheel halves (drive and idler wheels, 0.4 mm nozzle, black PETG), then twelve parts of track tensioners and bearing retainers.
+Printing now: the drivetrain, eight wheel halves (drive and idler wheels, 0.4 mm nozzle, black PETG, on the P1S), and a second plate of twelve parts, track tensioners and bearing retainers.
![Drivetrain plate](../media/images/build_05_drivetrain_plate_20260918.jpg)
![Tensioners and retainers](../media/images/build_06_tensioners_plate_20260918.jpg)
diff --git a/docs/software.md b/docs/software.md
index b282b32..5475503 100644
--- a/docs/software.md
+++ b/docs/software.md
@@ -2,8 +2,8 @@
MOSS runs on dimOS. The software here starts as a scaffold derived from vector-dimos, the external dimOS blueprint written for VECTOR (a mecanum rover on the same Jetson Orin Nano 8 GB).
-What carries over as is: gamepad teleop publishing a Twist, the rig/onboard split over zenoh, the recording tooling.
+What carries over: the idea of a gamepad module publishing a Twist behind a deadman, the rig/onboard split over zenoh, the recording tooling.
-What changes: VECTOR is holonomic (mecanum), MOSS is a differential-drive tracked rover. The strafe axis of the pad is ignored, and the motor layer is two Cytron MD10C drivers with encoder feedback instead of RS485 servo drives. `moss_dimos/diffdrive.py` holds the kinematics; the driver module is not written yet.
+What is written for MOSS: `moss_dimos/teleop_logic.py` (pure: two stick axes only, forward and yaw, no lateral input exists; deadzone; deadman with a brake window of zeros; every value validated) and `moss_dimos/diffdrive.py` (differential-drive kinematics, scaling instead of clipping, invalid configs and non-finite commands refused). `moss_dimos/gamepad.py` is the thin dimOS wrapper around them. The motor layer (two Cytron MD10C drivers with encoder feedback behind the Waveshare ESP32 board) is not written yet; neither is the stop on stale command. Both come with a written Jetson–ESP32 contract before the first roll.
Everything in `moss_dimos/` is cold-tested only until the base runs.
diff --git a/moss_dimos/diffdrive.py b/moss_dimos/diffdrive.py
index dac515d..bd63a84 100644
--- a/moss_dimos/diffdrive.py
+++ b/moss_dimos/diffdrive.py
@@ -4,32 +4,51 @@ Pure functions, no hardware, no dimOS import: cold-testable anywhere.
Convention: vx forward in m/s, wz counter-clockwise in rad/s, track_width in m
(distance between the two track centrelines). Left/right speeds in m/s at the
-track. vy is accepted and ignored: a tracked rover cannot strafe.
+track. There is no strafe on tracks: callers never pass a lateral speed.
+
+Every input is validated: a non-finite command or an invalid configuration
+raises ValueError instead of producing a motor command.
"""
from __future__ import annotations
+import math
from dataclasses import dataclass
+def _finite(name: str, value: float) -> float:
+ if not isinstance(value, (int, float)) or isinstance(value, bool) or not math.isfinite(value):
+ raise ValueError(f"{name} must be a finite number, got {value!r}")
+ return float(value)
+
+
@dataclass(frozen=True)
class DiffDriveConfig:
track_width_m: float = 0.30
max_track_speed_mps: float = 0.6
+ def __post_init__(self) -> None:
+ w = _finite("track_width_m", self.track_width_m)
+ v = _finite("max_track_speed_mps", self.max_track_speed_mps)
+ if w <= 0:
+ raise ValueError(f"track_width_m must be > 0, got {w}")
+ if v <= 0:
+ raise ValueError(f"max_track_speed_mps must be > 0, got {v} (a zero limit is a stop, not a config)")
-def twist_to_tracks(vx: float, wz: float, cfg: DiffDriveConfig, vy: float = 0.0) -> tuple[float, float]:
+
+def twist_to_tracks(vx: float, wz: float, cfg: DiffDriveConfig) -> tuple[float, float]:
"""Twist -> (left, right) track speeds in m/s, scaled to stay within the limit.
Scaling (not clipping) keeps the turn radius when a command is too fast:
a full-speed forward plus a turn slows both tracks together instead of
flattening the turn.
"""
- del vy # no strafe on tracks
+ vx = _finite("vx", vx)
+ wz = _finite("wz", wz)
half = 0.5 * cfg.track_width_m
left = vx - wz * half
right = vx + wz * half
peak = max(abs(left), abs(right))
- if peak > cfg.max_track_speed_mps > 0:
+ if peak > cfg.max_track_speed_mps:
k = cfg.max_track_speed_mps / peak
left, right = left * k, right * k
return left, right
@@ -37,6 +56,14 @@ def twist_to_tracks(vx: float, wz: float, cfg: DiffDriveConfig, vy: float = 0.0)
def tracks_to_twist(left: float, right: float, cfg: DiffDriveConfig) -> tuple[float, float]:
"""(left, right) track speeds -> (vx, wz). Inverse of twist_to_tracks when unscaled."""
+ left = _finite("left", left)
+ right = _finite("right", right)
vx = 0.5 * (left + right)
wz = (right - left) / cfg.track_width_m
return vx, wz
+
+
+def clamp_twist(vx: float, wz: float, cfg: DiffDriveConfig) -> tuple[float, float]:
+ """Twist -> the same twist scaled so that neither track exceeds the limit."""
+ left, right = twist_to_tracks(vx, wz, cfg)
+ return tracks_to_twist(left, right, cfg)
diff --git a/moss_dimos/gamepad.py b/moss_dimos/gamepad.py
index 9e4a211..c186f1a 100644
--- a/moss_dimos/gamepad.py
+++ b/moss_dimos/gamepad.py
@@ -1,24 +1,21 @@
-"""Gamepad teleop for MOSS - derived from vector-dimos/gamepad.py (Apache-2.0).
+"""Gamepad teleop for MOSS as a dimOS module: reads a pygame joystick, publishes Twist.
-MOSS is a differential-drive tracked rover: the strafe axis (vy) published here is
-ignored downstream by moss_dimos.diffdrive. Everything else (deadman, brake window,
-hot-plug, headless SDL) is the VECTOR behaviour, unchanged and cold-tested there.
-Not yet run on MOSS hardware.
+All decision logic lives in `moss_dimos.teleop_logic` (pure, cold-tested here);
+this file only wires it to pygame and to the dimOS stream. Runs headless (SDL
+dummy drivers) and waits for a pad instead of failing at startup.
+
+Not yet run on the MOSS base: there is no motor driver module in this
+repository yet (see docs/software.md).
"""
from __future__ import annotations
-from dataclasses import dataclass
import os
import threading
import time
-from typing import Any
-# SDL picks its drivers at import time, so this has to run before "import
-# pygame" (which happens lazily in the worker thread below). setdefault, not
-# assignment: an operator who exports SDL_VIDEODRIVER=x11 keeps their display.
os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
os.environ.setdefault("SDL_AUDIODRIVER", "dummy")
-os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1") # no banner in daemon logs
+os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
from dimos.core.core import rpc
from dimos.core.module import Module
@@ -27,460 +24,74 @@ from dimos.msgs.geometry_msgs.Twist import Twist
from dimos.msgs.geometry_msgs.Vector3 import Vector3
from dimos.utils.logging_config import setup_logger
-logger = setup_logger()
+from .teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twist
-DEFAULT_LINEAR_SPEED = 0.6 # m/s at full stick
-DEFAULT_ANGULAR_SPEED = 1.2 # rad/s at full stick
-DEFAULT_DEADZONE = 0.32
-DEFAULT_RATE_HZ = 50.0
-DEFAULT_BOOST = 2.0
-RESCAN_PERIOD_S = 2.0 # how often we re-scan SDL for a pad
+logger = setup_logger()
-# SDL axis indices on a standard dual-stick pad (DS4/DS5, Xbox, 8BitDo).
-AXIS_LEFT_X = 0
+# Standard dual-stick pad (pygame numbering). Override through the constructor.
AXIS_LEFT_Y = 1
-AXIS_RIGHT_X = 2 # MEASURED 27/08 15h21: right-stick X sweeps axis 2 full travel on this pad/mode (axis 3 is its vertical leakage - the random rotation)
-AXIS_R2 = 5
-
-
-@dataclass(frozen=True)
-class TeleopConfig:
- """Speed envelope for :func:`axes_to_twist` - no pygame, no module."""
-
- linear_speed: float = DEFAULT_LINEAR_SPEED
- angular_speed: float = DEFAULT_ANGULAR_SPEED
- deadzone: float = DEFAULT_DEADZONE
- boost_multiplier: float = DEFAULT_BOOST
-
-
-def apply_deadzone(value: float, deadzone: float) -> float:
- """Hard deadzone: below the threshold the axis reads zero.
-
- No rescaling of the remaining travel - a stick just past the deadzone
- gives a small command, which is what you want for fine positioning.
- """
- return 0.0 if abs(value) < deadzone else value
-
-
-# Absolute ceilings, applied LAST, regardless of config or boost. Born of the
-# 27/08 13h42 runaway: a fresh xboxdrv delivered full-deflection uninitialised
-# axes straight into tele_cmd_vel (priority channel, no guard anywhere) and
-# the rover left at 1200 W. No teleop output may ever exceed these.
-CLAMP_LINEAR_MS = 0.45
-SLEW_LINEAR_MS2 = 0.6 # max change of linear command per second: full stick
- # reaches the ceiling in ~0.75 s instead of one step -
- # the mecanum wheelspin fix, on top of the 400 ms ZLAC
- # ramps the adapter writes (the era tuning)
-CLAMP_ANGULAR_RADS = 0.8
-# Mecanum wheels ADD the commands: rim speed = |vx| + |vy| + (Lx+Ly)|wz|.
-# Felt on the very first piloted lap (2026-08-27): mixing stick and rotation
-# made the rover suddenly take off - each axis was under its own clamp while the
-# wheels ran near double. Envelope = the fastest single-stick feel (0.45).
-MECANUM_LEVER_M = 0.50 # Lx+Ly: half wheelbase 0.27 + half track 0.23 (54x46 cm chassis)
-WHEEL_ENVELOPE_MS = 0.45 # max rim speed however the sticks are mixed
-DEADMAN_BUTTON = 7 # MEASURED 2026-08-27: the chosen deadman button was pressed 21x -> index 7 (shanwan pad, Android mode). Held = commands allowed; released = zeros, always
-BRAKE_S = 0.5 # how long the zeros keep flowing after the deadman is
- # released - then TOTAL SILENCE until it is held again.
- # dimOS's MovementManager reads ANY tele_cmd_vel message,
- # zeros included, as "a human is driving": it cancels the
- # nav goal and mutes nav_cmd_vel for tele_cooldown_sec
- # (1.0 s). A pad at rest publishing 50 Hz of zeros
- # therefore killed exploration for good (28/08 audit).
- # 0.5 s < 1.0 s: the brake is seen, then autonomy gets
- # the bus back and the two coexist.
-# NOTE (2026-08-27): when this pad's radio dies mid-drive (sleep, battery -
-# observed on a piloted lap), the rover STOPS cleanly - observed behaviour, no
-# radio watchdog needed. Revisit ONLY if a radio death ever leaves a non-zero
-# command running.
-
-
-# ── 2026-09-13: metrox's two complaints from the 30/08 piloted lap ──────────
-# (docs/notes_etabli.md, his words) :
-# "transitions de commandes pas propres : relacher un stick puis le remettre
-# vite -> lag / collision de commandes"
-# "inertie teleop excessive : stick lache -> le rover glisse encore ~1,5 m"
-# One constant per button. Restoring the 2026-09-12 flight is one line each:
-# BRAKE_CANCEL_ON_DEADMAN = False, and brake_s left alone (0.5 s).
-
-BRAKE_CANCEL_ON_DEADMAN = True
-# A new deadman press CANCELS whatever is left of the brake window, on its
-# RISING EDGE (brake_until = 0.0). From that instant the brake path cannot emit
-# one more zero, however long the window was.
-#
-# MEASURED on the cold bench 2026-09-13, BEFORE this change, at 50 Hz with
-# BRAKE_S = 0.5 s and the deadman re-pressed 0.200 s after the release: the
-# module already published the stick command from the first tick after the
-# press (+0.213 s -> 0.012 m/s, one slew step) and no zero after it. So this
-# does NOT change the default flight, and it is NOT the cure for the felt lag
-# (that one is the slew restarting from rest: 0.6 m/s2 = 0.75 s to the ceiling).
-# What it buys is the right to LENGTHEN the zero window (brake_s below) without
-# a stale zero from the release ever chasing a fresh command down the bus -
-# which is precisely what the anti-inertia button wants to do.
-# False = the 12/09 behaviour exactly: the window always runs to its end.
-
-# The length of that window is now a knob, not a literal. 0.5 s is the value
-# flown since 28/08 (see BRAKE_S); the anti-inertia trial raises it towards
-# 1.0 s so the drives keep being TOLD to stop for longer instead of falling
-# silent and free-wheeling. dimOS builds this module from a blueprint with no
-# arguments of ours, so the environment is the operator's only lever:
-# VECTOR_BRAKE_S=1.0 tools/fly.sh GAMEPAD=1 REPOSITIONNE=1
-BRAKE_S_ENV = "VECTOR_BRAKE_S"
-BRAKE_S_MAX = 1.0
-# Why the ceiling is 1.0 and not more - the cost, in seconds, not a story.
-# dimOS's MovementManager treats ANY tele_cmd_vel message, zeros included, as
-# "a human is driving": _on_teleop cancels the nav goal on EVERY message and
-# _on_nav refuses to forward nav_cmd_vel until tele_cooldown_sec = 1.0 s has
-# passed since the LAST teleop message (movement_manager.py, re-read
-# 2026-09-13). So every release mutes autonomy for brake_s + 1.0 s: 1.5 s
-# today, 2.0 s at this ceiling. That number IS the justification.
-# TWO CLAIMS THAT USED TO STAND HERE AND WERE FALSE (adversarial review,
-# 2026-09-13) - written down so nobody rebuilds on them:
-# 1. "above the ceiling a pad at rest would mute exploration FOR GOOD". No:
-# whatever brake_s is, the zeros STOP at brake_s (see the worker loop) and
-# autonomy comes back 1.0 s later. "For good" was the pre-28/08 module,
-# which published 50 Hz of zeros for ever. The ceiling is prudence about
-# the 2 s above, not a cliff.
-# 2. "in the GAMEPAD=1 blueprint the gamepad IS the commander (direct drive
-# to cmd_vel, no nav), so the cost there is zero". That describes
-# `vector-dimos.gamepad` (blueprints.py remaps tele_cmd_vel -> cmd_vel).
-# tools/fly.sh runs `vector-dimos.explore`, where GAMEPAD=1 only ADDS this
-# module with NO remapping (nav_blueprints.py): the pad publishes on
-# tele_cmd_vel and MovementManager IS in the path. The cost on the flight
-# metrox actually runs is 2.0 s, not zero.
-
-BRAKE_UNTIL_STOPPED = False
-# ASKED FOR on 2026-09-13, and NOT implemented on purpose - here is why, so
-# nobody re-opens it blind. The idea: keep publishing zeros until the wheels
-# actually read below BRAKE_STOP_RPM, capped at BRAKE_STOP_MAX_S, then silence.
-# It needs wheel-speed feedback INSIDE this module, and this module has none:
-# GamepadTeleop declares exactly one stream, `tele_cmd_vel: Out[Twist]`, and it
-# runs in its own forkserver worker. The RPM feedback is read by
-# VectorBaseAdapter (adapter.read_velocities) in the COORDINATOR's process,
-# and nothing publishes it anywhere this module could subscribe to. Wiring it
-# would mean: a new Out on the coordinator, a new In here, a blueprint
-# remapping, and a new failure mode (feedback stops arriving -> the brake never
-# ends -> the pad mutes autonomy for ever) - i.e. new machinery inside an armed
-# chain, which is the one thing the 27/08 incident says not to do.
-# THE FALLBACK IS brake_s ABOVE: a longer window of zeros, no new plumbing, one
-# environment variable. Setting this flag True changes nothing but a warning.
-BRAKE_STOP_RPM = 5.0 # kept as the spec of the day we do wire feedback
-BRAKE_STOP_MAX_S = 2.0 # ...and its safety cap, so silence always comes
-
-
-def resolve_brake_s(brake_s: float | None = None,
- env: dict | None = None) -> float:
- """How long zeros keep flowing after the deadman is released, in seconds.
-
- Pure, cold-testable. Precedence: explicit argument > VECTOR_BRAKE_S >
- BRAKE_S (0.5 s, the 28/08 value). Out-of-range values are CLAMPED to
- [0.0, BRAKE_S_MAX] and logged; unparseable text falls back to BRAKE_S,
- loudly - a mistyped knob gives yesterday's flight, never a random one.
- """
- raw: float | None = None if brake_s is None else float(brake_s)
- if raw is None:
- text = (os.environ if env is None else env).get(BRAKE_S_ENV, "")
- text = text.strip() if isinstance(text, str) else ""
- if text:
- try:
- raw = float(text)
- except ValueError:
- logger.warning("%s=%r is not a number - brake window stays at "
- "%.2f s", BRAKE_S_ENV, text, BRAKE_S)
- raw = None
- if raw is None:
- return BRAKE_S
- value = max(0.0, min(BRAKE_S_MAX, raw))
- if value != raw:
- logger.warning("brake window %.2f s is outside [0.0, %.2f] s - clamped "
- "to %.2f s (above the ceiling a released pad mutes "
- "dimOS autonomy for good; see BRAKE_S_MAX)",
- raw, BRAKE_S_MAX, value)
- return value
-
-
-def brake_window_after_press(brake_until: float, deadman: bool,
- prev_deadman: bool) -> float:
- """The brake deadline this tick keeps - pure, cold-testable.
-
- RISING edge of the deadman (released -> held) cancels the window by
- returning 0.0, which is always in the past: the brake path can no longer
- publish. Anything else returns the window untouched - in particular a
- deadman simply HELD, because every driving tick re-arms the window itself
- and that one must live its full brake_s after the next release.
-
- With BRAKE_CANCEL_ON_DEADMAN = False this is the identity function, i.e.
- exactly the 2026-09-12 flight.
- """
- if BRAKE_CANCEL_ON_DEADMAN and deadman and not prev_deadman:
- return 0.0
- return brake_until
-
-
-def slew(prev: float, target: float, dt: float) -> float:
- """Rate-limit a linear command - pure, cold-testable."""
- step = SLEW_LINEAR_MS2 * dt
- return max(prev - step, min(prev + step, target))
-
-
-def clamp_twist(vx: float, vy: float, wz: float) -> tuple[float, float, float]:
- """The last gate before the bus - pure, cold-testable. Per-axis clamps,
- then the mecanum wheel envelope: translation and rotation ADD at the rim,
- so a mixed command is scaled down proportionally (the feel is preserved,
- the top speed is not exceeded)."""
- lim = CLAMP_LINEAR_MS
- vx = max(-lim, min(lim, vx))
- vy = max(-lim, min(lim, vy))
- wz = max(-CLAMP_ANGULAR_RADS, min(CLAMP_ANGULAR_RADS, wz))
- rim = abs(vx) + abs(vy) + MECANUM_LEVER_M * abs(wz)
- if rim > WHEEL_ENVELOPE_MS:
- k = WHEEL_ENVELOPE_MS / rim
- vx, vy, wz = vx * k, vy * k, wz * k
- return (vx, vy, wz)
-
-
-def axes_neutral(ax0: float, ax1: float, ax3: float, deadzone: float) -> bool:
- """True when every drive axis rests inside the deadzone - the trust gate:
- a pad (or a userspace driver) must be SEEN at neutral once before a single
- non-zero command is believed. Uninitialised axes never pass this."""
- return abs(ax0) < deadzone and abs(ax1) < deadzone and abs(ax3) < deadzone
-
-
-def axes_to_twist(ax0: float, ax1: float, ax3: float, ax5: float,
- cfg: TeleopConfig) -> tuple[float, float, float]:
- """Raw SDL axis values -> (vx m/s, vy m/s, wz rad/s).
-
- Pure function - the whole mapping lives here so it is testable without a
- pad, a display, or pygame.
-
- SDL reports stick up and stick left as NEGATIVE, while the robot frame is
- x forward / y left / wz counter-clockwise, hence the three sign flips:
- ax0 left stick X -> vy (stick left = -1 -> strafe left, +y)
- ax1 left stick Y -> vx (stick up = -1 -> forward, +x)
- ax3 right stick X -> wz (stick right = +1 -> clockwise, -wz)
- ax5 R2 -> boost when > 0 (trigger rests at -1)
-
- Boost multiplies the linear axes only: rotation stays at its capped rate,
- because a spin fast enough to be useful is already fast enough to hurt.
- """
- boost = cfg.boost_multiplier if ax5 > 0.0 else 1.0
- vx = -apply_deadzone(ax1, cfg.deadzone) * cfg.linear_speed * boost
- vy = -apply_deadzone(ax0, cfg.deadzone) * cfg.linear_speed * boost
- wz = -apply_deadzone(ax3, cfg.deadzone) * cfg.angular_speed
- return clamp_twist(vx, vy, wz)
+AXIS_RIGHT_X = 2
+BUTTON_DEADMAN = 4 # L1
+BUTTON_BOOST = 5 # R1
class GamepadTeleop(Module):
- """pygame.joystick teleop. Outputs Twist on tele_cmd_vel."""
-
- dedicated_worker = True
+ """Publishes `tele_cmd_vel` while the deadman is held, zeros for brake_s after, then nothing."""
tele_cmd_vel: Out[Twist]
- def __init__(self, linear_speed: float = DEFAULT_LINEAR_SPEED,
- angular_speed: float = DEFAULT_ANGULAR_SPEED,
- deadzone: float = DEFAULT_DEADZONE,
- rate_hz: float = DEFAULT_RATE_HZ,
- boost_multiplier: float = DEFAULT_BOOST,
- joystick_index: int = 0, brake_s: float | None = None,
- **kwargs: Any) -> None:
+ def __init__(self, config: TeleopConfig | None = None, axis_left_y: int = AXIS_LEFT_Y,
+ axis_right_x: int = AXIS_RIGHT_X, button_deadman: int = BUTTON_DEADMAN,
+ button_boost: int = BUTTON_BOOST, **kwargs) -> None:
super().__init__(**kwargs)
- self.cfg = TeleopConfig(linear_speed=linear_speed,
- angular_speed=angular_speed,
- deadzone=deadzone,
- boost_multiplier=boost_multiplier)
- self.rate_hz = rate_hz
- self.joystick_index = joystick_index
- # 2026-09-13: None -> BRAKE_S (0.5 s), or VECTOR_BRAKE_S. Resolved HERE,
- # at construction, so the value is fixed and logged once for a flight -
- # not re-read per tick.
- self.brake_s = resolve_brake_s(brake_s)
- self._stop_event = threading.Event()
+ self.cfg = config or TeleopConfig()
+ self.axis_left_y, self.axis_right_x = axis_left_y, axis_right_x
+ self.button_deadman, self.button_boost = button_deadman, button_boost
+ self._stop = threading.Event()
self._thread: threading.Thread | None = None
- # @rpc is not decoration for its own sake: Module.start/stop carry it,
- # and an override that drops it falls out of the class's rpcs table.
- # dimOS then proxies the call by pickling the module across the worker
- # pipe, which dies on our threading.Event ('cannot pickle _thread.lock').
@rpc
def start(self) -> None:
- super().start()
- self._stop_event.clear()
- self._thread = threading.Thread(target=self._worker,
- name="vector-gamepad", daemon=True)
+ self._stop.clear()
+ self._thread = threading.Thread(target=self._loop, name="moss-gamepad", daemon=True)
self._thread.start()
@rpc
def stop(self) -> None:
- self._stop_event.set()
- if self._thread is not None:
+ self._stop.set()
+ if self._thread:
self._thread.join(timeout=2.0)
- super().stop()
-
- def _publish(self, vx: float, vy: float, wz: float) -> None:
- self.tele_cmd_vel.publish(Twist(
- linear=Vector3(x=vx, y=vy, z=0.0),
- angular=Vector3(x=0.0, y=0.0, z=wz)))
- def _acquire(self, pygame: Any) -> Any:
- """Re-scan SDL for the pad. Returns an initialised joystick or None."""
- try:
- pygame.event.pump() # let SDL process hotplug events
- pygame.joystick.quit() # full re-scan: SDL caches the device
- pygame.joystick.init() # list from the last init()
- if pygame.joystick.get_count() <= self.joystick_index:
- return None
- pad = pygame.joystick.Joystick(self.joystick_index)
- pad.init()
- return pad
- except pygame.error as exc:
- logger.debug("Gamepad scan failed: %s", exc)
- return None
+ def _publish(self, vx: float, wz: float) -> None:
+ self.tele_cmd_vel.publish(Twist(linear=Vector3(x=vx, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=wz)))
- # Named _worker, NOT _loop: dimOS's Module keeps its asyncio event
- # loop in self._loop, which would shadow the method and make the
- # thread target the event loop object.
- def _worker(self) -> None:
- try:
- import pygame
- except ImportError:
- logger.error("pygame is not installed - gamepad teleop is off "
- "(pip install 'vector-dimos[gamepad]')")
- return
+ def _loop(self) -> None:
+ import pygame # optional dependency: pip install 'moss-dimos[gamepad]'
- pygame.init() # dummy video/audio drivers: works with no display
+ pygame.init()
pygame.joystick.init()
- period = 1.0 / self.rate_hz
+ period = 1.0 / self.cfg.rate_hz
+ brake = DeadmanBrake(self.cfg.brake_s)
pad = None
- trusted = False # neutral-first trust gate (13h42 runaway)
- prev_vx = prev_vy = 0.0 # slew-limiter state
- brake_until = 0.0 # publish zeros until this instant, then shut up
- prev_deadman = False # to see the RISING edge of the deadman (2026-09-13)
- waiting_logged = False
- if BRAKE_UNTIL_STOPPED:
- # Declared, never wired - see the constant. Say so at every start
- # rather than let an operator believe in a brake that is not there.
- logger.warning("BRAKE_UNTIL_STOPPED=True is INERT: this module has "
- "no wheel-speed feedback (see the constant). The "
- "brake window stays at %.2f s - raise it with %s.",
- self.brake_s, BRAKE_S_ENV)
- logger.info("gamepad brake window: %.2f s of zeros after the deadman is "
- "released, then silence (cancel-on-press: %s)",
- self.brake_s, BRAKE_CANCEL_ON_DEADMAN)
- try:
- while not self._stop_event.is_set():
- if pad is None:
- pad = self._acquire(pygame)
- if pad is None:
- if not waiting_logged:
- logger.info("waiting for gamepad (index %d)",
- self.joystick_index)
- waiting_logged = True
- self._stop_event.wait(RESCAN_PERIOD_S)
- continue
- waiting_logged = False
- trusted = False # every (re)connection re-earns trust at neutral
- logger.info("Gamepad connected: %s - waiting to see the sticks at NEUTRAL "
- "before trusting a single command", pad.get_name())
- try:
- pygame.event.pump()
- axes = (_axis(pad, AXIS_LEFT_X), _axis(pad, AXIS_LEFT_Y),
- _axis(pad, AXIS_RIGHT_X), _axis(pad, AXIS_R2))
- deadman = bool(pad.get_button(DEADMAN_BUTTON)) if pad.get_numbuttons() > DEADMAN_BUTTON else False
- if pygame.joystick.get_count() <= self.joystick_index:
- raise pygame.error("joystick disappeared")
- except pygame.error as exc:
- pad = None
- trusted = False
- prev_deadman = False # a pad that is gone holds nothing
- # ... and it holds no RAMP either (2026-09-13, adversarial
- # bench). prev_vx/prev_vy were reset on the deadman RELEASE
- # path only, so a pad that vanished for one read while
- # driving at the ceiling came back with 0.45 m/s of slew
- # state: the first trusted tick, STICKS AT NEUTRAL, published
- # +0.438 m/s and the rover left on its own for ~0.75 s -
- # the exact 28/08 replay the release path was written to
- # kill. The trust gate does not catch it: neutral axes
- # satisfy the gate, and the danger is inside this module.
- prev_vx = prev_vy = 0.0
- self._publish(0.0, 0.0, 0.0) # one zero Twist, then wait
- logger.warning("Gamepad lost (%s) - back to waiting", exc)
- continue
- # TRUST GATE (the 13h42 runaway): until the sticks have been
- # SEEN at neutral once, this pad's words are worth nothing.
- if not trusted:
- if axes_neutral(axes[0], axes[1], axes[2], self.cfg.deadzone):
- trusted = True
- logger.info("Gamepad axes seen at neutral - commands now trusted "
- "(hold the deadman button to drive)")
- else:
- if time.monotonic() < brake_until:
- self._publish(0.0, 0.0, 0.0)
- time.sleep(period)
- continue
- # BRAKE CANCEL (2026-09-13, metrox 30/08 "collision de
- # commandes"): the RISING edge of the deadman ends the brake
- # window there and then, so not one zero left over from the
- # release can still go out behind the commands of the new
- # press. Rising edge, not "held": a window re-armed by a tick
- # that DROVE (below) must keep its own life.
- # Set BRAKE_CANCEL_ON_DEADMAN = False to fly the 12/09 way.
- #
- # ORDER MATTERS, and it was WRONG for a few hours on 2026-09-13
- # (caught by an adversarial cold bench, fixed the same day):
- # these two lines used to sit ABOVE the trust gate. On any pad
- # that has not yet earned trust - i.e. after EVERY reconnection,
- # and after every one-tick read glitch, which is exactly when
- # the rover is rolling with nobody commanding it - a deadman
- # press cancelled the brake window while the untrusted path
- # could publish NOTHING to replace it. Measured on that bench:
- # a single missed read at 0.45 m/s took the zeros published
- # after the dropout from 25 (to 0.486 s) down to 1 (0.011 s).
- # BRAKE_S exists to REPEAT the stop order on a latest-only bus;
- # the cancel may therefore only run on a tick that is itself
- # about to publish a command - i.e. here, below the gate.
- brake_until = brake_window_after_press(brake_until, deadman,
- prev_deadman)
- prev_deadman = deadman
- # DEADMAN: no held button, no motion - ever.
- if not deadman:
- # Releasing also resets the slew state: the ramp restarts
- # from rest, so a re-press with the sticks centred cannot
- # replay the last driven speed (28/08 audit: 0.44 m/s for
- # ~0.75 s, sticks at rest).
- prev_vx = prev_vy = 0.0
- # BRAKE THEN SILENCE (see BRAKE_S / self.brake_s): zeros for
- # brake_s after the last command, then not one message until
- # the deadman is held again - a released pad must not mute
- # autonomy.
- if time.monotonic() < brake_until:
- self._publish(0.0, 0.0, 0.0)
- time.sleep(period)
- continue
- vx, vy, wz = axes_to_twist(*axes, self.cfg)
- vx = slew(prev_vx, vx, period)
- vy = slew(prev_vy, vy, period)
- prev_vx, prev_vy = vx, vy
- # envelope AFTER the slew too: while vx decays and wz is
- # instant, the mix could transiently exceed the rim ceiling
- vx, vy, wz = clamp_twist(vx, vy, wz)
- self._publish(vx, vy, wz)
- brake_until = time.monotonic() + self.brake_s
- time.sleep(period)
- finally:
+ while not self._stop.is_set():
+ if pad is None:
+ pygame.joystick.quit(); pygame.joystick.init()
+ if pygame.joystick.get_count() == 0:
+ time.sleep(1.0); continue
+ pad = pygame.joystick.Joystick(0); pad.init()
+ logger.info(f"gamepad connected: {pad.get_name()}")
+ pygame.event.pump()
try:
- self._publish(0.0, 0.0, 0.0)
- except Exception: # stream already torn down: nothing to do
- logger.debug("no zero Twist on shutdown", exc_info=True)
- pygame.joystick.quit()
- pygame.quit()
-
-
-def _axis(pad: Any, index: int) -> float:
- """Axis value, or 0.0 when this pad has no such axis.
-
- Pads differ (some report 4 axes, not 6). A missing R2 must read as "no
- boost", not throw the loop into the reconnect path on every tick.
- """
- if index >= pad.get_numaxes():
- return 0.0
- return float(pad.get_axis(index))
+ held = bool(pad.get_button(self.button_deadman))
+ boost = bool(pad.get_button(self.button_boost))
+ ly, rx = pad.get_axis(self.axis_left_y), pad.get_axis(self.axis_right_x)
+ except pygame.error:
+ logger.warning("gamepad lost; publishing zeros then waiting for it")
+ self._publish(0.0, 0.0); pad = None; continue
+ state = brake.tick(held, time.monotonic())
+ if state == "command":
+ vx, wz = axes_to_twist(ly, rx, self.cfg, boost_held=boost)
+ self._publish(vx, wz)
+ elif state == "brake":
+ self._publish(0.0, 0.0)
+ time.sleep(period)
diff --git a/moss_dimos/teleop_logic.py b/moss_dimos/teleop_logic.py
new file mode 100644
index 0000000..3c2531a
--- /dev/null
+++ b/moss_dimos/teleop_logic.py
@@ -0,0 +1,100 @@
+"""Gamepad teleop logic for MOSS, pure and cold-testable (no pygame, no dimOS).
+
+The pad drives a differential-drive tracked base: the LEFT stick vertical axis
+is forward speed, the RIGHT stick horizontal axis is yaw rate. No other axis is
+read, so no lateral input can ever change a track command.
+
+Safety behaviour (state machine in DeadmanBrake):
+- commands are published only while the deadman button is held;
+- on release, zeros are published for `brake_s` seconds (the brake window),
+ then nothing at all: a pad at rest owns nothing;
+- pressing the deadman again ends the brake window immediately.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+from .diffdrive import DiffDriveConfig, clamp_twist
+
+
+def _finite(name: str, value: float) -> float:
+ if not isinstance(value, (int, float)) or isinstance(value, bool) or not math.isfinite(value):
+ raise ValueError(f"{name} must be a finite number, got {value!r}")
+ return float(value)
+
+
+@dataclass(frozen=True)
+class TeleopConfig:
+ drive: DiffDriveConfig = DiffDriveConfig()
+ max_yaw_rps: float = 2.0 # yaw rate at full right-stick deflection
+ deadzone: float = 0.08 # stick values below this (absolute) read as 0
+ boost: float = 1.0 # multiplier while the boost button is held (1.0 = off)
+ brake_s: float = 0.5 # zeros published after the deadman is released
+ rate_hz: float = 50.0
+
+ def __post_init__(self) -> None:
+ y = _finite("max_yaw_rps", self.max_yaw_rps)
+ d = _finite("deadzone", self.deadzone)
+ b = _finite("boost", self.boost)
+ s = _finite("brake_s", self.brake_s)
+ r = _finite("rate_hz", self.rate_hz)
+ if y <= 0:
+ raise ValueError("max_yaw_rps must be > 0")
+ if not 0 <= d < 1:
+ raise ValueError("deadzone must be in [0, 1)")
+ if b < 1:
+ raise ValueError("boost must be >= 1 (1.0 disables it)")
+ if s < 0:
+ raise ValueError("brake_s must be >= 0")
+ if r <= 0:
+ raise ValueError("rate_hz must be > 0")
+
+
+def _deadzone(x: float, dz: float) -> float:
+ """Rescale a stick axis so the output is 0 inside the deadzone and reaches ±1 at the rim."""
+ a = abs(x)
+ if a <= dz:
+ return 0.0
+ y = (a - dz) / (1.0 - dz)
+ return math.copysign(min(y, 1.0), x)
+
+
+def axes_to_twist(left_y: float, right_x: float, cfg: TeleopConfig, boost_held: bool = False) -> tuple[float, float]:
+ """Stick axes in [-1, 1] -> (vx m/s, wz rad/s), already clamped to the track envelope.
+
+ Pad convention: pushing the left stick UP reads negative on most pads, so
+ forward is -left_y. Pushing the right stick RIGHT (positive) turns right,
+ which is a negative yaw rate (counter-clockwise positive).
+ """
+ ly = _finite("left_y", left_y)
+ rx = _finite("right_x", right_x)
+ ly = max(-1.0, min(1.0, ly))
+ rx = max(-1.0, min(1.0, rx))
+ k = cfg.boost if boost_held else 1.0
+ vx = -_deadzone(ly, cfg.deadzone) * cfg.drive.max_track_speed_mps * k
+ wz = -_deadzone(rx, cfg.deadzone) * cfg.max_yaw_rps * k
+ return clamp_twist(vx, wz, cfg.drive)
+
+
+class DeadmanBrake:
+ """Decides, each tick, what to publish: 'command', 'brake' (zeros) or 'silent'."""
+
+ def __init__(self, brake_s: float) -> None:
+ self.brake_s = _finite("brake_s", brake_s)
+ self._held = False
+ self._released_at: float | None = None
+
+ def tick(self, deadman_held: bool, now_s: float) -> str:
+ now_s = _finite("now_s", now_s)
+ if deadman_held:
+ self._held = True
+ self._released_at = None
+ return "command"
+ if self._held:
+ self._held = False
+ self._released_at = now_s
+ if self._released_at is not None and now_s - self._released_at <= self.brake_s:
+ return "brake"
+ self._released_at = None
+ return "silent"
diff --git a/scripts/git-pre-push-prive.sh b/scripts/git-pre-push-prive.sh
deleted file mode 100755
index a6824ca..0000000
--- a/scripts/git-pre-push-prive.sh
+++ /dev/null
@@ -1,33 +0,0 @@
-#!/usr/bin/env bash
-# git-pre-push-prive.sh — LE VERROU « privé = local uniquement » (metrox 04/09/2026 13h28 :
-# « il faut blinder ça pour que ça n'arrive plus, les trucs privés ça devrait être en local uniquement »).
-# Installé comme .git/hooks/pre-push dans les dépôts qui portent du privé. Il refuse TOUT push, sauf
-# ALLOW_PUSH=1 explicite ET aucun fichier de la liste privée dans la plage poussée.
-# Né du palais poussé sur GitHub par la boucle RSI (20-27/05/2026, découvert le 02/09).
-set -u
-REPO="$(git rev-parse --show-toplevel)"
-case "$REPO" in
- */memory) echo "✗ pre-push : le PALAIS ne se pousse JAMAIS, nulle part (règle absolue)." >&2; exit 1;;
-esac
-PRIVE_RE='^(CURRENT\.md|TODO\.md|PRIVE.*|prive/|_perime/CURRENT_|_perime/ORDRES_|docs/charte_magicien|docs/love_first|docs/dossier_showrobotics|docs/astra_|docs/vita_camping|docs/discord_recon|docs/dossier_pitch|docs/journal|docs/reves|docs/reveils|docs/.*prive|.*secrets\.env|.*\.env$|infra/wg0)'
-if [ "${ALLOW_PUSH:-0}" != "1" ]; then
- echo "✗ pre-push refusé : ce dépôt porte du privé (CURRENT, TODO, journaux, recon). Un push demande" >&2
- echo " ALLOW_PUSH=1 explicite ET une plage sans fichier privé. Le hors-site du privé = backup chiffré, pas GitHub." >&2
- exit 1
-fi
-# plage poussée : lit stdin (local_ref local_sha remote_ref remote_sha)
-Z=0000000000000000000000000000000000000000
-while read -r lref lsha rref rsha; do
- [ "$lsha" = "$Z" ] && continue
- if [ "$rsha" = "$Z" ]; then
- # branche neuve chez le distant : TOUT l'arbre part -> on regarde tout l'arbre
- hits=$(git ls-tree -r --name-only "$lsha" 2>/dev/null | grep -E "$PRIVE_RE" | head -5)
- else
- hits=$(git diff --name-only "$rsha..$lsha" 2>/dev/null | grep -E "$PRIVE_RE" | head -5)
- fi
- if [ -n "$hits" ]; then
- echo "✗ pre-push refusé : la plage $rref contient du privé :" >&2; echo "$hits" | sed 's/^/ /' >&2; exit 1
- fi
-done
-echo "pre-push : ALLOW_PUSH=1, aucun fichier privé dans la plage - push autorisé" >&2
-exit 0
diff --git a/tests/test_diffdrive_cold.py b/tests/test_diffdrive_cold.py
index 6210443..c8593df 100644
--- a/tests/test_diffdrive_cold.py
+++ b/tests/test_diffdrive_cold.py
@@ -1,7 +1,9 @@
-"""Cold tests: known inputs -> known outputs, in physical units."""
+"""Cold tests: known inputs -> known outputs, in physical units; invalid inputs are refused."""
import math
-from moss_dimos.diffdrive import DiffDriveConfig, tracks_to_twist, twist_to_tracks
+import pytest
+
+from moss_dimos.diffdrive import DiffDriveConfig, clamp_twist, tracks_to_twist, twist_to_tracks
CFG = DiffDriveConfig(track_width_m=0.30, max_track_speed_mps=0.6)
@@ -17,8 +19,7 @@ def test_spin_in_place_opposite_tracks():
def test_roundtrip_73_percent_of_a_turn():
vx, wz = 0.25, 0.73
- left, right = twist_to_tracks(vx, wz, CFG)
- back_vx, back_wz = tracks_to_twist(left, right, CFG)
+ back_vx, back_wz = tracks_to_twist(*twist_to_tracks(vx, wz, CFG), CFG)
assert math.isclose(back_vx, vx) and math.isclose(back_wz, wz)
@@ -28,5 +29,18 @@ def test_over_limit_is_scaled_not_clipped_keeps_ratio():
assert math.isclose(right / left, 1.3 / 0.7)
-def test_strafe_is_ignored():
- assert twist_to_tracks(0.2, 0.0, CFG, vy=0.9) == twist_to_tracks(0.2, 0.0, CFG)
+def test_clamp_twist_full_forward_hits_the_limit_exactly():
+ assert clamp_twist(5.0, 0.0, CFG) == (0.6, 0.0)
+
+
[email protected]("width,speed", [(0.0, 0.6), (-0.3, 0.6), (0.3, 0.0), (0.3, -1.0),
+ (float("nan"), 0.6), (0.3, float("inf"))])
+def test_invalid_config_is_refused(width, speed):
+ with pytest.raises(ValueError):
+ DiffDriveConfig(track_width_m=width, max_track_speed_mps=speed)
+
+
[email protected]("vx,wz", [(float("nan"), 0.0), (0.0, float("nan")), (float("inf"), 0.0), (0.1, float("-inf"))])
+def test_non_finite_command_is_refused_never_a_motor_command(vx, wz):
+ with pytest.raises(ValueError):
+ twist_to_tracks(vx, wz, CFG)
diff --git a/tests/test_gamepad_import_cold.py b/tests/test_gamepad_import_cold.py
index 2b3cce8..12c5b86 100644
--- a/tests/test_gamepad_import_cold.py
+++ b/tests/test_gamepad_import_cold.py
@@ -1,12 +1,14 @@
-"""The derived gamepad module must at least import and expose its pure helpers (no pad, no dimOS run)."""
+"""The dimOS wrapper must import when its optional dependencies are present; only their absence may skip."""
import importlib
import pytest
-def test_gamepad_module_imports_or_skips_without_dimos():
+def test_gamepad_module_imports_or_skips_only_for_optional_deps():
try:
mod = importlib.import_module("moss_dimos.gamepad")
- except ModuleNotFoundError as e: # dimos / pygame absent on a cold box
- pytest.skip(f"optional dependency missing: {e.name}")
- assert hasattr(mod, "axes_to_twist")
+ except ModuleNotFoundError as e:
+ if e.name and (e.name == "dimos" or e.name.startswith("dimos.") or e.name == "pygame"):
+ pytest.skip(f"optional dependency missing: {e.name}")
+ raise
+ assert hasattr(mod, "GamepadTeleop")
diff --git a/tests/test_teleop_logic_cold.py b/tests/test_teleop_logic_cold.py
new file mode 100644
index 0000000..ef04128
--- /dev/null
+++ b/tests/test_teleop_logic_cold.py
@@ -0,0 +1,77 @@
+"""Cold tests of the pad logic: strafe cannot exist, forward reaches the limit, deadman/brake timing."""
+import math
+
+import pytest
+
+from moss_dimos.diffdrive import DiffDriveConfig
+from moss_dimos.teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twist
+
+CFG = TeleopConfig(drive=DiffDriveConfig(track_width_m=0.30, max_track_speed_mps=0.6), max_yaw_rps=2.0, deadzone=0.08, brake_s=0.5)
+
+
+def test_full_forward_gives_max_track_speed_on_both_tracks():
+ vx, wz = axes_to_twist(-1.0, 0.0, CFG) # stick up reads -1
+ assert math.isclose(vx, 0.6) and wz == 0.0
+
+
+def test_the_pad_has_no_lateral_input_at_all():
+ # The API takes two axes only: there is nothing a third axis could change.
+ import inspect
+ params = list(inspect.signature(axes_to_twist).parameters)
+ assert params[:2] == ["left_y", "right_x"] and "left_x" not in params and "vy" not in params
+
+
+def test_deadzone_reads_as_zero_and_rescales_to_the_rim():
+ assert axes_to_twist(-0.05, 0.03, CFG) == (0.0, 0.0)
+ vx, _ = axes_to_twist(-0.54, 0.0, CFG) # (0.54-0.08)/(0.92) = 0.5 of the limit
+ assert math.isclose(vx, 0.3)
+
+
+def test_right_stick_right_turns_right_negative_yaw():
+ _, wz = axes_to_twist(0.0, 1.0, CFG)
+ assert wz < 0 and math.isclose(abs(wz), 2.0)
+
+
+def test_full_forward_plus_full_turn_is_scaled_not_flattened():
+ vx, wz = axes_to_twist(-1.0, -1.0, CFG) # raw 0.6 m/s + 2 rad/s -> outer track 0.9 > 0.6
+ assert math.isclose(max(abs(vx - 0.15 * wz), abs(vx + 0.15 * wz)), 0.6)
+ assert math.isclose(wz / vx, 2.0 / 0.6) # turn radius kept
+
+
+def test_boost_multiplies_but_never_exceeds_the_track_limit():
+ boosted = TeleopConfig(drive=CFG.drive, boost=1.5)
+ vx, _ = axes_to_twist(-1.0, 0.0, boosted, boost_held=True)
+ assert math.isclose(vx, 0.6)
+
+
[email protected]("ly,rx", [(float("nan"), 0.0), (0.0, float("inf"))])
+def test_non_finite_axis_is_refused(ly, rx):
+ with pytest.raises(ValueError):
+ axes_to_twist(ly, rx, CFG)
+
+
[email protected]("kw", [dict(max_yaw_rps=0), dict(deadzone=1.0), dict(boost=0.5), dict(brake_s=-1), dict(rate_hz=0)])
+def test_invalid_teleop_config_is_refused(kw):
+ with pytest.raises(ValueError):
+ TeleopConfig(**kw)
+
+
+def test_deadman_brake_window_then_silence():
+ b = DeadmanBrake(0.5)
+ assert b.tick(True, 10.0) == "command"
+ assert b.tick(False, 10.1) == "brake" # release observed at 10.1: the window runs from here
+ assert b.tick(False, 10.6) == "brake" # inclusive end of the window
+ assert b.tick(False, 10.61) == "silent"
+ assert b.tick(False, 20.0) == "silent"
+
+
+def test_deadman_pressed_again_cancels_the_brake_window():
+ b = DeadmanBrake(1.0)
+ b.tick(True, 0.0); assert b.tick(False, 0.1) == "brake"
+ assert b.tick(True, 0.2) == "command"
+ assert b.tick(False, 0.3) == "brake" # a fresh window starts from the new release
+
+
+def test_a_pad_at_rest_never_publishes():
+ b = DeadmanBrake(0.5)
+ assert all(b.tick(False, t) == "silent" for t in (0.0, 0.1, 5.0))