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

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.py
index 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 six
blueprints, 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 through
dimOS's own `dimos run` lookup (get_by_name) from a
temporary dist-info built from pyproject.toml
G. 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.ret
got = 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 # noq
from dimos.robot.get_all_blueprints import get_by_name # noqa: E402
listed = [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.py
new file mode 100644
index 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.md
new file mode 100644
index 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.py
index c49e292..9271bf9 100644
--- a/moss_dimos/arm.py
+++ b/moss_dimos/arm.py
@@ -61,6 +61,7 @@ GAP_CLOSED_M = 0.0
GRIPPER_SPEED_M_PER_S = 0.2 # the task's ceiling on the gripper: the whole gap in half a second
START_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 ControlCoordinator
SIM_ADAPTER = "sim_mujoco" # dimOS's shared-memory MuJoCo adapter
REAL_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.py
index 107f114..90bfa93 100644
--- a/moss_dimos/arm_feetech.py
+++ b/moss_dimos/arm_feetech.py
@@ -265,7 +265,10 @@ class FeetechSo101Adapter:
return self._dof
def 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 JointLimits
lo, hi = [], []
for j in A.ARM_JOINTS:
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index 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 Twist
from dimos.msgs.geometry_msgs.Vector3 import Vector3
from dimos.navigation.movement_manager.movement_manager import MovementManager
from moss_dimos.explorer2 import Explorer2
from 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 only
parts.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 on
the CAD scene and drives the sim adapter; without the flag the Feetech
adapter 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_HZ
return 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.py
new file mode 100644
index 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.py
new file mode 100644
index 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.toml
index 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.py
index 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 TrajectoryExecutionStatus
from 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]