MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
"""Gamepad teleop for MOSS as a dimOS module: pygame.joystick in, Twist out.
dimOS ships keyboard, phone and Quest teleop but no gamepad; this module
fills the gap. It publishes on ``tele_cmd_vel``: wire it through dimOS's
MovementManager so navigation and teleop coexist, or remap it to ``cmd_vel``
for direct drive.
Mapping (standard dual-stick):
left stick Y -> vx (forward) left stick X -> vy (strafe)
right stick X -> wz (rotation) R2 held -> boost
The tracked base ignores vy (moss_adapter); it is still published, so the
same module drives a holonomic base unchanged.
Publishes ONLY while a stick is out of its deadzone - there is no deadman
by default: a small tracked rover at walking pace does not need a held button,
and MOSS_PAD_DEADMAN=r1 (or an SDL button number) brings one back - plus a
brake of zeros right after the sticks return to centre (or the optional
deadman is released) - 0.5 s by default, up to 1.0 s with MOSS_BRAKE_S - then
silence (see BRAKE_S and resolve_brake_s). A pad lying at rest owns nothing,
so the same stack can explore on its own while the pad is plugged in.
Moving a stick again (or pressing the deadman again) ends that brake window
on the spot (BRAKE_CANCEL_ON_DEADMAN), but only on a tick that is trusted and
about to publish - see the worker.
Wiring in this package's blueprints (moss_dimos.blueprints):
moss-gamepad, moss-nav tele_cmd_vel is REMAPPED to cmd_vel: direct
drive, no MovementManager in the path
moss-explore tele_cmd_vel is NOT remapped: the Twist goes
tele_cmd_vel -> MovementManager -> cmd_vel, and
any pad message cancels the current nav goal
Runs headless: the robot's computer has no display, so SDL is pinned to its
dummy video/audio drivers below. The pad is hot-pluggable - the module waits
for one instead of giving up at startup, and goes back to waiting if it is
unplugged, so ``dimos run moss-dimos.moss-gamepad`` can be started before the
receiver is in.
"""
from __future__ import annotations
from dataclasses import dataclass
import os
from collections.abc import Mapping
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
from dimos.core.core import rpc
from dimos.core.module import Module
from dimos.core.stream import Out
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()
# Generic defaults. The MOSS blueprints always pass the base's own scale
# (moss_adapter.teleop_speeds), so these are only the fallback.
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
# SDL axis indices on a standard dual-stick pad (DS4/DS5, Xbox, 8BitDo).
# The pad's LAYOUT depends on how it shows up: the MOSS pad on its 2.4 GHz dongle in Android mode
# (2563:0526) is the default; the same pad on its USB cable enumerates as an Xbox 360 pad (045e:028e,
# kernel xpad), where R1 is button 5, the right stick X is axis 3 and the triggers are axes 2 and 5.
# MOSS_PAD_LAYOUT=android|xinput picks a set; MOSS_PAD_AXIS_RIGHT_X / MOSS_PAD_AXIS_R2 override one
# value each; MOSS_PAD_DEADMAN adds a held button (none by default: r1 = the layout's R1, or an SDL
# button number); MOSS_PAD_INVERT_Y=1 / MOSS_PAD_INVERT_TURN=1 flip a stick whose sign is the other way
# round (a pad that reports stick UP as positive drives backwards on a forward push); MOSS_PAD_INDEX
# picks the SDL joystick when several are plugged (the mute dongle is usually index 0). All read once
# at import.
PAD_LAYOUTS = {
"android": {"r1": 7, "axis_right_x": 2, "axis_r2": 5}, # the shanwan receiver in Android mode (2563:0526)
"xinput": {"r1": 5, "axis_right_x": 3, "axis_r2": 5}, # an Xbox 360 pad to the kernel (xpad): the shanwan pad on its cable
"ds4": {"r1": 5, "axis_right_x": 2, "axis_r2": 4}, # a DualShock 4 to the kernel (hid-generic): the GameSir 2.4G receiver in its DInput mode
}
DEADMAN_OFF = ("", "none", "off", "no") # MOSS_PAD_DEADMAN values that mean: no deadman
JS_SYSFS_GLOB = "/sys/class/input/js*/device/name"
def layout_for_name(name: str | None) -> str:
"""The layout a pad's kernel name calls for: "Sony ... Wireless Controller" / DualShock -> ds4,
"X-Box" / Xbox -> xinput, anything else (the shanwan Android GamePad included) -> android. Pure."""
n = (name or "").lower()
if "sony" in n or "dualshock" in n or "wireless controller" in n:
return "ds4"
if "x-box" in n or "xbox" in n or "x box" in n:
return "xinput"
return "android"
def detect_pad_name() -> str | None:
"""The kernel name of the first /dev/input/js* (sysfs), None without one."""
import glob
for path in sorted(glob.glob(JS_SYSFS_GLOB)):
try:
with open(path) as f:
return f.read().strip()
except OSError:
continue
return None
def _on(raw: str | None) -> bool:
"""An on/off switch as the stack reads them: 1 / true / yes / on."""
return (raw or "").strip().lower() in ("1", "true", "yes", "on")
def pad_layout(env: Mapping[str, str] | None = None, pad_name: str | None = None) -> dict[str, int | None]:
"""The button / axis numbers in force: a named layout, then one-value overrides. Pure when
the layout is named; MOSS_PAD_LAYOUT=auto picks it from the pad's kernel name (pad_name, else
the first js device in sysfs), so the same robot.env serves a pad that shows up as a
DualShock 4 one day and as an Xbox 360 pad the next.
"deadman" is None by default (the sticks drive); MOSS_PAD_DEADMAN=r1 is the layout's R1,
an integer is that SDL button, none / off / empty is no deadman."""
e = os.environ if env is None else env
name = (e.get("MOSS_PAD_LAYOUT") or "android").strip().lower()
if name == "auto":
name = layout_for_name(detect_pad_name() if pad_name is None else pad_name)
base = PAD_LAYOUTS.get(name, PAD_LAYOUTS["android"])
out: dict[str, int | None] = {"deadman": None, "axis_right_x": base["axis_right_x"], "axis_r2": base["axis_r2"], "name": name,
"y_sign": -1 if _on(e.get("MOSS_PAD_INVERT_Y")) else 1,
"turn_sign": -1 if _on(e.get("MOSS_PAD_INVERT_TURN")) else 1}
raw = (e.get("MOSS_PAD_DEADMAN") or "").strip().lower()
if raw == "r1":
out["deadman"] = base["r1"]
elif raw not in DEADMAN_OFF:
try:
out["deadman"] = int(raw)
except ValueError:
pass
for key, var in (("axis_right_x", "MOSS_PAD_AXIS_RIGHT_X"), ("axis_r2", "MOSS_PAD_AXIS_R2")):
raw = (e.get(var) or "").strip()
if raw:
try:
out[key] = int(raw)
except ValueError:
pass
return out
def pad_index(env: Mapping[str, str] | None = None) -> int:
"""MOSS_PAD_INDEX, else 0. Pure."""
raw = ((os.environ if env is None else env).get("MOSS_PAD_INDEX") or "").strip()
try:
return max(0, int(raw)) if raw else 0
except ValueError:
return 0
_LAYOUT = pad_layout()
LAYOUT_NAME = _LAYOUT["name"] # the layout in force (auto resolved at import, from the first js device)
AXIS_LEFT_X = 0
AXIS_LEFT_Y = 1
AXIS_RIGHT_X = _LAYOUT["axis_right_x"] # android: measured on the 2563:0526 pad, right-stick X sweeps axis 2 over its full travel (axis 3 is its vertical, which leaks a little)
AXIS_R2 = _LAYOUT["axis_r2"]
Y_SIGN = _LAYOUT["y_sign"] # -1 with MOSS_PAD_INVERT_Y: the pad reports stick up as positive
TURN_SIGN = _LAYOUT["turn_sign"] # -1 with MOSS_PAD_INVERT_TURN: the pad reports stick right as negative
@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
envelope: Any = None # TwistEnvelope (defined below); None = DEFAULT_ENVELOPE
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. A userspace
# pad driver that has just started can report uninitialised axes at full
# deflection; without a last gate those go straight onto tele_cmd_vel (a
# priority channel). 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 0.45 m/s in ~0.75 s instead of one step, so
# the base never sees a step command from the pad
CLAMP_ANGULAR_RADS = 0.8
# The rim envelope: translation and rotation ADD at the wheels (rim speed =
# |vx| + |vy| + lever * |wz|), so each axis under its own clamp can still
# drive the wheels near double. The fallback below is conservative: a long
# lever and the rim held to the fastest single-stick feel (0.45 m/s). MOSS
# passes its own envelope, sized to the tracks (moss_adapter.teleop_envelope).
DEFAULT_LEVER_M = 0.50 # m of rim speed per rad/s of rotation
DEFAULT_RIM_MS = 0.45 # max rim speed however the sticks are mixed
DEADMAN_BUTTON = _LAYOUT["deadman"] # None = no deadman, the sticks drive (default). MOSS_PAD_DEADMAN=r1: SDL button 7 on the 2563:0526 pad in Android mode, 5 on the cable (xinput). 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 would
# therefore stop exploration for good.
# 0.5 s < 1.0 s: the brake is seen, then autonomy gets
# the bus back and the two coexist.
# When the pad's radio dies mid-drive (sleep, battery), this module with this
# pad has been seen to stop the base cleanly, so there is no radio watchdog
# here. Revisit only if a radio loss ever leaves a non-zero command running.
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.
#
# At the default BRAKE_S = 0.5 s and 50 Hz, with the deadman re-pressed 0.200 s
# after the release, the module publishes the stick command from the first
# tick after the press either way (+0.213 s -> 0.012 m/s, one slew step) and no
# zero after it: the cancel does not change the default behaviour, and the lag
# felt on a quick re-press is the slew restarting from rest (0.6 m/s2 = 0.75 s
# to the ceiling). What the cancel 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.
# False = the window always runs to its end.
# The length of that window is a knob. 0.5 s is the default (see BRAKE_S);
# raising it towards 1.0 s keeps the base being TOLD to stop for longer
# instead of falling silent and coasting. dimOS builds this module from a
# blueprint, so the environment of the `dimos run` process is the operator's
# lever (workers inherit it):
# MOSS_BRAKE_S=1.0 dimos run moss-dimos.moss-gamepad
BRAKE_S_ENV = "MOSS_BRAKE_S"
BRAKE_S_MAX = 1.0
# Why the ceiling is 1.0 and not more - the cost, in seconds.
# 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). So on
# moss-explore every release mutes autonomy for brake_s + 1.0 s: 1.5 s at the
# default, 2.0 s at this ceiling. The zeros always stop at brake_s, whatever
# its value; the ceiling only bounds that muted time.
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 > MOSS_BRAKE_S >
BRAKE_S (0.5 s). 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 the default window, 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 (each release mutes dimOS autonomy for the "
"window + 1.0 s; 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.
"""
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))
@dataclass(frozen=True)
class TwistEnvelope:
"""The ceilings of clamp_twist. The defaults are the conservative
fallback above. MOSS passes its own (moss_adapter.teleop_envelope): a
tracked base's rim is |vx| + |wz| * track/2."""
linear: float = CLAMP_LINEAR_MS
angular: float = CLAMP_ANGULAR_RADS
lever: float = DEFAULT_LEVER_M
rim: float = DEFAULT_RIM_MS
DEFAULT_ENVELOPE = TwistEnvelope()
def clamp_twist(vx: float, vy: float, wz: float,
env: TwistEnvelope = DEFAULT_ENVELOPE) -> tuple[float, float, float]:
"""The last gate before the bus - pure, cold-testable. Per-axis clamps,
then the rim 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 = env.linear
vx = max(-lim, min(lim, vx))
vy = max(-lim, min(lim, vy))
wz = max(-env.angular, min(env.angular, wz))
rim = abs(vx) + abs(vy) + env.lever * abs(wz)
if rim > env.rim:
k = env.rim / rim
vx, vy, wz = vx * k, vy * k, wz * k
return (vx, vy, wz)
def axes_neutral(ax0: float, ax1: float, ax2: 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(ax2) < deadzone
def sticks_active(ax0: float, ax1: float, ax2: float, deadzone: float) -> bool:
"""True when a drive axis is out of its deadzone. Without a deadman this IS
the "command present" condition: the exact complement of axes_neutral, so
an inactive tick is a tick whose twist would have been zero anyway."""
return not axes_neutral(ax0, ax1, ax2, deadzone)
def axes_to_twist(ax0: float, ax1: float, ax2: 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.
Each argument is named after its SDL axis index (AXIS_LEFT_X = 0,
AXIS_LEFT_Y = 1, AXIS_RIGHT_X = 2, AXIS_R2 = 5).
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)
ax2 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(ax2, cfg.deadzone) * cfg.angular_speed
return clamp_twist(vx, vy, wz, cfg.envelope or DEFAULT_ENVELOPE)
class GamepadTeleop(Module):
"""pygame.joystick teleop. Outputs Twist on tele_cmd_vel."""
dedicated_worker = True
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 | None = None, brake_s: float | None = None,
envelope: Any = None,
**kwargs: Any) -> None:
super().__init__(**kwargs)
# envelope: a TwistEnvelope, or its dict (blueprints carry plain
# values), or None = DEFAULT_ENVELOPE. MOSS passes moss_adapter.teleop_envelope().
if isinstance(envelope, dict):
envelope = TwistEnvelope(**envelope)
self.envelope = envelope or DEFAULT_ENVELOPE
self.cfg = TeleopConfig(linear_speed=linear_speed,
angular_speed=angular_speed,
deadzone=deadzone,
boost_multiplier=boost_multiplier,
envelope=self.envelope)
self.rate_hz = rate_hz
self.joystick_index = pad_index() if joystick_index is None else joystick_index
# None -> MOSS_BRAKE_S, else BRAKE_S (0.5 s). Resolved HERE, at
# construction, so the value is fixed and logged once per run - not
# re-read per tick.
self.brake_s = resolve_brake_s(brake_s)
self._stop_event = 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="moss-gamepad", daemon=True)
self._thread.start()
@rpc
def stop(self) -> None:
self._stop_event.set()
if self._thread is not None:
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
# 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 pygame)")
return
pygame.init() # dummy video/audio drivers: works with no display
pygame.joystick.init()
period = 1.0 / self.rate_hz
pad = None
trusted = False # neutral-first trust gate
prev_vx = prev_vy = 0.0 # slew-limiter state
brake_until = 0.0 # publish zeros until this instant, then shut up
prev_active = False # to see the RISING edge of a command (stick out, or deadman pressed)
waiting_logged = False
logger.info("gamepad layout: %s (right stick X = axis %d, R2 = axis %d)", LAYOUT_NAME, AXIS_RIGHT_X, AXIS_R2)
if DEADMAN_BUTTON is None:
logger.info("gamepad: no deadman - a stick out of its deadzone drives, centred sticks stop "
"(MOSS_PAD_DEADMAN=r1 brings the held button back)")
else:
logger.info("gamepad: deadman = SDL button %d, held to drive", DEADMAN_BUTTON)
logger.info("gamepad brake window: %.2f s of zeros after the sticks return to centre%s, "
"then silence (cancel-on-press: %s)",
self.brake_s,
"" if DEADMAN_BUTTON is None else " or the deadman is released",
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), Y_SIGN * _axis(pad, AXIS_LEFT_Y),
TURN_SIGN * _axis(pad, AXIS_RIGHT_X), _axis(pad, AXIS_R2))
if DEADMAN_BUTTON is None:
deadman = True # no held button required
else:
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_active = False # a pad that is gone holds nothing
# ... and it holds no RAMP either. The slew state must die
# with the pad: otherwise a pad that vanished for one read
# while driving at the ceiling comes back with 0.45 m/s of
# slew state, and the first trusted tick, STICKS AT
# NEUTRAL, publishes +0.438 m/s - the rover leaves on its
# own for ~0.75 s. 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: 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 (%s)",
"move a stick to drive" if DEADMAN_BUTTON is None
else "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: 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.
#
# ORDER MATTERS: these two lines sit BELOW the trust gate. On
# a pad that has not yet earned trust - after EVERY
# reconnection, and after every one-tick read glitch, which is
# exactly when the rover may be rolling with nobody commanding
# it - a deadman press above the gate would cancel the brake
# window while the untrusted path can publish NOTHING to
# replace it: a single missed read at 0.45 m/s would cut 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.
# A COMMAND IS PRESENT when the optional deadman is held (always, when there
# is none) AND, without a deadman, a stick is out of its deadzone: a pad lying
# at rest publishes nothing either way.
active = deadman and (DEADMAN_BUTTON is not None
or sticks_active(axes[0], axes[1], axes[2], self.cfg.deadzone))
brake_until = brake_window_after_press(brake_until, active,
prev_active)
prev_active = active
# NO COMMAND (sticks centred, or the optional deadman released): no motion - ever.
if not active:
# 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.
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.envelope)
self._publish(vx, vy, wz)
brake_until = time.monotonic() + self.brake_s
time.sleep(period)
finally:
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))