MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
#!/usr/bin/env python3
"""Speed watchdog: tails the run's dimOS log, computes the body's speed from
the lidar odometry lines, and stops the run when the body moves faster than
the profile's ceiling (robot_profile max_speed_mps, 0.45 m/s, overridden by
MOSS_MAX_SPEED_MPS). It must be running whenever the rover explores
unattended: tools/arm_speed_guard.sh arms it for one run.
The ceiling is the planner's cruise plus a margin, never below it: a ceiling
at or under the speed the stack itself commands would stop every normal run.
Cruise = dimOS LocalPlanner._speed (PLANNER_SPEED_MPS, 0.55 m/s) x the
launcher's speed scale (MOSS_NERF_SPEED, 0.7 by default in tools/moss-fly.sh)
= 0.385 m/s. The watchdog refuses to start (exit 1, before its pid file
exists, so the arm script reports it did not come up) when the ceiling is not
above that cruise.
Speed is measured between the TIMESTAMPS THE LOG CARRIES, never between the
moments the lines reach this process: LidarOdometry rate-limits `lidar odom #N`
(log_every_s = 2.0) and the interval jitters around it, so a wall-clock window
(0.02 < dt < 2.0) throws away most samples - 183 of the 238 intervals of a
recorded field log - and can never strike in time. One interval above the
ceiling is a kill; no cadence is assumed anywhere.
The kill is two steps. First the exploration stop through tools/explore_ctl.py,
on the run's own bus. Then the stack itself goes down (SPEED_GUARD_STACK_STOP,
default `tools/moss-fly.sh stop`): the motor board is open loop (PWM percent,
no speed loop), so a body faster than the ceiling is not something a stopped
explorer is sure to end; with the stack down the firmware deadman stops the
tracks 300 ms after the last command.
It guards ONE run: the run dir is frozen at startup (SPEED_GUARD_RUN_DIR, which the
arm script passes, else the newest run dir) and the pid written to PID_FILE
(the launcher kills THAT pid, not whatever `pgrep speed_guard` finds). A
watchdog whose run is over stands down by itself - either a newer run dir
appeared, or the log it tails stopped growing for IDLE_S. Without this, an
orphan of the previous run keeps tailing a dead log while everything that
reads its mere presence as 'armed' lets the live run explore unguarded.
Where dimOS logs a run: an installed dimOS under <state>/dimos/logs/<run>,
a dimOS from a git clone under the clone; the run registry
(<state>/dimos/runs/*.json) names the log_dir in both cases, so both are read.
"""
import atexit, glob, json, math, os, re, shlex, signal, subprocess, sys, threading, time
from datetime import datetime
TOOLS = os.path.dirname(os.path.abspath(__file__))
ROOT = os.path.dirname(TOOLS)
sys.path.insert(0, TOOLS)
sys.path.insert(0, ROOT)
import explore_ctl # noqa: E402 the stop rides the run's bus, same module the launcher uses
from moss_dimos import robot_profile # noqa: E402
def limit_ms(env=None) -> float:
"""The ceiling in m/s: the profile's max_speed_mps (MOSS_MAX_SPEED_MPS overrides it)."""
return robot_profile.current(env).max_speed_mps
PLANNER_SPEED_MPS = 0.55 # dimOS LocalPlanner._speed (replanning_a_star/local_planner.py): the follower's cruise
NERF_SPEED_DEFAULT = 0.7 # tools/moss-fly.sh: NERF_SPEED="${MOSS_NERF_SPEED:-0.7}", passed as --nerf-speed
def cruise_mps(env=None) -> float:
"""The speed the stack commands when the follower is aligned: dimOS's
PLANNER_SPEED_MPS times the run's speed scale (MOSS_NERF_SPEED, the
launcher's default when unset; dimOS applies it only below 1)."""
env = os.environ if env is None else env
try:
nerf = float(str(env.get("MOSS_NERF_SPEED", "")).strip() or NERF_SPEED_DEFAULT)
except ValueError:
nerf = NERF_SPEED_DEFAULT
return PLANNER_SPEED_MPS * (nerf if 0.0 < nerf < 1.0 else 1.0)
def ceiling_refusal(limit: float, cruise: float):
"""Why this ceiling cannot guard a run (it would stop every normal one),
or None when it sits above the commanded cruise."""
if limit > cruise:
return None
return (f"ceiling {limit:g} m/s is not above the planner's cruise {cruise:.3f} m/s "
f"({PLANNER_SPEED_MPS:g} m/s x MOSS_NERF_SPEED): every normal run would be stopped - "
f"raise MOSS_MAX_SPEED_MPS above {cruise:.3f} or lower MOSS_NERF_SPEED; not armed")
LIMIT_MS = limit_ms() # hard envelope, read once per watchdog: beyond = runaway
DIMOS_STATE = os.path.join(os.environ.get("XDG_STATE_HOME") or "~/.local/state", "dimos")
LOG_GLOB = os.path.join(DIMOS_STATE, "logs", "*") # an installed dimOS: one dir per run
RUNS_GLOB = os.path.join(DIMOS_STATE, "runs", "*.json") # the run registry: its log_dir, wherever dimOS logs
LAUNCH_LOG = "/tmp/moss_fly_launch.log" # tools/moss-fly.sh's launch output, tailed too when present
IDLE_S = float(os.environ.get("SPEED_GUARD_IDLE_S", "30")) # log silent this long -> the run is over
POLL_S = float(os.environ.get("SPEED_GUARD_POLL_S", "2")) # how often the stand-down is re-checked
PID_FILE = os.environ.get("SPEED_GUARD_PID_FILE", "/tmp/moss_speed_guard.pid")
STACK_STOP = shlex.split(os.environ.get("SPEED_GUARD_STACK_STOP", "tools/moss-fly.sh stop")) # run from the checkout root
STACK_STOP_TIMEOUT_S = 60.0
ODOM_RE = re.compile(r"lidar odom #(\d+): x=([+-][\d.]+) y=([+-][\d.]+)")
TS_RE = re.compile(r'"timestamp":\s*"([^"]+)"')
def run_dirs(glob_pat: str = LOG_GLOB, runs_glob: str = RUNS_GLOB) -> list:
"""The dimos run dirs, oldest mtime first: every dir under the log root,
plus every log_dir the run registry names (a dimOS from a git clone logs
under the clone, and only its registry entry says where)."""
dirs = {os.path.realpath(d) for d in glob.glob(os.path.expanduser(glob_pat)) if os.path.isdir(d)}
for entry in glob.glob(os.path.expanduser(runs_glob)):
try:
with open(entry) as f:
log_dir = json.load(f).get("log_dir")
except (OSError, ValueError, AttributeError):
continue # half-written or foreign file
if isinstance(log_dir, str) and os.path.isdir(log_dir):
dirs.add(os.path.realpath(log_dir))
return sorted(dirs, key=_mtime)
def _mtime(path: str) -> float:
"""mtime, or -1 for a path that vanished since it was listed."""
try:
return os.path.getmtime(path)
except OSError:
return -1.0
def newest_run(glob_pat: str = LOG_GLOB, runs_glob: str = RUNS_GLOB) -> str:
runs = run_dirs(glob_pat, runs_glob)
if not runs:
sys.exit("no dimos run logs found")
return runs[-1]
def log_of(run_dir: str) -> str:
return os.path.join(run_dir, "main.jsonl")
def write_pid_file(path: str = PID_FILE) -> None:
"""Stamp our pid so the launcher can kill THIS watchdog by pid."""
with open(path, "w") as f:
f.write(f"{os.getpid()}\n")
atexit.register(clear_pid_file, path)
def clear_pid_file(path: str = PID_FILE) -> None:
"""Drop the pid file, but only while it still names us: a newer watchdog
that overwrote it must keep its own."""
try:
if int(open(path).read().strip()) == os.getpid():
os.remove(path)
except (OSError, ValueError):
pass
class RunWatch:
"""Stand-down decision for the ONE run this watchdog guards: a newer run dir
means the live run has its own watchdog and we are its orphan; a log that
stopped growing for idle_s means the run we guard is over (or dead)."""
def __init__(self, run_dir: str, idle_s: float = IDLE_S, glob_pat: str = LOG_GLOB,
clock=time.time, runs_glob: str = RUNS_GLOB):
self.run_dir = os.path.realpath(run_dir)
self.log_path = log_of(run_dir)
self.idle_s = idle_s
self.glob_pat = glob_pat
self.runs_glob = runs_glob
self.clock = clock
self.size = self._size()
self.last_growth = clock()
def _size(self) -> int:
try:
return os.path.getsize(self.log_path)
except OSError:
return -1 # no log to grow: the idle limit will retire us
def reason(self):
"""Why this watchdog must stand down, or None while it still guards the
live run."""
# our own dir always competes: a run dir the listing misses (a registry
# entry already swept) must not make an OLDER dir look like the newest
runs = sorted(set(run_dirs(self.glob_pat, self.runs_glob)) | {self.run_dir}, key=_mtime)
if runs[-1] != self.run_dir:
return (f"{os.path.basename(runs[-1])} is now the newest run, this process only tails "
f"{os.path.basename(self.run_dir)} - the live run is guarded by its own watchdog")
size, now = self._size(), self.clock()
if size != self.size:
self.size, self.last_growth = size, now
return None
idle = now - self.last_growth
if idle >= self.idle_s:
return (f"{self.log_path} has not grown for {idle:.0f} s (limit {self.idle_s:g} s) "
"- the run it guards is over")
return None
def supervise(watch: "RunWatch", tail, poll_s: float = POLL_S) -> None:
"""Kill the tail - and with it the main loop - as soon as this watchdog is
no longer the guard of a live run."""
while tail.poll() is None:
why = watch.reason()
if why:
print(f"[speed guard] standing down: {why}", flush=True)
tail.terminate()
return
time.sleep(poll_s)
def parse_odom(line: str):
"""(t seconds, x metres, y metres) from an odom log line, or None when the
line carries no odom sample or no timestamp to date it with."""
m = ODOM_RE.search(line)
ts = TS_RE.search(line)
if not m or not ts:
return None
try:
t = datetime.fromisoformat(ts.group(1).replace("Z", "+00:00")).timestamp()
except ValueError:
return None
return t, float(m.group(2)), float(m.group(3))
class SpeedGuard:
"""Body speed over consecutive odom lines, dated by the log itself."""
def __init__(self, limit: float = None):
self.limit = LIMIT_MS if limit is None else limit
self.prev = None # (t, x, y) of the last dated odom line
def feed(self, line: str):
"""Speed in m/s over the interval this line closes, or None when the
line closes no interval (not odom, undated, or not after the previous)."""
sample = parse_odom(line)
if sample is None:
return None
t, x, y = sample
prev, self.prev = self.prev, sample
if prev is None or t <= prev[0]:
return None
return math.hypot(x - prev[1], y - prev[2]) / (t - prev[0])
def breach(self, line: str):
"""That speed when it exceeds the envelope, else None."""
v = self.feed(line)
return v if v is not None and v > self.limit else None
def stop_all() -> None:
"""Exploration stop on the run's bus, then the stack down. Neither failure
takes the watchdog down with it."""
try:
print(f"[speed guard] STOP EXPLORE published on {explore_ctl.publish('stop')}", flush=True)
except Exception as e: # a sick bus must not take the watchdog down with it
print(f"[speed guard] STOP EXPLORE FAILED: {e}", flush=True)
try:
r = subprocess.run(STACK_STOP, cwd=ROOT, stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, timeout=STACK_STOP_TIMEOUT_S)
last = (r.stdout or "").strip().splitlines()[-1:] or ["(no output)"]
print(f"[speed guard] STACK STOP ({' '.join(STACK_STOP)}) exit {r.returncode}: {last[0]}", flush=True)
except Exception as e: # a launcher that hangs or is missing must not either
print(f"[speed guard] STACK STOP FAILED ({' '.join(STACK_STOP)}): {e}", flush=True)
def main() -> None:
run_dir = os.environ.get("SPEED_GUARD_RUN_DIR", "").strip() or newest_run() # frozen: this watchdog guards THIS run
if not os.path.isdir(run_dir):
sys.exit(f"SPEED_GUARD_RUN_DIR={run_dir} is not a directory")
cruise = cruise_mps()
why = ceiling_refusal(LIMIT_MS, cruise)
if why:
sys.exit(f"[speed guard] {why}") # before the pid file: the arm script sees no watchdog come up
path = log_of(run_dir)
write_pid_file()
watch = RunWatch(run_dir)
print(f"[speed guard] watching {path} (limit {LIMIT_MS} m/s, cruise {cruise:.3f} m/s commanded, "
f"idle limit {IDLE_S:g} s, pid {os.getpid()} -> {PID_FILE})", flush=True)
guard = SpeedGuard()
paths = [path] + ([LAUNCH_LOG] if os.path.exists(LAUNCH_LOG) else [])
signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) # killed by pid: leave through atexit, pid file and all
with subprocess.Popen(["tail", "-Fn0", *paths], stdout=subprocess.PIPE, text=True) as tail:
threading.Thread(target=supervise, args=(watch, tail), daemon=True).start()
try:
for line in tail.stdout:
if "BUMP" in line or "bump" in line:
print(f"[speed guard] {line.rstrip()}", flush=True)
v = guard.breach(line)
if v is not None:
print(f"[speed guard] {v:.2f} m/s over the {LIMIT_MS} m/s envelope", flush=True)
stop_all()
finally:
tail.terminate() # or the context manager waits forever on a tail nobody killed
print(f"[speed guard] stopped watching {path}", flush=True)
if __name__ == "__main__":
main()