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

base: stall guard (the encoders are the bumper: a commanded track whose encoder did not move for 1 s -> stop, counted, logged); coordinator publishes bump on a stall for the planner's escape; explorer: a late verdict is only swallowed within 1 s of the goal publish (a 40 s pause measured on the floor otherwise)

metrox24a4ff65455e
diff --git a/benches/bench_explorer2_cold.py b/benches/bench_explorer2_cold.py
index 4bdcc44..35a66c3 100644
--- a/benches/bench_explorer2_cold.py
+++ b/benches/bench_explorer2_cold.py
@@ -697,6 +697,7 @@ else:
ex._last_goal = None
ex._last_goal_ok = None
ex._late_answer_owed = False
+ ex._goal_published_t = 0.0
ex._tuning = DEFAULT_TUNING
ex.goal_reached_event = threading.Event()
ex.stop_event = threading.Event()
@@ -731,6 +732,22 @@ else:
ex._state.failed == [] and abs(float(ex.latest_odometry.position.x)
- (START[0] + 2 * STEP_M)) < 1e-6)
+ # The verdict owed to goal 1 is only swallowed inside LATE_VERDICT_S of goal 2's
+ # publish. A verdict landing after that window is goal 2's own (its "no path"
+ # after a replanning): credited, and the loop wakes at once instead of
+ # sleeping the whole goal_timeout (a 45 s pause on the floor otherwise).
+ _explorer2_module.LATE_VERDICT_S = 0.05
+ def gave_up_later(ex):
+ threading.Timer(0.15, lambda: gave_up(ex)).start()
+ ex, planner, ended = run_loop({1: drove, 2: acts(gave_up_later, last)})
+ _explorer2_module.LATE_VERDICT_S = 1.0
+ check("a verdict past the late window is credited to the goal in flight",
+ [(round(f[0], 2), round(f[1], 2)) for f in ex._state.failed] == [STUB_GOAL],
+ f"{ex._state.failed}")
+ check("and wakes the loop at once (0.15 s, not the 0.4 s timeout)",
+ ended - planner.published_at[1] < GOAL_TIMEOUT_S - 0.05,
+ f"{ended - planner.published_at[1]:.2f} s")
+
# Nothing is owed on the first goal of a run: a no-path verdict on it is the
# planner talking about THAT goal, and it must still bite at once.
ex, planner, ended = run_loop({1: acts(gave_up, last)})
diff --git a/benches/bench_moss_adapter_cold.py b/benches/bench_moss_adapter_cold.py
index c4877f8..6bb75c5 100644
--- a/benches/bench_moss_adapter_cold.py
+++ b/benches/bench_moss_adapter_cold.py
@@ -483,6 +483,45 @@ pb.disconnect()
os.environ.pop("MOSS_PULSE_MS", None)
os.environ.pop("MOSS_PULSE_MIN_PCT", None)
+
+# ── 14. the stall guard: the encoders are the bumper ───────────────────
+print("14. stall guard: a track commanded for STALL_S whose encoder did not move -> stop, counted (the encoders are the bumper)")
+from moss_dimos.moss_adapter import STALL_S, STALL_MIN_TICKS, telemetry_payload as _tp # noqa: E402
+os.environ["MOSS_PULSE_MS"] = "0"
+fw9 = FakeMossFirmware(auto_time=True) # the stock fake never turns its encoders: every drive is a push against a wall
+f9 = lambda: fw9 # noqa: E731
+f9.is_mock = True
+sa = MossBaseAdapter(dof=3, port_factory=f9, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=100, min_pct=0))
+check(sa.connect() and sa.write_enable(True), "connected and armed on a fake with frozen encoders")
+t_start = time.monotonic()
+while time.monotonic() - t_start < STALL_S + 0.6 and not sa.stalled:
+ sa.write_velocities([0.06, 0.0, 0.0]) # 40 %, refreshed like a planner would
+ settle(0.1)
+check(sa.stalled and sa.stall_count == 1, f"stalled once after about {STALL_S:.0f} s of a held command: stalled {sa.stalled}, count {sa.stall_count}")
+check(fw9.commands[-1] == "stop" and fw9.mode == OFF, f"'stop' was sent and the fake is OFF: last {fw9.commands[-1]!r}, mode {fw9.mode}")
+check(sa.target_pct == (0, 0), "the command is dropped (no keepalive pushes on)")
+doc = _tp(sa, 0.0)
+check(doc is not None and doc.get("stalled") is True and doc.get("stall_count") == 1, f"telemetry_payload carries stalled/stall_count: {None if doc is None else (doc.get('stalled'), doc.get('stall_count'))}")
+check(sa.write_velocities([-0.06, 0.0, 0.0]) and sa.stalled is False, "the next accepted command (the escape, in reverse) clears the flag")
+sa.write_stop(); sa.disconnect()
+# a fake whose encoders turn never stalls, however long the command is held
+class Turning(FakeMossFirmware):
+ def _telemetry_line(self):
+ if self.mode == DRIVE:
+ self.ticks[0] += 80; self.ticks[1] += 80
+ return super()._telemetry_line()
+fw10 = Turning(auto_time=True)
+f10 = lambda: fw10 # noqa: E731
+f10.is_mock = True
+sb = MossBaseAdapter(dof=3, port_factory=f10, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=100, min_pct=0))
+check(sb.connect() and sb.write_enable(True), "a fake whose encoders turn")
+t_start = time.monotonic()
+while time.monotonic() - t_start < STALL_S + 0.6:
+ sb.write_velocities([0.06, 0.0, 0.0]); settle(0.1)
+check(not sb.stalled and sb.stall_count == 0 and fw10.mode == DRIVE, f"no stall while the encoders move: count {sb.stall_count}, mode {fw10.mode}")
+sb.write_stop(); sb.disconnect()
+os.environ.pop("MOSS_PULSE_MS", None)
+
print(f"{OK} OK, {KO} KO")
print("TEST PASSED" if KO == 0 else "TEST FAILED")
sys.exit(1 if KO else 0)
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index 7c81558..33d9355 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -259,12 +259,13 @@ v3_line = {"ms": 1234, "fw": "qtpy-mdd3a-v3", "mode": 2, "reason": "drive",
doc = telemetry_payload(FakeAdapter(v3_line, 0.3), 1760000000.0)
expected = {"ts": 1760000000.0, "fw": "qtpy-mdd3a-v3", "driver": None, "mode": 2, "reason": "drive",
"bus_mV": 11208, "ina_ok": True, "ticks": [160585, 158263], "target_pct": [40, -40],
- "output_pct": [35, -35], "ramp_ms": [5, 5], "firmware_age_s": 0.3, "mock": False}
+ "output_pct": [35, -35], "ramp_ms": [5, 5], "firmware_age_s": 0.3, "mock": False,
+ "stalled": False, "stall_count": 0}
check("v3 line, age 0.3 s, now 1760000000 -> the known document", doc == expected,
"" if doc == expected else f"{doc}")
check("exactly the cockpit's keys, in order", list(doc) == ["ts", "fw", "driver", "mode", "reason", "bus_mV", "ina_ok",
"ticks", "target_pct", "output_pct", "ramp_ms",
- "firmware_age_s", "mock"])
+ "firmware_age_s", "mock", "stalled", "stall_count"])
mdd10a = dict(v3_line, driver="mdd10a")
check("an MDD10A build's driver key is carried", telemetry_payload(FakeAdapter(mdd10a, 0.0), 0.0)["driver"] == "mdd10a")
check("no telemetry yet -> None (nothing published)", telemetry_payload(FakeAdapter(None, None), 0.0) is None)
@@ -544,8 +545,8 @@ os.environ.pop("MOSS_STATE_DIR")
ins = set(MossRecorder.__annotations__) | set(MossRecorder.__mro__[1].__annotations__)
check("MossRecorder records odom, global_costmap, lidar, color_image, wrist_image, cmd_vel, base_telemetry, episode_marker",
{"odom", "global_costmap", "lidar", "color_image", "wrist_image", "cmd_vel", "base_telemetry", "episode_marker"} <= ins)
-check("MossControlCoordinator has the base_telemetry Out, and no other stream of its own",
- set(MossControlCoordinator.__annotations__) == {"base_telemetry"}, f"{sorted(MossControlCoordinator.__annotations__)}")
+check("MossControlCoordinator has the base_telemetry and bump Outs, and no other stream of its own",
+ set(MossControlCoordinator.__annotations__) == {"base_telemetry", "bump"}, f"{sorted(MossControlCoordinator.__annotations__)}")
check("its start/stop overrides keep @rpc (else dimOS pickles the module to call them)",
{"start", "stop"} <= set(MossControlCoordinator.rpcs))
check("it runs in its own worker (dedicated_worker)", MossControlCoordinator.dedicated_worker is True)
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index 2e0b60e..bc2fc2c 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -96,6 +96,7 @@ from dimos.msgs.std_msgs.String import String
from dimos.utils.logging_config import setup_logger
from moss_dimos import robot_profile
+from dimos.msgs.std_msgs.Bool import Bool
from moss_dimos.moss_adapter import telemetry_payload
from moss_dimos.robot_profile import (MOSS_LIDAR_MISSING, MOSS_QTPY_MISSING,
RobotProfile)
@@ -152,6 +153,7 @@ class MossControlCoordinator(ControlCoordinator):
"""
base_telemetry: Out[String]
+ bump: Out[Bool] # a stall of the base (moss_adapter stall guard) = a contact: the planner's escape backs off
TELEMETRY_PERIOD_S = 0.5
@@ -187,6 +189,12 @@ class MossControlCoordinator(ControlCoordinator):
payload = telemetry_payload(getattr(hw, "adapter", None), now)
if payload is not None:
self.base_telemetry.publish(String(data=json.dumps(payload)))
+ stalls = int(payload.get("stall_count") or 0)
+ if stalls > getattr(self, "_stalls_seen", 0):
+ self._stalls_seen = stalls
+ _logger.warning(f"base stall #{stalls}: publishing bump (contact) for the planner's escape")
+ if getattr(self.bump, "transport", None) is not None:
+ self.bump.publish(Bool(data=True))
last_error = None
except Exception as exc: # noqa: BLE001
cause = f"{type(exc).__name__}: {exc}"
diff --git a/moss_dimos/explorer2.py b/moss_dimos/explorer2.py
index c2123d7..bcb8d0c 100644
--- a/moss_dimos/explorer2.py
+++ b/moss_dimos/explorer2.py
@@ -147,6 +147,9 @@ FREE = 0
UNKNOWN = -1
OCCUPIED = 100
OCCUPANCY_THRESHOLD = 99 # dimOS WavefrontConfig.occupancy_threshold; costmap2d
+LATE_VERDICT_S = 1.0 # a goal_reached this soon after a goal publish, when one is owed to the goal that
+ # timed out unanswered, is that old goal's cancel (the planner's monitor cancels it on
+ # its next tick, 10 Hz); anything later is the goal in flight's own verdict
# publishes 100 for obstacles
_EIGHT = np.ones((3, 3), dtype=bool)
@@ -1171,6 +1174,7 @@ if HAVE_DIMOS: # pragma: no cover - needs the dimOS stack
self._state = ExploreState()
self._last_goal: tuple[float, float] | None = None
self._last_goal_ok: bool | None = None
+ self._goal_published_t = 0.0 # monotonic time of the last goal publish (the late-verdict window starts there)
# True while a goal that timed out unanswered can still be answered.
# goal_reached is a bare Bool with no goal identity in it, and the
# planner's monitor thread (10 Hz) can cancel the OLD goal at any
@@ -1199,14 +1203,18 @@ if HAVE_DIMOS: # pragma: no cover - needs the dimOS stack
# module ignores it and sleeps its whole goal_timeout for nothing.
def _on_goal_reached(self, msg: Bool) -> None:
if self._late_answer_owed:
- # The previous goal's verdict, landing after the current goal was
- # published. Crediting it would exclude a frontier the rover has
- # never driven to. At most one is owed: the planner cancels the
- # old goal once, then adopts the new one.
self._late_answer_owed = False
- logger.info("late goal_reached: the previous goal's verdict, not this one's - "
- "the goal in flight keeps its own timeout")
- return
+ if time.monotonic() - self._goal_published_t <= LATE_VERDICT_S:
+ # The previous goal's verdict, landing right after the current
+ # goal was published (the planner's monitor cancels the old goal
+ # on its next tick). Crediting it would exclude a frontier the
+ # rover has never driven to. At most one is owed.
+ logger.info("late goal_reached: the previous goal's verdict, not this one's - "
+ "the goal in flight keeps its own timeout")
+ return
+ # Past the window the planner has long adopted this goal: a verdict
+ # now is about THIS goal (its own "no path", a replanning that found
+ # nothing) and must wake the loop at once, not after goal_timeout.
self._last_goal_ok = bool(getattr(msg, "data", False))
self.goal_reached_event.set()
@@ -1268,16 +1276,15 @@ if HAVE_DIMOS: # pragma: no cover - needs the dimOS stack
goal.frame_id = "world"
goal.ts = costmap.ts
# The goal being replaced timed out unanswered: the planner still
- # owes it a verdict, so the first goal_reached after this publish
- # belongs to it, not to the goal below. Swallow that one. The
- # cost when it was in fact this goal's own "no path" is one
- # goal_timeout instead of an instant wake - and the timeout
- # branch then decides on the gap actually closed, in metres.
+ # owes it a verdict, so a goal_reached landing within LATE_VERDICT_S
+ # of this publish belongs to it, not to the goal below (swallowed).
+ # Any verdict later than that is this goal's own and wakes the loop.
self._late_answer_owed = self._last_goal is not None and self._last_goal_ok is None
self._last_goal = (float(goal.position.x), float(goal.position.y))
self._last_goal_ok = None
gap_at_issue = self._gap_to(self._last_goal)
self.goal_reached_event.clear()
+ self._goal_published_t = time.monotonic()
self.goal_request.publish(goal)
logger.info(
f"goal {state.targets_issued}: ({goal.position.x:.2f}, {goal.position.y:.2f}) "
diff --git a/moss_dimos/moss_adapter.py b/moss_dimos/moss_adapter.py
index 3c58d52..cf29eb9 100644
--- a/moss_dimos/moss_adapter.py
+++ b/moss_dimos/moss_adapter.py
@@ -68,6 +68,7 @@ Calibration (env, all optional):
full-power slices over the period (pulse_tracks), the way MOSS's tracks can be steered
MOSS_PULSE_MIN_PCT duty floor of a non-zero track while pulsing (default 40)
MOSS_PIVOT_PCT every pivot (tracks in opposite directions) is sent at this percent, both tracks (default 100, 0 = as asked)
+ (no variable) stall guard: a commanded track whose encoder moved < STALL_MIN_TICKS over STALL_S -> 'stop', logged, counted
MOSS_MOCK_BUS any value but 0/false/no/off: drive the fake firmware (no motors)
"""
from __future__ import annotations
@@ -102,6 +103,10 @@ REPLY_TIMEOUT_S = 0.150 # the firmware answers within a loop tick (ms)
CONNECT_PROBE_S = 1.000 # the first "status" of a process: reader thread start + first parse on a Jetson take longer than a reply tick
TELEMETRY_FRESH_S = 1.0 # mode read from telemetry older than this = unknown
MOCK_BUS_ENV = "MOSS_MOCK_BUS"
+STALL_S = 1.0 # a track commanded for this long whose encoder did not move is STALLED: the
+ # rover is pushing against something (no bumper on MOSS: the encoders are the bumper)
+STALL_MIN_TICKS = 20 # ticks over STALL_S that count as motion (a track at 40 % makes ~800 per second)
+STALL_WINDOW_S = 2.0 # how long the tick history is kept for the comparison
PULSE_MS_DEFAULT = 0 # MOSS_PULSE_MS: period of the full-duty time slicing, 0 = off (then MOSS_MIN_PCT applies).
# Off on MOSS: with the drive wheels seated on their axles the tracks turn continuously
# from 40 % (floor probe), so slices are only for a base that moves at full duty alone
@@ -329,6 +334,10 @@ class MossBaseAdapter:
self._ramp = "" # the ramp sent at connect ("" = the firmware default)
self._tick = 0
self._pivot_pct = pivot_pct() # pivots sent at this percent on both tracks (0 = as asked)
+ self._tick_hist: list[tuple[float, int, int]] = [] # (monotonic, ticks L, ticks R) from telemetry, STALL_WINDOW_S deep
+ self._drive_since: float | None = None # when the current non-zero command started
+ self._stalled = False # True from a stall until the next accepted command
+ self._stall_count = 0 # stalls since connect (telemetry_payload, the coordinator's bump)
# the mapping geometry: when pulsing, the static-friction floor becomes the duty floor of the pulses
self._map_geometry = replace(self._geometry, min_pct=pulse_min_pct()) if self._pulse_n else self._geometry
self._last_logged = None
@@ -570,6 +579,11 @@ class MossBaseAdapter:
left, right = self._slice(want)
ok = self._drive(left, right)
if ok:
+ if want == (0, 0):
+ self._drive_since = None
+ elif self._want == (0, 0) or self._drive_since is None:
+ self._drive_since = time.monotonic()
+ self._stalled = False
self._want = want
self._target = (left, right)
self._target_twist = (vx, 0.0, wz)
@@ -611,6 +625,7 @@ class MossBaseAdapter:
if ok:
self._target = (0, 0)
self._want = (0, 0)
+ self._drive_since = None
self._target_twist = (0.0, 0.0, 0.0)
self._target_t = time.monotonic()
self._stopped_for_stale = True
@@ -640,6 +655,8 @@ class MossBaseAdapter:
self._velocity = (0.0, 0.0, 0.0)
continue
if self._want != (0, 0):
+ if self._stall_check():
+ continue
# pulsing: the slice of this tick, 'drive 0 0' included (an OFF slice
# keeps the firmware armed and its 300 ms deadman fed)
self._target = self._slice(self._want)
@@ -653,6 +670,62 @@ class MossBaseAdapter:
_log().info(f"MOSS telemetry: out={tel.get('output_pct')} target={tel.get('target_pct')} "
f"bus={tel.get('bus_mV')} mV ticks={tel.get('ticks')} mode={tel.get('mode')}")
+ def _stall_check(self) -> bool:
+ """Called under the lock, every keepalive tick while a non-zero command
+ is held. Samples the encoder ticks from telemetry; once the command has
+ been held for STALL_S, a commanded track whose ticks moved less than
+ STALL_MIN_TICKS over the last STALL_S is a stall: 'stop' is sent, the
+ command dropped, the stall counted and logged. Returns True on a stall
+ (the caller then sends nothing more this tick). The next accepted
+ command clears the flag - the planner's escape drives away."""
+ tel = self._telemetry
+ now = time.monotonic()
+ if tel and self._telemetry_t is not None and now - self._telemetry_t < TELEMETRY_FRESH_S:
+ try:
+ ticks = (int(tel["ticks"][0]), int(tel["ticks"][1]))
+ except (KeyError, TypeError, ValueError, IndexError):
+ ticks = None
+ if ticks is not None and (not self._tick_hist or self._tick_hist[-1][0] < self._telemetry_t):
+ self._tick_hist.append((self._telemetry_t, ticks[0], ticks[1]))
+ while self._tick_hist and now - self._tick_hist[0][0] > STALL_WINDOW_S:
+ self._tick_hist.pop(0)
+ if self._drive_since is None or now - self._drive_since < STALL_S or len(self._tick_hist) < 2:
+ return False
+ old = [h for h in self._tick_hist if now - h[0] >= STALL_S]
+ if not old:
+ return False
+ t_old, l_old, r_old = old[-1]
+ t_new, l_new, r_new = self._tick_hist[-1]
+ if t_new - t_old < STALL_S * 0.8:
+ return False
+ want_l, want_r = self._want
+ stalled_l = want_l != 0 and abs(l_new - l_old) < STALL_MIN_TICKS
+ stalled_r = want_r != 0 and abs(r_new - r_old) < STALL_MIN_TICKS
+ if not (stalled_l or stalled_r):
+ return False
+ self._send("stop")
+ self._stall_count += 1
+ self._stalled = True
+ self._want = (0, 0)
+ self._target = (0, 0)
+ self._drive_since = None
+ self._stopped_for_stale = True
+ if not self._geometry.ticks_per_m:
+ self._velocity = (0.0, 0.0, 0.0)
+ _log().warning(f"MOSS base: STALL #{self._stall_count}: tracks commanded L={want_l:+d}% R={want_r:+d}% "
+ f"but the encoders moved L={l_new - l_old:+d} R={r_new - r_old:+d} ticks in "
+ f"{t_new - t_old:.1f} s - stopped (the encoders are the bumper)")
+ return True
+
+ @property
+ def stalled(self) -> bool:
+ """True from a stall until the next accepted command."""
+ return self._stalled
+
+ @property
+ def stall_count(self) -> int:
+ return self._stall_count
+
def _log_cmd(self, twist, pct) -> None:
if pct == self._last_logged:
return
@@ -727,4 +800,6 @@ def telemetry_payload(adapter, now: float) -> dict | None:
doc[key] = tel.get(key)
doc["firmware_age_s"] = None if age is None else round(float(age), 3)
doc["mock"] = bool(getattr(adapter, "mock_bus", False))
+ doc["stalled"] = bool(getattr(adapter, "stalled", False))
+ doc["stall_count"] = int(getattr(adapter, "stall_count", 0) or 0)
return doc