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.pyindex 4bdcc44..35a66c3 100644--- a/benches/bench_explorer2_cold.py+++ b/benches/bench_explorer2_cold.py@@ -697,6 +697,7 @@ else:ex._last_goal = Noneex._last_goal_ok = Noneex._late_answer_owed = False+ ex._goal_published_t = 0.0ex._tuning = DEFAULT_TUNINGex.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.pyindex 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.pyindex 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.pyindex 2e0b60e..bc2fc2c 100644--- a/moss_dimos/blueprints.py+++ b/moss_dimos/blueprints.py@@ -96,6 +96,7 @@ from dimos.msgs.std_msgs.String import Stringfrom dimos.utils.logging_config import setup_loggerfrom moss_dimos import robot_profile+from dimos.msgs.std_msgs.Bool import Boolfrom moss_dimos.moss_adapter import telemetry_payloadfrom 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 offTELEMETRY_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 = Noneexcept Exception as exc: # noqa: BLE001cause = f"{type(exc).__name__}: {exc}"diff --git a/moss_dimos/explorer2.py b/moss_dimos/explorer2.pyindex c2123d7..bcb8d0c 100644--- a/moss_dimos/explorer2.py+++ b/moss_dimos/explorer2.py@@ -147,6 +147,9 @@ FREE = 0UNKNOWN = -1OCCUPIED = 100OCCUPANCY_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 stackself._state = ExploreState()self._last_goal: tuple[float, float] | None = Noneself._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 stackgoal.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 Noneself._last_goal = (float(goal.position.x), float(goal.position.y))self._last_goal_ok = Nonegap_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.pyindex 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 steeredMOSS_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, countedMOSS_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 tickTELEMETRY_FRESH_S = 1.0 # mode read from telemetry older than this = unknownMOCK_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 comparisonPULSE_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 = 0self._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 pulsesself._map_geometry = replace(self._geometry, min_pct=pulse_min_pct()) if self._pulse_n else self._geometryself._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 = Falseself._want = wantself._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 = Noneself._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)continueif 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