MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
"""The litter mission: explore, see a piece of litter, go to it, put it at
the arm's pick point, run the pick-and-drop cycle, explore again.
`MissionLogic` is the whole decision, pure: it takes what the robot knows
(the confirmed objects in the world frame from `moss_dimos.litter_detect`,
the odometry pose, the planner's verdict) and returns commands; the bench
drives it with a fake robot and a fake clock. `LitterMission` is the thin
dimOS module around it: streams in, streams out, a 10 Hz tick, the arm's
cycle as a subprocess (tools/moss_arm_play.py over RPC to the arm's
coordinator).
States and what they publish:
EXPLORING nothing; the explorer drives. A confirmed object whose class
is litter, not yet picked or skipped, becomes the target.
STOPPING stop_explore_cmd once; wait STOP_SETTLE_S so the explorer's
own "stay here" goal lands first (the cockpit's rule).
APPROACHING goal_request once: the point STANDOFF_M before the object on
the line from the robot, facing it (or straight to SERVOING
when the robot is already that close). goal_reached True ->
SERVOING; False -> the object is skipped; a timeout -> SERVOING
from wherever the rover stopped.
SERVOING tele_cmd_vel at every tick (the pad's channel: the movement
manager lets it win and cancels any navigation): turn until
the robot's pick point reaches the object (one law for the
turn and the creep, the pick point as the controlled point),
then a zero twist. The base has
a power floor (a non-zero track is raised to about half
power) and pivots at full power, so a slow twist does not
exist: near the object (SERVO_FAR_M) and while turning, the
commands come as pulses, one tick of twist then SERVO_REST_S
of zeros, and the rover moves a step at a time. Far from it
the twist is continuous. A timeout skips the object.
PICKING the pick command runs (blocking, in its own thread); done ->
the object is marked picked; a failure skips it.
RESUMING explore_cmd once, then EXPLORING.
The pick point is where the cycle's `pick` pose puts the tool, in
base_link (the lidar's floor projection): PICK_XY_M, from the CAD scene by
default (the tool point less the scanner's axis) and set from a tape
measure on the robot with MOSS_PICK_XY_M. The whole final approach depends
on it, and the camera's mount must share the origin (MOSS_CAMERA_XYZ_M).
"""
from __future__ import annotations
import math
import os
import shlex
import subprocess
import sys
import threading
import time
from dataclasses import dataclass, field
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
PICK_XY_ENV = "MOSS_PICK_XY_M"
PICK_XY_DEFAULT = (0.215, -0.094) # the cycle's pick tool point (0.345, -0.010) less the scanner's axis (0.130, 0.084), CAD scene; to measure
STANDOFF_ENV = "MOSS_STANDOFF_M"
STANDOFF_DEFAULT = 0.6
TOL_ENV = "MOSS_PICK_TOL_M"
TOL_DEFAULT = 0.02
PICK_CMD_ENV = "MOSS_PICK_CMD" # the command that runs the cycle; "none" = mark picked without moving the arm
PICK_SIM_ENV = "MOSS_PICK_SIM" # 1: the cycle on the simulated arm (--sim)
AUTOSTART_ENV = "MOSS_MISSION_AUTOSTART" # 0: start in EXPLORING without sending explore_cmd (the cockpit started it)
STOP_SETTLE_S = 3.0
APPROACH_TIMEOUT_S = 90.0
SERVO_TIMEOUT_S = 60.0
PICK_TIMEOUT_S = 180.0
RESUME_SETTLE_S = 1.0
SKIP_FOR_S = 600.0 # a skipped object is tried again after this
NEAR_DONE_M = 0.15 # an object this close to a picked one is the same object
MAX_TARGET_RANGE_M = 4.0
SERVO_V_MAX = 0.08 # m/s (the base raises any non-zero command to its floor anyway)
SERVO_V_MIN = 0.03
SERVO_W_MAX = 0.4 # rad/s
SERVO_W_MIN = 0.12
ALIGN_TOL_ENV = "MOSS_SERVO_ALIGN_DEG"
ALIGN_TOL_DEG = 4.0 # kept as a knob for the report; the law below needs no bearing tolerance
K_V = 0.8
K_W = 1.5
TICK_HZ = 10.0
PULSE_S_ENV = "MOSS_SERVO_PULSE_S"
PULSE_S = 0.1 # one tick of twist...
REST_S_ENV = "MOSS_SERVO_REST_S"
REST_S = 0.4 # ...then this long of zeros, near the object and while turning
FAR_M_ENV = "MOSS_SERVO_FAR_M"
FAR_M = 0.25 # beyond this error the twist is continuous
EXPLORING, STOPPING, APPROACHING, SERVOING, PICKING, RESUMING = (
"EXPLORING", "STOPPING", "APPROACHING", "SERVOING", "PICKING", "RESUMING")
def pick_xy(env: dict | None = None) -> tuple[float, float]:
env = os.environ if env is None else env
raw = env.get(PICK_XY_ENV, "").split()
if len(raw) == 2:
try:
return (float(raw[0]), float(raw[1]))
except ValueError:
pass
return PICK_XY_DEFAULT
def _env_float(name: str, default: float, env: dict | None = None) -> float:
env = os.environ if env is None else env
try:
return float(env.get(name, "").strip() or default)
except ValueError:
return default
def wrap(a: float) -> float:
return (a + math.pi) % (2 * math.pi) - math.pi
def in_base(ox: float, oy: float, rx: float, ry: float, yaw: float) -> tuple[float, float]:
"""A world point in base_link (x ahead, y left) for a robot at (rx, ry, yaw)."""
dx, dy = ox - rx, oy - ry
c, s = math.cos(yaw), math.sin(yaw)
return (dx * c + dy * s, -dx * s + dy * c)
def standoff_goal(ox: float, oy: float, rx: float, ry: float, d: float) -> tuple[float, float, float]:
"""The pose d before the object on the line from the robot, facing it."""
dx, dy = ox - rx, oy - ry
r = math.hypot(dx, dy)
if r < 1e-6:
return (rx, ry, 0.0)
ux, uy = dx / r, dy / r
gx, gy = ox - d * ux, oy - d * uy
return (gx, gy, math.atan2(oy - gy, ox - gx))
@dataclass
class Target:
track_id: int
name: str
x: float
y: float
@dataclass
class Seen:
track_id: int
name: str
x: float
y: float
count: int = 1
@dataclass
class MissionLogic:
pick: tuple[float, float] = PICK_XY_DEFAULT
standoff_m: float = STANDOFF_DEFAULT
tol_m: float = TOL_DEFAULT
classes: set = field(default_factory=lambda: {"Glass", "Metal", "Paper", "Plastic", "Waste"})
clock: object = time.monotonic
align_tol_rad: float = math.radians(ALIGN_TOL_DEG)
pulse_s: float = PULSE_S
rest_s: float = REST_S
far_m: float = FAR_M
_pulse_until: float = -1.0 # the pulse in progress ends here
_rest_until: float = -1.0 # zeros until here
_last_db: float = 0.0 # the bearing error of the last servo_want
state: str = EXPLORING
target: Target | None = None
pose: tuple[float, float, float] | None = None
objects: list = field(default_factory=list)
done: list = field(default_factory=list) # (x, y) of picked objects
skipped: dict = field(default_factory=dict) # track_id -> time skipped
picked_ids: set = field(default_factory=set)
_since: float = 0.0
_goal_verdict: bool | None = None
_pick_result: bool | None = None
log: list = field(default_factory=list)
# ── inputs ──
def on_objects(self, seen: list) -> None:
self.objects = list(seen)
def on_odom(self, x: float, y: float, yaw: float) -> None:
self.pose = (x, y, yaw)
def on_goal_reached(self, ok: bool) -> None:
if self.state == APPROACHING:
self._goal_verdict = bool(ok)
def pick_done(self, ok: bool) -> None:
if self.state == PICKING:
self._pick_result = bool(ok)
# ── the decision ──
def _enter(self, state: str, now: float, why: str = "") -> None:
self.log.append((now, state, why))
self.state = state
self._since = now
def _is_done(self, x: float, y: float) -> bool:
return any(math.hypot(x - dx, y - dy) < NEAR_DONE_M for dx, dy in self.done)
def choose_target(self, now: float) -> Target | None:
if self.pose is None:
return None
rx, ry, _ = self.pose
best = None
for o in self.objects:
if o.name not in self.classes or o.track_id in self.picked_ids or self._is_done(o.x, o.y):
continue
t_skip = self.skipped.get(o.track_id)
if t_skip is not None and now - t_skip < SKIP_FOR_S:
continue
r = math.hypot(o.x - rx, o.y - ry)
if r > MAX_TARGET_RANGE_M:
continue
if best is None or r < best[0]:
best = (r, o)
return Target(best[1].track_id, best[1].name, best[1].x, best[1].y) if best else None
def _refresh_target(self) -> None:
"""The object's latest position, when the detector still sees it."""
if self.target is None:
return
for o in self.objects:
if o.track_id == self.target.track_id:
self.target.x, self.target.y = o.x, o.y
return
def servo_want(self) -> tuple[float, float, bool, float]:
"""(v, w, arrived, error m): the twist that drives the pick point onto
the object. The pick point P sits at (px, py) in base_link; its
velocity under a twist (v, w) is (v - w py, w px), so the twist for a
wanted velocity of P is w = vy / px, v = vx + w py: one law for the
turn and the creep together, no turn-then-creep alternation."""
rx, ry, yaw = self.pose
bx, by = in_base(self.target.x, self.target.y, rx, ry, yaw)
px, py = self.pick
ex, ey = bx - px, by - py
err = math.hypot(ex, ey)
bearing = math.atan2(by, bx)
self._last_db = wrap(bearing - math.atan2(py, px))
if err <= self.tol_m:
return (0.0, 0.0, True, err)
vx, vy = K_V * ex, K_V * ey
speed = math.hypot(vx, vy)
if speed > SERVO_V_MAX:
vx, vy = vx * SERVO_V_MAX / speed, vy * SERVO_V_MAX / speed
w = vy / px
if abs(w) > SERVO_W_MAX:
k = SERVO_W_MAX / abs(w)
w, vx = w * k, vx * k
v = max(-SERVO_V_MAX, min(SERVO_V_MAX, vx + w * py))
if abs(v) < SERVO_V_MIN and abs(v) > 1e-6:
v = math.copysign(SERVO_V_MIN, v)
if abs(w) < SERVO_W_MIN and abs(w) > 1e-6:
w = math.copysign(SERVO_W_MIN, w)
return (v, w, False, err)
def servo(self, now: float | None = None) -> tuple[float, float, bool]:
"""(v, w, arrived) toward the pick point for the current target, pulsed
near it and while turning: a twist for pulse_s, zeros for rest_s."""
now = self.clock() if now is None else now
v, w, arrived, err = self.servo_want()
if arrived:
self._pulse_until = self._rest_until = -1.0
return (0.0, 0.0, True)
if err > self.far_m:
return (v, w, False)
if now < self._pulse_until:
return (v, w, False)
if now < self._rest_until:
return (0.0, 0.0, False)
self._pulse_until = now + self.pulse_s
self._rest_until = self._pulse_until + self.rest_s
return (v, w, False)
def _skip(self, now: float, why: str) -> list:
if self.target is not None:
self.skipped[self.target.track_id] = now
self._enter(RESUMING, now, why)
return [("twist", 0.0, 0.0), ("explore",), ("state", f"{RESUMING}: {why}")]
def tick(self, now: float | None = None) -> list:
"""One step; the commands to publish, in order."""
now = self.clock() if now is None else now
out: list = []
if self.state == EXPLORING:
t = self.choose_target(now)
if t is not None:
self.target = t
self._enter(STOPPING, now, f"{t.name} #{t.track_id} at ({t.x:.2f}, {t.y:.2f})")
out += [("stop_explore",), ("state", f"{STOPPING}: {t.name} #{t.track_id} at ({t.x:.2f}, {t.y:.2f})")]
elif self.state == STOPPING:
if now - self._since >= STOP_SETTLE_S:
self._refresh_target()
rx, ry, _ = self.pose
r = math.hypot(self.target.x - rx, self.target.y - ry)
if r <= self.standoff_m + 0.2:
self._enter(SERVOING, now, f"already {r:.2f} m away")
out.append(("state", f"{SERVOING}: already {r:.2f} m away"))
else:
gx, gy, gyaw = standoff_goal(self.target.x, self.target.y, rx, ry, self.standoff_m)
self._goal_verdict = None
self._enter(APPROACHING, now, f"goal ({gx:.2f}, {gy:.2f}, {math.degrees(gyaw):.0f} deg)")
out += [("goal", gx, gy, gyaw), ("state", f"{APPROACHING}: goal ({gx:.2f}, {gy:.2f})")]
elif self.state == APPROACHING:
if self._goal_verdict is True:
self._enter(SERVOING, now, "goal reached")
out.append(("state", f"{SERVOING}: goal reached"))
elif self._goal_verdict is False:
out += self._skip(now, "the planner gave up the goal")
elif now - self._since >= APPROACH_TIMEOUT_S:
self._enter(SERVOING, now, "approach timeout, servoing from here")
out.append(("state", f"{SERVOING}: approach timeout"))
elif self.state == SERVOING:
self._refresh_target()
if self.pose is None:
return out
v, w, arrived = self.servo(now)
if arrived:
out.append(("twist", 0.0, 0.0))
self._pick_result = None
self._enter(PICKING, now, "at the pick point")
out += [("pick",), ("state", f"{PICKING}: {self.target.name} #{self.target.track_id}")]
elif now - self._since >= SERVO_TIMEOUT_S:
out += self._skip(now, "servo timeout")
else:
out.append(("twist", v, w))
elif self.state == PICKING:
if self._pick_result is True:
self.done.append((self.target.x, self.target.y))
self.picked_ids.add(self.target.track_id)
self._enter(RESUMING, now, "picked")
out += [("explore",), ("state", f"{RESUMING}: picked {self.target.name} #{self.target.track_id}")]
elif self._pick_result is False:
out += self._skip(now, "the pick failed")
elif now - self._since >= PICK_TIMEOUT_S:
out += self._skip(now, "pick timeout")
elif self.state == RESUMING:
if now - self._since >= RESUME_SETTLE_S:
self.target = None
self._enter(EXPLORING, now, "")
out.append(("state", EXPLORING))
return out
# ── the dimOS module ──
_MISSION_CLASS = None
def _module_class():
"""Built on first use: the dimOS imports stay out of a plain import of this file.
One class per process, exposed as `moss_dimos.mission.LitterMission` with that
qualname, so dimOS can pickle it by reference into its worker processes."""
global _MISSION_CLASS
if _MISSION_CLASS is not None:
return _MISSION_CLASS
from dimos.core.core import rpc
from dimos.core.module import Module
from dimos.core.stream import In, Out
from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
from dimos.msgs.geometry_msgs.Quaternion import Quaternion
from dimos.msgs.geometry_msgs.Twist import Twist
from dimos.msgs.geometry_msgs.Vector3 import Vector3
from dimos.msgs.std_msgs.String import String
from dimos.perception.experimental.object import Object as DetObject
from dimos_lcm.std_msgs import Bool
from dimos.utils.logging_config import setup_logger
logger = setup_logger()
# dimOS resolves the stream annotations in this module's namespace: the names must be there
globals().update(In=In, Out=Out, PoseStamped=PoseStamped, Twist=Twist, Bool=Bool, String=String,
DetObject=DetObject)
class LitterMission(Module):
objects: In[list[DetObject]]
odom: In[PoseStamped]
goal_reached: In[Bool]
goal_request: Out[PoseStamped]
tele_cmd_vel: Out[Twist]
stop_explore_cmd: Out[Bool]
explore_cmd: Out[Bool]
mission_state: Out[String]
def __init__(self, pick_x: float | None = None, pick_y: float | None = None,
standoff_m: float | None = None, tol_m: float | None = None,
classes: str | None = None, pick_cmd: str | None = None, pick_sim: bool | None = None,
autostart: bool | None = None, **kwargs):
super().__init__(**kwargs)
px, py = pick_xy()
from moss_dimos.litter_detect import litter_classes
cls = {c.strip() for c in classes.split(",")} if classes else litter_classes()
self._logic = MissionLogic(pick=(pick_x if pick_x is not None else px, pick_y if pick_y is not None else py),
standoff_m=standoff_m if standoff_m is not None else _env_float(STANDOFF_ENV, STANDOFF_DEFAULT),
tol_m=tol_m if tol_m is not None else _env_float(TOL_ENV, TOL_DEFAULT),
classes=cls,
align_tol_rad=math.radians(_env_float(ALIGN_TOL_ENV, ALIGN_TOL_DEG)),
pulse_s=_env_float(PULSE_S_ENV, PULSE_S), rest_s=_env_float(REST_S_ENV, REST_S),
far_m=_env_float(FAR_M_ENV, FAR_M))
self._pick_cmd = pick_cmd if pick_cmd is not None else os.environ.get(PICK_CMD_ENV, "").strip()
self._pick_sim = pick_sim if pick_sim is not None else os.environ.get(PICK_SIM_ENV, "").strip() in ("1", "true", "yes", "on")
self._autostart = autostart if autostart is not None else os.environ.get(AUTOSTART_ENV, "1").strip() not in ("0", "false", "no", "off")
self._lock = threading.Lock()
self._stop = threading.Event()
self._thread: threading.Thread | None = None
self._pick_thread: threading.Thread | None = None
self._last_state_text = ""
@property
def logic(self) -> MissionLogic:
return self._logic
@rpc
def start(self) -> None:
super().start()
self.objects.subscribe(self._on_objects)
self.odom.subscribe(self._on_odom)
self.goal_reached.subscribe(self._on_goal_reached)
self._stop.clear()
self._thread = threading.Thread(target=self._run, name="litter-mission", daemon=True)
self._thread.start()
lg = self._logic
logger.info(f"litter mission started: pick point {lg.pick} m in base_link, standoff "
f"{lg.standoff_m} m, tolerance {lg.tol_m} m, align {math.degrees(lg.align_tol_rad):.0f} deg, "
f"pulses {lg.pulse_s} s on / {lg.rest_s} s off within {lg.far_m} m, classes {sorted(lg.classes)}, "
f"pick command {self._pick_cmd or 'tools/moss_arm_play.py'}{' --sim' if self._pick_sim else ''}")
if self._autostart:
self.explore_cmd.publish(Bool(data=True))
@rpc
def stop(self) -> None:
self._stop.set()
if self._thread is not None:
self._thread.join(timeout=2.0)
try:
self.tele_cmd_vel.publish(Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)))
except Exception: # noqa: BLE001
pass
super().stop()
@rpc
def mission_status(self) -> dict:
lg = self._logic
return {"state": lg.state, "target": None if lg.target is None else
{"track_id": lg.target.track_id, "name": lg.target.name, "x": lg.target.x, "y": lg.target.y},
"picked": len(lg.done), "skipped": len(lg.skipped)}
def _on_objects(self, msg) -> None:
seen = []
for o in msg or []:
try:
seen.append(Seen(int(o.track_id), str(o.name), float(o.center.x), float(o.center.y),
int(getattr(o, "detections_count", 1))))
except Exception: # noqa: BLE001 - an object without a centre
continue
with self._lock:
self._logic.on_objects(seen)
def _on_odom(self, msg) -> None:
try:
yaw = float(msg.orientation.to_euler().z)
x, y = float(msg.position.x), float(msg.position.y)
except Exception: # noqa: BLE001
return
with self._lock:
self._logic.on_odom(x, y, yaw)
def _on_goal_reached(self, msg) -> None:
with self._lock:
self._logic.on_goal_reached(bool(msg.data))
def _run(self) -> None:
period = 1.0 / TICK_HZ
while not self._stop.is_set():
t0 = time.monotonic()
with self._lock:
cmds = self._logic.tick(t0)
for c in cmds:
try:
self._do(c)
except Exception as exc: # noqa: BLE001
logger.warning(f"litter mission: command {c[0]} failed: {exc}")
time.sleep(max(0.0, period - (time.monotonic() - t0)))
def _do(self, c: tuple) -> None:
kind = c[0]
if kind == "twist":
self.tele_cmd_vel.publish(Twist(Vector3(float(c[1]), 0.0, 0.0), Vector3(0.0, 0.0, float(c[2]))))
elif kind == "goal":
_, x, y, yaw = c
q = Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw)))
self.goal_request.publish(PoseStamped(time.time(), "world",
position=Vector3(float(x), float(y), 0.0), orientation=q))
elif kind == "stop_explore":
self.stop_explore_cmd.publish(Bool(data=True))
elif kind == "explore":
self.explore_cmd.publish(Bool(data=True))
elif kind == "pick":
self._start_pick()
elif kind == "state":
text = str(c[1])
if text != self._last_state_text:
self._last_state_text = text
logger.info(f"litter mission: {text}")
self.mission_state.publish(String(data=text))
def _start_pick(self) -> None:
if self._pick_cmd.lower() == "none":
with self._lock:
self._logic.pick_done(True)
return
argv = shlex.split(self._pick_cmd) if self._pick_cmd else [sys.executable, str(ROOT / "tools" / "moss_arm_play.py")]
if self._pick_sim and "--sim" not in argv:
argv.append("--sim")
def run() -> None:
try:
rc = subprocess.run(argv, timeout=PICK_TIMEOUT_S, check=False).returncode
except Exception as exc: # noqa: BLE001
logger.warning(f"litter mission: the pick command failed to run: {exc}")
rc = 1
logger.info(f"litter mission: pick command ended with {rc}")
with self._lock:
self._logic.pick_done(rc == 0)
self._pick_thread = threading.Thread(target=run, name="litter-pick", daemon=True)
self._pick_thread.start()
LitterMission.__module__ = __name__
LitterMission.__qualname__ = "LitterMission"
_MISSION_CLASS = LitterMission
return LitterMission
def __getattr__(name: str):
if name == "LitterMission":
cls = _module_class()
globals()[name] = cls
return cls
raise AttributeError(f"module {__name__!r} has no attribute {name!r}")