MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
litter: the detection on the nose camera (dimOS's object scene registration with a YOLO trained on litter), the mission (explore, stop, approach, servo to the pick point, pick over RPC, explore again), the moss-litter blueprint, a cold bench on a fake robot
metrox686548ee4704
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.pyindex 12a2187..0ba00e0 100644--- a/benches/bench_moss_blueprints_cold.py+++ b/benches/bench_moss_blueprints_cold.py@@ -1,4 +1,4 @@-"""Cold bench for moss_dimos.blueprints: devices, the import, the five+"""Cold bench for moss_dimos.blueprints: devices, the import, the sixblueprints, the coordinator in a fresh worker.Known input -> known output in physical units. Groups:@@ -21,8 +21,8 @@ Known input -> known output in physical units. Groups:E. the camera - the base_link -> camera_link mount from the profile(pitch -> quaternion)H. wrist index - wrist_camera_index() on a fake /dev/v4l/by-id- F. entry points - pyproject: distribution moss-dimos, the five entry points- on the five lazy builders; then each one resolved through+ F. entry points - pyproject: distribution moss-dimos, the six entry points+ on the six lazy builders; then each one resolved throughdimOS's own `dimos run` lookup (get_by_name) from atemporary dist-info built from pyproject.tomlG. blueprints - moss-base, moss-gamepad, moss-nav and moss-explore build:@@ -243,7 +243,7 @@ check("the import runs in a clean interpreter", got is not None, f"rc {probe.retgot = got or {}check("'moss' unknown to the registry before the import", got.get("before") is None, f"{got.get('before')}")check(f"after it: 'moss' -> {FACTORY} (the factory path is a string)", got.get("after") == FACTORY, f"{got.get('after')}")-check("no blueprint built at import (all five are lazy)", got.get("built") == [], f"{got.get('built')}")+check("no blueprint built at import (all six are lazy)", got.get("built") == [], f"{got.get('built')}")check("no pad, lidar, map, planner, explorer, recorder or camera module imported with it",got.get("heavy") == [], f"{got.get('heavy')}")check("no environment variable set by the import (ROBOT_PROFILE, PERSISTENT_MAP untouched)",@@ -375,9 +375,10 @@ TARGETS = {"moss-base": "moss_dimos.blueprints:moss_base_blueprint","moss-gamepad": "moss_dimos.blueprints:moss_gamepad_blueprint","moss-nav": "moss_dimos.blueprints:moss_nav_blueprint","moss-explore": "moss_dimos.blueprints:moss_explore_blueprint",- "moss-arm": "moss_dimos.blueprints:moss_arm_blueprint"}+ "moss-arm": "moss_dimos.blueprints:moss_arm_blueprint",+ "moss-litter": "moss_dimos.blueprints:moss_litter_blueprint"}check("distribution name moss-dimos (the dimOS namespace)", project.get("name") == "moss-dimos", f"{project.get('name')}")-check("exactly the five MOSS entry points, on moss_dimos.blueprints", eps == TARGETS, f"{eps}")+check("exactly the six MOSS entry points, on moss_dimos.blueprints", eps == TARGETS, f"{eps}")check("every target has a lazy builder", {t.split(":")[1] for t in eps.values()} == set(mb._BUILDERS),f"{sorted(mb._BUILDERS)}")check("names match dimOS's ^[a-z0-9]+(-[a-z0-9]+)*$", bool(eps) and all(re.match(r"^[a-z0-9]+(-[a-z0-9]+)*$", k) for k in eps))@@ -399,8 +400,8 @@ from dimos.robot.external_blueprints import list_external_blueprint_names # noqfrom dimos.robot.get_all_blueprints import get_by_name # noqa: E402listed = [n for n in list_external_blueprint_names() if n.startswith("moss-dimos.")]-check("dimOS lists moss-dimos.moss-arm / moss-base / moss-explore / moss-gamepad / moss-nav",- listed == ["moss-dimos.moss-arm", "moss-dimos.moss-base", "moss-dimos.moss-explore", "moss-dimos.moss-gamepad", "moss-dimos.moss-nav"],+check("dimOS lists moss-dimos.moss-arm / moss-base / moss-explore / moss-gamepad / moss-litter / moss-nav",+ listed == ["moss-dimos.moss-arm", "moss-dimos.moss-base", "moss-dimos.moss-explore", "moss-dimos.moss-gamepad", "moss-dimos.moss-litter", "moss-dimos.moss-nav"],f"{listed}")resolved = {}for local in TARGETS:diff --git a/benches/bench_moss_mission_cold.py b/benches/bench_moss_mission_cold.pynew file mode 100644index 0000000..282d103--- /dev/null+++ b/benches/bench_moss_mission_cold.py@@ -0,0 +1,292 @@+"""Cold bench for moss_dimos.mission: the litter mission on a fake robot,+no hardware, no dimOS stack.++Known input -> known output in metres, degrees and seconds. Groups:++ A. geometry - a world point in base_link for a robot facing +x, +y and+ -x; the standoff goal 0.6 m before an object, facing it;+ the angle wrap+ B. servo - the servo on a fake unicycle (v, w integrated at 10 Hz)+ brings an object 1 m ahead and 0.3 m left onto the pick+ point within 2 cm in under 60 s, turning first, then+ creeping; an object behind the robot too; the twists+ never exceed the ceilings+ C. the mission - the state machine on the fake robot, tick by tick:+ nothing while no litter is seen; a confirmed "Plastic"+ 2 m away stops the explorer, then (3 s later) sends the+ standoff goal; goal_reached True starts the servo; the+ servo ends at the pick point and the pick command fires;+ pick_done True resumes exploring and the same object is+ never targeted again; a "Chair" is ignored; goal_reached+ False skips the object (explore resumes) and it stays+ skipped; a servo timeout skips too; an object already+ within reach goes straight from STOPPING to SERVOING+ D. the module - under dimOS: the module instantiates, its status RPC+ answers, a goal command becomes a PoseStamped in the world+ frame with the right yaw, a twist command a Twist, objects+ and odometry messages reach the logic, the "none" pick+ command marks the object picked++Run (A-C need only the repository; D the dimOS of the robot or main):+ nice -n 10 <dimos venv>/bin/python benches/bench_moss_mission_cold.py+"""++from __future__ import annotations++import math+import os+import sys+import time+from pathlib import Path+from types import SimpleNamespace++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))++if os.nice(0) < 10:+ os.nice(10 - os.nice(0))++from moss_dimos import mission as M # noqa: E402++FAIL: list[str] = []+OK = 0+DEG = 180.0 / math.pi+++def check(cond: bool, what: str) -> None:+ global OK+ if cond:+ OK += 1+ print(f" ok {what}")+ else:+ FAIL.append(what)+ print(f" FAIL {what}")+++class FakeRobot:+ """A unicycle: v along its heading, w about z, integrated per tick."""++ def __init__(self, x=0.0, y=0.0, yaw=0.0) -> None:+ self.x, self.y, self.yaw = x, y, yaw++ def step(self, v: float, w: float, dt: float) -> None:+ self.yaw = M.wrap(self.yaw + w * dt)+ self.x += v * math.cos(self.yaw) * dt+ self.y += v * math.sin(self.yaw) * dt+++class Clock:+ def __init__(self) -> None:+ self.t = 1000.0++ def __call__(self) -> float:+ return self.t+++PICK = (0.209, -0.098)++# ── A. geometry ──+print("A. geometry")+bx, by = M.in_base(1.0, 0.0, 0.0, 0.0, 0.0)+check(abs(bx - 1.0) < 1e-9 and abs(by) < 1e-9, "object 1 m ahead, robot facing +x: (1, 0) in base_link")+bx, by = M.in_base(0.0, 1.0, 0.0, 0.0, math.pi / 2)+check(abs(bx - 1.0) < 1e-9 and abs(by) < 1e-9, "object at +y, robot facing +y: (1, 0)")+bx, by = M.in_base(0.0, 1.0, 0.0, 0.0, 0.0)+check(abs(bx) < 1e-9 and abs(by - 1.0) < 1e-9, "object at +y, robot facing +x: (0, 1) = on the left")+bx, by = M.in_base(1.0, 0.0, 2.0, 0.0, math.pi)+check(abs(bx - 1.0) < 1e-9 and abs(by) < 1e-9, "robot at x 2 facing -x, object at x 1: 1 m ahead")+gx, gy, gyaw = M.standoff_goal(2.0, 0.0, 0.0, 0.0, 0.6)+check(abs(gx - 1.4) < 1e-9 and abs(gy) < 1e-9 and abs(gyaw) < 1e-9, "standoff goal (1.4, 0, 0 deg) for an object at (2, 0)")+gx, gy, gyaw = M.standoff_goal(0.0, 2.0, 0.0, 0.0, 0.6)+check(abs(gx) < 1e-9 and abs(gy - 1.4) < 1e-9 and abs(gyaw - math.pi / 2) < 1e-9, "standoff goal (0, 1.4, 90 deg) for an object at (0, 2)")+check(abs(M.wrap(3 * math.pi) - math.pi) < 1e-9 or abs(M.wrap(3 * math.pi) + math.pi) < 1e-9, "wrap(3 pi) = +-pi")+check(abs(M.wrap(math.radians(-370)) - math.radians(-10)) < 1e-9, "wrap(-370 deg) = -10 deg")++# ── B. servo ──+print("B. servo")+++def run_servo(obj, start, limit_s=60.0):+ clk = Clock()+ lg = M.MissionLogic(pick=PICK, clock=clk)+ lg.target = M.Target(1, "Plastic", *obj)+ lg._enter(M.SERVOING, clk.t)+ rob = FakeRobot(*start)+ lg.on_odom(rob.x, rob.y, rob.yaw)+ vmax = wmax = 0.0+ t = 0.0+ turned_first = None+ while t < limit_s:+ cmds = lg.tick(clk.t)+ tw = [c for c in cmds if c[0] == "twist"]+ if any(c[0] == "pick" for c in cmds):+ break+ if tw:+ _, v, w = tw[-1]+ vmax, wmax = max(vmax, abs(v)), max(wmax, abs(w))+ if turned_first is None and (abs(v) > 0 or abs(w) > 0):+ turned_first = abs(v) == 0.0 and abs(w) > 0.0+ rob.step(v, w, 0.1)+ clk.t += 0.1+ t += 0.1+ lg.on_odom(rob.x, rob.y, rob.yaw)+ bx, by = M.in_base(obj[0], obj[1], rob.x, rob.y, rob.yaw)+ err = math.hypot(bx - PICK[0], by - PICK[1])+ return lg, t, err, vmax, wmax, turned_first, (bx, by)+++lg, t, err, vmax, wmax, turned_first, b = run_servo((1.0, 0.3), (0.0, 0.0, 0.0))+check(lg.state == M.PICKING, f"object 1 m ahead, 0.3 m left: PICKING after {t:.1f} s")+check(err <= 0.02, f"object in base_link ({b[0]:.3f}, {b[1]:.3f}) = the pick point within {err * 100:.1f} cm")+check(turned_first is True, "it turned first (v = 0 while aligning)")+check(vmax <= M.SERVO_V_MAX + 1e-9 and wmax <= M.SERVO_W_MAX + 1e-9, f"twists within the ceilings (v {vmax:.2f} m/s, w {wmax:.2f} rad/s)")+lg, t, err, *_ = run_servo((-0.8, 0.4), (0.0, 0.0, 0.0))+check(lg.state == M.PICKING and err <= 0.02, f"object behind the robot: PICKING after {t:.1f} s, {err * 100:.1f} cm")+lg, t, err, *_ = run_servo((0.0, 1.5), (0.0, 0.0, 0.0), limit_s=80)+check(lg.state == M.PICKING and err <= 0.02, f"object 1.5 m to the left: PICKING after {t:.1f} s, {err * 100:.1f} cm")++# ── C. the mission ──+print("C. the mission")+clk = Clock()+lg = M.MissionLogic(pick=PICK, clock=clk)+rob = FakeRobot()+lg.on_odom(rob.x, rob.y, rob.yaw)+check(lg.tick(clk.t) == [] and lg.state == M.EXPLORING, "nothing seen: EXPLORING, no command")+lg.on_objects([M.Seen(7, "Chair", 1.0, 0.0, 9)])+check(lg.tick(clk.t) == [] and lg.state == M.EXPLORING, "a Chair is not litter")+lg.on_objects([M.Seen(7, "Chair", 1.0, 0.0, 9), M.Seen(3, "Plastic", 2.0, 0.5, 6)])+cmds = lg.tick(clk.t)+check(cmds and cmds[0] == ("stop_explore",) and lg.state == M.STOPPING and lg.target.track_id == 3,+ "a confirmed Plastic 2 m away: stop_explore, STOPPING, target #3")+clk.t += 1.0+check(lg.tick(clk.t) == [] and lg.state == M.STOPPING, "1 s later: still settling")+clk.t += 2.5+cmds = lg.tick(clk.t)+goal = [c for c in cmds if c[0] == "goal"]+gx, gy, gyaw = M.standoff_goal(2.0, 0.5, 0.0, 0.0, M.STANDOFF_DEFAULT)+check(lg.state == M.APPROACHING and goal and abs(goal[0][1] - gx) < 1e-9 and abs(goal[0][2] - gy) < 1e-9 and abs(goal[0][3] - gyaw) < 1e-9,+ f"3.5 s after the stop: goal ({gx:.2f}, {gy:.2f}, {math.degrees(gyaw):.0f} deg), APPROACHING")+clk.t += 5.0+check(lg.tick(clk.t) == [] and lg.state == M.APPROACHING, "waiting for the planner")+rob.x, rob.y, rob.yaw = gx, gy, gyaw+lg.on_odom(rob.x, rob.y, rob.yaw)+lg.on_goal_reached(True)+cmds = lg.tick(clk.t)+check(lg.state == M.SERVOING, "goal reached: SERVOING")+picked = False+t = 0.0+while t < 60.0 and not picked:+ cmds = lg.tick(clk.t)+ for c in cmds:+ if c[0] == "twist":+ rob.step(c[1], c[2], 0.1)+ if c[0] == "pick":+ picked = True+ clk.t += 0.1+ t += 0.1+ lg.on_odom(rob.x, rob.y, rob.yaw)+bx, by = M.in_base(2.0, 0.5, rob.x, rob.y, rob.yaw)+check(picked and lg.state == M.PICKING, f"the servo ends at the pick point in {t:.1f} s and the pick command fires")+check(math.hypot(bx - PICK[0], by - PICK[1]) <= 0.02, f"object in base_link ({bx:.3f}, {by:.3f})")+check(cmds and cmds[0] == ("twist", 0.0, 0.0), "a zero twist first")+clk.t += 2.0+check(lg.tick(clk.t) == [] and lg.state == M.PICKING, "PICKING waits for the pick command")+lg.pick_done(True)+cmds = lg.tick(clk.t)+check(("explore",) in cmds and lg.state == M.RESUMING and lg.done == [(2.0, 0.5)], "pick done: explore_cmd, RESUMING, the object remembered")+clk.t += 1.5+lg.tick(clk.t)+check(lg.state == M.EXPLORING, "then EXPLORING")+lg.on_objects([M.Seen(3, "Plastic", 2.02, 0.49, 20), M.Seen(9, "Plastic", 2.05, 0.52, 6)])+check(lg.tick(clk.t) == [] and lg.state == M.EXPLORING, "the picked object, and a new track at the same spot, are not targeted again")+# a refused goal skips the object+lg.on_objects([M.Seen(11, "Metal", rob.x + 4.5, rob.y, 6)])+lg.tick(clk.t)+check(lg.state == M.EXPLORING, "an object 4.5 m away is out of range")+lg.on_objects([M.Seen(11, "Metal", rob.x + 1.5, rob.y, 6)])+lg.tick(clk.t)+clk.t += 3.5+lg.tick(clk.t)+check(lg.state == M.APPROACHING and lg.target.track_id == 11, "a Metal 1.5 m away: APPROACHING #11")+lg.on_goal_reached(False)+cmds = lg.tick(clk.t)+check(lg.state == M.RESUMING and ("explore",) in cmds and 11 in lg.skipped, "the planner gave up: skipped, explore again")+clk.t += 1.5+lg.tick(clk.t)+lg.tick(clk.t)+check(lg.state == M.EXPLORING and lg.target is None, "the skipped object is not targeted again")+# servo timeout+lg.on_objects([M.Seen(12, "Paper", 2.3, 0.0, 6)])+lg.tick(clk.t)+clk.t += 3.5+lg.tick(clk.t)+lg.on_goal_reached(True)+lg.tick(clk.t)+check(lg.state == M.SERVOING and lg.target.track_id == 12, "a Paper: SERVOING #12")+clk.t += M.SERVO_TIMEOUT_S + 0.5+cmds = lg.tick(clk.t)+check(lg.state == M.RESUMING and 12 in lg.skipped and cmds[0] == ("twist", 0.0, 0.0), "the robot never got there (no odometry progress): servo timeout, zero twist, skipped")+clk.t += 1.5+lg.tick(clk.t)+# already within reach+lg.on_objects([M.Seen(13, "Glass", rob.x + 0.5, rob.y, 6)])+lg.on_odom(rob.x, rob.y, 0.0)+lg.tick(clk.t)+clk.t += 3.5+cmds = lg.tick(clk.t)+check(lg.state == M.SERVOING and not any(c[0] == "goal" for c in cmds), "a Glass 0.5 m ahead: no goal, straight to SERVOING")++# ── D. the module ──+print("D. the module")+try:+ cls = M.LitterMission+except Exception as exc: # noqa: BLE001+ print(f" (dimOS not importable here: {exc}; D skipped)")+else:+ mod = cls(pick_cmd="none", autostart=False)+ st = mod.mission_status()+ check(st["state"] == M.EXPLORING and st["picked"] == 0, f"status: {st}")+ sent: dict = {}++ class Rec:+ def __init__(self, k):+ self.k = k++ def publish(self, msg):+ sent.setdefault(self.k, []).append(msg)++ for k in ("goal_request", "tele_cmd_vel", "stop_explore_cmd", "explore_cmd", "mission_state"):+ setattr(mod, k, Rec(k))+ mod._do(("goal", 1.0, 2.0, math.pi / 2))+ g = sent["goal_request"][0]+ yaw = g.orientation.to_euler().z+ check(g.frame_id == "world" and abs(g.position.x - 1.0) < 1e-9 and abs(g.position.y - 2.0) < 1e-9 and abs(yaw - math.pi / 2) < 1e-6,+ f"a goal command: PoseStamped in world at (1, 2), yaw {math.degrees(yaw):.0f} deg")+ mod._do(("twist", 0.05, -0.2))+ tw = sent["tele_cmd_vel"][0]+ check(abs(tw.linear.x - 0.05) < 1e-9 and abs(tw.angular.z + 0.2) < 1e-9, "a twist command: Twist(0.05, -0.2)")+ mod._do(("stop_explore",))+ mod._do(("explore",))+ check(sent["stop_explore_cmd"][0].data is True and sent["explore_cmd"][0].data is True, "stop_explore and explore: Bool(True)")+ mod._do(("state", "SERVOING: test"))+ check(sent["mission_state"][0].data == "SERVOING: test", "a state line on mission_state")+ objs = [SimpleNamespace(track_id=5, name="Plastic", center=SimpleNamespace(x=1.5, y=0.2, z=0.0), detections_count=8)]+ mod._on_objects(objs)+ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+ from dimos.msgs.geometry_msgs.Quaternion import Quaternion+ from dimos.msgs.geometry_msgs.Vector3 import Vector3++ mod._on_odom(PoseStamped(time.time(), "world", position=Vector3(0.0, 0.0, 0.0),+ orientation=Quaternion.from_euler(Vector3(0.0, 0.0, math.radians(30)))))+ lg2 = mod.logic+ check(lg2.objects[0].track_id == 5 and abs(lg2.objects[0].x - 1.5) < 1e-9 and abs(lg2.pose[2] - math.radians(30)) < 1e-6,+ "objects and odometry reach the logic (yaw 30 deg)")+ lg2.state = M.PICKING+ lg2.target = M.Target(5, "Plastic", 1.5, 0.2)+ mod._start_pick()+ check(lg2._pick_result is True, "the 'none' pick command marks the object picked without moving anything")++print(f"\n{OK} OK, {len(FAIL)} FAIL")+for f in FAIL:+ print(f" - {f}")+sys.exit(1 if FAIL else 0)diff --git a/docs/litter.md b/docs/litter.mdnew file mode 100644index 0000000..09c5612--- /dev/null+++ b/docs/litter.md@@ -0,0 +1,84 @@+# Litter: see it, go to it, pick it++`moss-litter` is `moss-explore` plus two modules: the litter detection on+the nose camera and the mission that drives from a sighting to the pick.+The arm runs in its own `dimos run` (`moss-arm`, docs/arm.md); the mission+calls its cycle over RPC.++```sh+.venv/bin/dimos run moss-dimos.moss-arm # one terminal: the arm's coordinator+.venv/bin/dimos run moss-dimos.moss-litter # another: explore, detect, approach, pick, explore+```++## Detection++`moss_dimos/litter_detect.py` is dimOS's object scene registration+(`ObjectSceneRegistrationModule`) with one change: it runs a YOLO trained+on litter from a weights file of ours instead of the open-vocabulary model+dimOS downloads. The module aligns colour and depth frames, lifts every box+to a 3D object through the depth image and the camera's transform to the+world frame of the odometry, and publishes on `objects` the ones confirmed+by repeated sightings (`MOSS_LITTER_MIN_DETECTIONS`, 6). Two sightings+closer than 0.2 m are one object.++The weights are not in this repository. The ones the mission is tuned to+are a small YOLO trained on waste images with five material classes+(glass, metal, paper, plastic, waste), published under the MIT licence at+github.com/gianlucasposito/YOLO-Waste-Detection (`best_model.pt`, 6 MB):++```sh+mkdir -p ~/.local/state/moss/models+curl -L -o ~/.local/state/moss/models/litter.pt \+ https://raw.githubusercontent.com/gianlucasposito/YOLO-Waste-Detection/main/best_model.pt+```++`MOSS_LITTER_WEIGHTS` points elsewhere; `MOSS_LITTER_CLASSES` names the+classes the mission picks up (the five by default); `MOSS_LITTER_DEVICE`+forces `cpu` or `cuda`. The detector needs `torch` and `ultralytics` in the+robot's venv, which the base stack does not: on an arm64 host without a+CUDA torch, the CPU wheels do (about 200 ms a frame for this model on the+rover's CPU is the expectation; the camera runs at 5 fps):++```sh+uv pip install --python .venv/bin/python --index-url https://download.pytorch.org/whl/cpu torch torchvision+uv pip install --python .venv/bin/python ultralytics+```++## The mission++`moss_dimos/mission.py`. The decision (`MissionLogic`) is pure and the+bench drives it on a fake robot; the module around it reads `objects`,+`odom` and `goal_reached`, writes `goal_request`, `tele_cmd_vel`,+`stop_explore_cmd`, `explore_cmd` and a `mission_state` line, ticks at+10 Hz.++| State | What happens |+|---|---|+| EXPLORING | the explorer drives; a confirmed object of a litter class within 4 m, not picked or skipped, becomes the target |+| STOPPING | `stop_explore_cmd`, then 3 s for the explorer's own "stay here" goal to land first |+| APPROACHING | `goal_request`: 0.6 m before the object on the line from the robot, facing it; `goal_reached` True → SERVOING, False → the object is skipped; 90 s → SERVOING from wherever the rover is |+| SERVOING | `tele_cmd_vel` at every tick (the pad's channel: the movement manager lets it win): turn until the object's bearing is the pick point's, creep until its range is the pick point's, within 2 cm; at most 0.08 m/s and 0.4 rad/s; 60 s → skipped |+| PICKING | the pick command (`MOSS_PICK_CMD`, default `tools/moss_arm_play.py`; `MOSS_PICK_SIM=1` adds `--sim`; `none` marks the object picked without moving the arm); exit 0 → picked, else skipped |+| RESUMING | `explore_cmd`, then EXPLORING; a picked object, and anything within 0.15 m of it, is never a target again; a skipped one is tried again after 10 min |++**The pick point** is where the cycle's `pick` pose puts the tool, in+base_link (the lidar's floor projection): `MOSS_PICK_XY_M`, default+`0.209 -0.098` (the cycle's tool point less the lidar's carriage in the+CAD scene). The whole final approach rests on it; a tape measure from the+lidar's axis to the arm's pan axis on the rover is the number to set+before a run. `MOSS_STANDOFF_M` (0.6) and `MOSS_PICK_TOL_M` (0.02) are the+other two knobs. `MOSS_MISSION_AUTOSTART=0` leaves the first `explore_cmd`+to the cockpit.++```sh+python benches/bench_moss_mission_cold.py # the logic on a fake robot; the module under dimOS+```++## What this does not do yet++The approach is open-loop on odometry once the object is confirmed: the+object's world position is refreshed while the camera still sees it, and+the last 20 cm run on the odometry alone. The pick pose is the cycle's+one pose: the rover brings the object to it, the arm does not reach for+the object. The bin is the rover's own. No wrist-camera check before the+grasp.diff --git a/moss_dimos/arm.py b/moss_dimos/arm.pyindex c49e292..9271bf9 100644--- a/moss_dimos/arm.py+++ b/moss_dimos/arm.py@@ -61,6 +61,7 @@ GAP_CLOSED_M = 0.0GRIPPER_SPEED_M_PER_S = 0.2 # the task's ceiling on the gripper: the whole gap in half a secondSTART_TOLERANCE_RAD = 0.05 # the task refuses a trajectory starting further from the arm than this+ARM_COORDINATOR_NAME = "ArmCoordinator" # the arm's coordinator serves its RPCs under this name: the base's keeps ControlCoordinatorSIM_ADAPTER = "sim_mujoco" # dimOS's shared-memory MuJoCo adapterREAL_ADAPTER = "feetech_so101"REAL_ADAPTER_PATH = "moss_dimos.arm_feetech:FeetechSo101Adapter"@@ -280,10 +281,18 @@ def velocity_limits() -> dict[str, float]:def arm_trajectory_task():+ """The coordinator's trajectory task on the six joints. The per-joint+ ceiling goes in where the installed dimOS takes it (`velocity_limits`);+ an older dimOS caps at 1 rad/s itself, and the servos' own goal speed+ (the Feetech adapter sets it) holds the sweep's ceiling on the robot."""+ import inspect+from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task- return joint_trajectory_task(list(JOINT_NAMES), start_position_tolerance=START_TOLERANCE_RAD,- velocity_limits=velocity_limits())+ kw = {"start_position_tolerance": START_TOLERANCE_RAD}+ if "velocity_limits" in inspect.signature(joint_trajectory_task).parameters:+ kw["velocity_limits"] = velocity_limits()+ return joint_trajectory_task(list(JOINT_NAMES), **kw)def sim_parts(scene: str | os.PathLike, start_rad: list[float] | None = None, gap_m: float = GAP_CLOSED_M):diff --git a/moss_dimos/arm_feetech.py b/moss_dimos/arm_feetech.pyindex 107f114..90bfa93 100644--- a/moss_dimos/arm_feetech.py+++ b/moss_dimos/arm_feetech.py@@ -265,7 +265,10 @@ class FeetechSo101Adapter:return self._dofdef get_limits(self):- from dimos.hardware.spec import JointLimits+ try:+ from dimos.hardware.spec import JointLimits+ except ImportError: # the robot's dimOS keeps it with the manipulator spec+ from dimos.hardware.manipulators.spec import JointLimitslo, hi = [], []for j in A.ARM_JOINTS:diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.pyindex d928615..eef6c71 100644--- a/moss_dimos/blueprints.py+++ b/moss_dimos/blueprints.py@@ -690,20 +690,14 @@ def build_moss_nav():])-def build_moss_explore():- """moss-nav's modules plus the planner, the arbiter and the explorer. The- pad stays on tele_cmd_vel: MovementManager lets it win and cancels any- navigation. Exploration waits for `explore_cmd`."""- from dimos.core.coordination.blueprints import autoconnect+def _explore_parts(profile, ports):+ """moss-nav's modules plus the planner, the arbiter and the explorer."""from dimos.msgs.geometry_msgs.Twist import Twistfrom dimos.msgs.geometry_msgs.Vector3 import Vector3from dimos.navigation.movement_manager.movement_manager import MovementManagerfrom moss_dimos.explorer2 import Explorer2from moss_dimos.recovering_planner import RecoveringPlanner- profile = robot_profile.current()- ports = moss_ports()- _announce(ports, profile)lethal, pivot = robot_profile.derived(profile)parts = _common_parts(ports, profile) + [RecoveringPlanner.blueprint(robot_width=profile.robot_width_m,@@ -722,6 +716,49 @@ def build_moss_explore():if reloc_map:from dimos.mapping.relocalization.lidar.module import LidarWindowRelocalization # dimOS main onlyparts.append(LidarWindowRelocalization.blueprint(map_file=reloc_map))+ return parts+++def build_moss_explore():+ """moss-nav's modules plus the planner, the arbiter and the explorer. The+ pad stays on tele_cmd_vel: MovementManager lets it win and cancels any+ navigation. Exploration waits for `explore_cmd`."""+ from dimos.core.coordination.blueprints import autoconnect++ profile = robot_profile.current()+ ports = moss_ports()+ _announce(ports, profile)+ return _jpeg_camera(autoconnect(*_explore_parts(profile, ports))).remappings([+ (MossControlCoordinator, "twist_command", "cmd_vel"),+ ])+++LITTER_MIN_DETECTIONS_ENV = "MOSS_LITTER_MIN_DETECTIONS" # sightings before an object is confirmed (6)+LITTER_MIN_DETECTIONS = 6+LITTER_DISTANCE_M = 0.2 # two sightings closer than this are one object+++def build_moss_litter():+ """moss-explore plus the litter detection on the nose camera and the+ mission: explore, see litter, go to it, put it at the arm's pick point,+ run the cycle on the arm's coordinator (moss-arm, another `dimos run`),+ explore again. docs/litter.md."""+ from dimos.core.coordination.blueprints import autoconnect+ from moss_dimos.litter_detect import LitterDetection+ from moss_dimos.mission import LitterMission++ profile = robot_profile.current()+ ports = moss_ports()+ _announce(ports, profile)+ try:+ min_det = int(os.environ.get(LITTER_MIN_DETECTIONS_ENV, "").strip() or LITTER_MIN_DETECTIONS)+ except ValueError:+ min_det = LITTER_MIN_DETECTIONS+ parts = _explore_parts(profile, ports) + [+ LitterDetection.blueprint(target_frame="world", min_detections_for_permanent=min_det,+ distance_threshold=LITTER_DISTANCE_M),+ LitterMission.blueprint(),+ ]return _jpeg_camera(autoconnect(*parts)).remappings([(MossControlCoordinator, "twist_command", "cmd_vel"),])@@ -729,7 +766,8 @@ def build_moss_explore():def build_moss_arm():"""The arm alone: one coordinator with the six-joint component and the- trajectory task (tools/moss_arm_play.py sends the cycle over RPC).+ trajectory task, serving its RPCs as ArmCoordinator so it can run next+ to the base's coordinator (tools/moss_arm_play.py sends the cycle over RPC).`dimos --simulation mujoco run moss-dimos.moss-arm` adds the simulator onthe CAD scene and drives the sim adapter; without the flag the Feetechadapter opens the arm's port (MOSS_ARM_PORT). Ticks at 50 Hz on the@@ -755,7 +793,7 @@ def build_moss_arm():tick = ARM_TICK_HZreturn autoconnect(ControlCoordinator.blueprint(hardware=[hw], tasks=[arm.arm_trajectory_task()],- tick_rate=tick, instance_name=COORDINATOR_INSTANCE_NAME),+ tick_rate=tick, instance_name=arm.ARM_COORDINATOR_NAME),*extra,)@@ -766,6 +804,7 @@ ARM_TICK_HZ = 50.0 # the real bus: six reads and six writes at about 1.2 ms ea# pyproject entry point target -> builder. Keys must equal the targets._BUILDERS = {"moss_base_blueprint": build_moss_base,"moss_arm_blueprint": build_moss_arm,+ "moss_litter_blueprint": build_moss_litter,"moss_gamepad_blueprint": build_moss_gamepad,"moss_nav_blueprint": build_moss_nav,"moss_explore_blueprint": build_moss_explore}diff --git a/moss_dimos/litter_detect.py b/moss_dimos/litter_detect.pynew file mode 100644index 0000000..f387f5c--- /dev/null+++ b/moss_dimos/litter_detect.py@@ -0,0 +1,73 @@+"""Litter detection: dimOS's object scene registration on the nose camera,+with a YOLO trained on litter instead of the open-vocabulary model it ships+with.++`LitterDetection` is dimOS's `ObjectSceneRegistrationModule` with one change:+`start()` builds a `Yolo2DDetector` on a weights file of ours (`weights`,+an absolute path; MOSS_LITTER_WEIGHTS in the blueprint) instead of the+YOLOE the module downloads. Everything else is the module's: colour and+depth frames aligned in time, each 2D box lifted to a 3D object through the+depth image and the camera's transform to `target_frame` (the world frame+of the odometry), objects confirmed after `min_detections_for_permanent`+sightings and published on `objects`, which the mission reads.++The weights are not in this repository: a small YOLO trained on waste+images, five material classes (glass, metal, paper, plastic, waste), from+a public repository under the MIT licence; docs/litter.md says where.+Confidence and IoU are the detector's own (0.5 / 0.6).+"""++from __future__ import annotations++import os+from pathlib import Path+from typing import Any++from dimos.core.core import rpc+from dimos.perception.experimental import object_scene_registration as osr+from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule++WEIGHTS_ENV = "MOSS_LITTER_WEIGHTS"+WEIGHTS_DEFAULT = "~/.local/state/moss/models/litter.pt"+LITTER_CLASSES_ENV = "MOSS_LITTER_CLASSES" # comma-separated class names the mission picks up+LITTER_CLASSES_DEFAULT = "Glass,Metal,Paper,Plastic,Waste" # the five-material model's names+DETECTOR_DEVICE_ENV = "MOSS_LITTER_DEVICE" # cpu, cuda; unset = the detector's own choice+++def weights_path(env: dict | None = None) -> Path:+ env = os.environ if env is None else env+ return Path(env.get(WEIGHTS_ENV, "").strip() or WEIGHTS_DEFAULT).expanduser()+++def litter_classes(env: dict | None = None) -> set[str]:+ env = os.environ if env is None else env+ raw = env.get(LITTER_CLASSES_ENV, "").strip() or LITTER_CLASSES_DEFAULT+ return {c.strip() for c in raw.split(",") if c.strip()}+++class LitterDetection(ObjectSceneRegistrationModule):+ """Object scene registration with our YOLO weights."""++ def __init__(self, weights: str | os.PathLike | None = None, device: str | None = None,+ **kwargs: Any) -> None:+ super().__init__(**kwargs)+ self._weights = Path(weights).expanduser() if weights else weights_path()+ self._device = device or os.environ.get(DETECTOR_DEVICE_ENV, "").strip() or None++ @rpc+ def start(self) -> None:+ """The module's start with our detector in place of the YOLOE."""+ from dimos.perception.detection.detectors.yolo import Yolo2DDetector++ if not self._weights.is_file():+ raise FileNotFoundError(f"litter detection: no weights at {self._weights} ({WEIGHTS_ENV})")+ osr.Module.start(self)+ # an absolute model_path makes the detector skip its LFS lookup+ self._detector = Yolo2DDetector(model_path=str(self._weights.parent), model_name=self._weights.name,+ device=self._device)+ self.camera_info.subscribe(lambda msg: setattr(self, "_camera_info", msg))+ aligned = osr.align_timestamped(self.color_image.observable(), self.depth_image.observable(),+ buffer_size=2.0, match_tolerance=0.1)+ osr.backpressure(aligned).subscribe(self._on_aligned_frames)+ osr.logger.info(f"litter detection started: {self._weights.name}, device {self._device or 'auto'}, "+ f"target frame {self._target_frame}")diff --git a/moss_dimos/mission.py b/moss_dimos/mission.pynew file mode 100644index 0000000..49d4694--- /dev/null+++ b/moss_dimos/mission.py@@ -0,0 +1,485 @@+"""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 object's bearing is the pick point's, creep until its+ range is the pick point's, both within TOL_M; then a zero+ twist. Slow: at most SERVO_V_MAX and SERVO_W_MAX. 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 lidar's carriage as base_link) and set from a tape measure on+the robot with MOSS_PICK_XY_M. The whole final approach depends on it.+"""++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.209, -0.098) # the cycle's pick tool point less the lidar carriage, 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+SERVO_V_MIN = 0.03+SERVO_W_MAX = 0.4 # rad/s+SERVO_W_MIN = 0.12+ALIGN_TOL_RAD = math.radians(3.0)+K_V = 0.8+K_W = 1.5+TICK_HZ = 10.0++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+ 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(self) -> tuple[float, float, bool]:+ """(v, w, arrived) toward the pick point for the current target."""+ 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+ if math.hypot(ex, ey) <= self.tol_m:+ return (0.0, 0.0, True)+ bearing = math.atan2(by, bx)+ want = math.atan2(py, px)+ db = wrap(bearing - want)+ if abs(db) > ALIGN_TOL_RAD:+ w = max(-SERVO_W_MAX, min(SERVO_W_MAX, K_W * db))+ w = math.copysign(max(abs(w), SERVO_W_MIN), w)+ return (0.0, w, False)+ dr = math.hypot(bx, by) - math.hypot(px, py)+ v = max(-SERVO_V_MAX, min(SERVO_V_MAX, K_V * dr))+ v = math.copysign(max(abs(v), SERVO_V_MIN), v)+ w = max(-SERVO_W_MAX, min(SERVO_W_MAX, K_W * db))+ 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()+ 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 ──++def _module_class():+ """Built on first use: the dimOS imports stay out of a plain import of this file."""+ 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)+ 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()+ logger.info(f"litter mission started: pick point {self._logic.pick} m in base_link, standoff "+ f"{self._logic.standoff_m} m, tolerance {self._logic.tol_m} m, classes {sorted(self._logic.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()++ 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}")diff --git a/pyproject.toml b/pyproject.tomlindex c120f6c..3333cd7 100644--- a/pyproject.toml+++ b/pyproject.toml@@ -34,6 +34,7 @@ moss-gamepad = "moss_dimos.blueprints:moss_gamepad_blueprint"moss-nav = "moss_dimos.blueprints:moss_nav_blueprint"moss-explore = "moss_dimos.blueprints:moss_explore_blueprint"moss-arm = "moss_dimos.blueprints:moss_arm_blueprint"+moss-litter = "moss_dimos.blueprints:moss_litter_blueprint"[tool.setuptools]packages = ["moss_dimos", "moss_pi"]diff --git a/tools/moss_arm_play.py b/tools/moss_arm_play.pyindex cb5f2f3..c7877c6 100644--- a/tools/moss_arm_play.py+++ b/tools/moss_arm_play.py@@ -82,7 +82,7 @@ def main() -> int:from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatusfrom dimos.core.rpc_client import RPCClient- client = RPCClient(None, ControlCoordinator)+ client = RPCClient(None, ControlCoordinator, remote_name=A.ARM_COORDINATOR_NAME)try:now = client.get_joint_positions() or {}missing = [j for j in A.JOINT_NAMES if j not in now]