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

perception: RealSense depth on (aligned, 5 fps), lidar_odometry depth path live from 12 cm, costmap camera layer (hit by camera obstacles, freed by camera floor and body, never carved by lidar rays); cockpit: startup camConnect() crash fixed (the page loaded nothing), camera obstacles drawn in amber with a counter; base: pulse duty floor (MOSS_PULSE_MIN_PCT), pivot percent (MOSS_PIVOT_PCT), probe --pivot, preset fondu

metroxab0e07a01de1
diff --git a/benches/bench_costmap2d_cold.py b/benches/bench_costmap2d_cold.py
index d97d2dd..b799202 100644
--- a/benches/bench_costmap2d_cold.py
+++ b/benches/bench_costmap2d_cold.py
@@ -235,7 +235,8 @@ def test_body_clear_is_the_moss_footprint() -> None:
def test_the_lidar_tag_is_one_exact_height() -> None:
"""lidar_odometry lifts every return to exactly lidar_height_m; anything
else on the cloud (a depth camera's points, at their true height) is not
- a lidar return, even millimetres away from the scan plane."""
+ a lidar return, even millimetres away from the scan plane - it is a camera
+ obstacle, hit without a ray."""
cloud = np.array([[1.0, 0.0, np.float32(0.20)], [2.0, 0.0, 0.2005], [3.0, 0.0, 0.37],
[4.0, 0.0, 0.0]], dtype=np.float32).astype(np.float64)
assert list(lidar_returns(cloud)) == [True, False, False, False], list(lidar_returns(cloud))
@@ -255,12 +256,34 @@ def test_the_lidar_tag_is_one_exact_height() -> None:
asyncio.run(m.handle_odom(_odom(0.0, y)))
asyncio.run(m.handle_lidar(_cloud(np.vstack([_lidar_cloud(ROOM), stray]))))
assert m._grid.value_at(2.5, 0.0) == 100, "the wall is mapped"
- assert m._grid.value_at(1.0, 0.5) != 100, "the stray point is not"
+ assert m._grid.value_at(1.0, 0.5) == 100, "the off-plane point is a camera obstacle: mapped after two viewpoints"
n = m._revolutions
asyncio.run(m.handle_lidar(_cloud(stray)))
assert m._revolutions == n, "a cloud with no lidar return is not a revolution"
- print(" float32 0.20 m -> lidar; 0.2005 / 0.37 / 0.0 -> not; MOSS_LIDAR_HEIGHT_M=0.37 moves the tag; "
- "a point 2 mm off the plane never reaches the map")
+ # a camera obstacle from ONE viewpoint stays below OCCUPIED_AT, like a lidar hit
+ m2 = costmap()
+ asyncio.run(m2.handle_odom(_odom(0.0, 0.0)))
+ asyncio.run(m2.handle_explore_cmd(Bool(data=True)))
+ low = np.array([[1.5, -0.4, 0.05]]) # a shoe: 5 cm high, invisible to the scan plane
+ for _ in range(3):
+ asyncio.run(m2.handle_lidar(_cloud(np.vstack([_lidar_cloud(ROOM), low]))))
+ assert m2._grid.value_at(1.5, -0.4) != 100, "one parked viewpoint never makes a camera obstacle a wall"
+ asyncio.run(m2.handle_odom(_odom(0.0, 0.15)))
+ asyncio.run(m2.handle_lidar(_cloud(np.vstack([_lidar_cloud(ROOM), low]))))
+ assert m2._grid.value_at(1.5, -0.4) == 100, "the second viewpoint makes the shoe a wall"
+ # the lidar sees the wall behind the shoe: its rays cross the shoe's cell every other
+ # revolution, and the shoe survives (camera layer, never carved by a lidar ray)
+ for _ in range(6):
+ asyncio.run(m2.handle_lidar(_cloud(_lidar_cloud(ROOM))))
+ assert m2._grid.value_at(1.5, -0.4) == 100, "lidar rays through the shoe's cell do not erase it"
+ # the shoe is taken away: the camera sees bare floor there -> freed after two floor samples
+ floor = np.array([[1.5, -0.4, 0.0], [1.52, -0.42, 0.0]])
+ for _ in range(2):
+ asyncio.run(m2.handle_camera_floor(_cloud(floor)))
+ assert m2._grid.value_at(1.5, -0.4) != 100, "bare floor seen by the camera frees the shoe's cell"
+ # the camera point carves no ray: the floor between the rover and the shoe is not marked free by it
+ print(" float32 0.20 m -> lidar; 0.2005 / 0.37 / 0.0 -> camera; MOSS_LIDAR_HEIGHT_M=0.37 moves the tag; "
+ "a camera obstacle maps after two viewpoints (a 5 cm shoe), never from one parked spot")
def test_the_map_writes_only_during_the_mission() -> None:
@@ -402,7 +425,7 @@ def test_the_mission_signals_cross_the_process_boundary() -> None:
from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic
cm = {(s.name, s.type): s.direction for s in BlueprintAtom.create(MossCostMap, {}).streams}
- want = {("lidar", PointCloud2): "in", ("odom", PoseStamped): "in", ("explore_cmd", Bool): "in",
+ want = {("lidar", PointCloud2): "in", ("camera_floor", PointCloud2): "in", ("odom", PoseStamped): "in", ("explore_cmd", Bool): "in",
("explore_done", Bool): "in", ("stop_explore_cmd", Bool): "in",
("global_costmap", OccupancyGrid): "out"}
assert cm == want, f"stream table: extra {set(cm) - set(want)}, missing {set(want) - set(cm)}"
diff --git a/benches/bench_lidar_odometry_cold.py b/benches/bench_lidar_odometry_cold.py
index d11a9e0..0a6d957 100644
--- a/benches/bench_lidar_odometry_cold.py
+++ b/benches/bench_lidar_odometry_cold.py
@@ -338,7 +338,7 @@ check("wheel odometry is read for the log only: the pose follows the motion mode
# --- E. the optional depth path -------------------------------------------------
-print("E. depth camera (idle on MOSS): one pixel through the profile's mount")
+print("E. depth camera (live on MOSS): one pixel through the profile's mount")
o = odometry(Kiss())
o._K = (600.0, 600.0, 0.0, 0.0)
o._depth_n = LO.DEPTH_EVERY - 1 # the next frame is the one processed
diff --git a/benches/bench_moss_adapter_cold.py b/benches/bench_moss_adapter_cold.py
index 3b8029b..1774892 100644
--- a/benches/bench_moss_adapter_cold.py
+++ b/benches/bench_moss_adapter_cold.py
@@ -423,7 +423,14 @@ print()
# ── 13. full-duty pulses: a percent becomes a duty of 100 % slices ─────
print("13. pulse_tracks + the adapter pulsing on the fake (MOSS_PULSE_MS=500 -> 5 slices of 100 ms)")
-from moss_dimos.moss_adapter import pulse_tracks, pulse_ms, PULSE_RAMP # noqa: E402
+from moss_dimos.moss_adapter import pulse_tracks, pulse_ms, pulse_min_pct, pivot_tracks, pivot_pct, PULSE_RAMP # noqa: E402
+check(pivot_tracks((-8, 8), 100) == (-100, 100), "a planner pivot at 8 % -> -100/+100 (as the pad does it)")
+check(pivot_tracks((8, -8), 100) == (100, -100), "...the other way round")
+check(pivot_tracks((52, 40), 100) == (52, 40), "an arc (same signs) passes unchanged")
+check(pivot_tracks((0, 40), 100) == (0, 40), "one track stopped: not a pivot, unchanged")
+check(pivot_tracks((-8, 8), 0) == (-8, 8), "MOSS_PIVOT_PCT=0 -> as asked")
+os.environ.pop("MOSS_PIVOT_PCT", None)
+check(pivot_pct() == 100, "MOSS_PIVOT_PCT unset -> 100")
check([pulse_tracks((100, 100), k, 5) for k in range(5)] == [(100, 100)] * 5, "100 % -> on every slice")
check([pulse_tracks((20, 20), k, 5) for k in range(5)] == [(100, 100), (0, 0), (0, 0), (0, 0), (0, 0)], "20 % -> one slice of 100 % then four off")
check([pulse_tracks((50, -50), k, 5) for k in range(5)] == [(100, -100)] * 3 + [(0, 0)] * 2, "50 % -> three slices on (ceil of 2.5), sign kept per track")
@@ -440,6 +447,11 @@ os.environ.pop("MOSS_PULSE_MS", None)
check(pulse_ms() == 500, "MOSS_PULSE_MS unset -> 500 (the MOSS default: pulsing on)")
os.environ["MOSS_PULSE_MS"] = "500"
os.environ.pop("MOSS_RAMP_MS", None)
+os.environ.pop("MOSS_PULSE_MIN_PCT", None)
+check(pulse_min_pct() == 40, "MOSS_PULSE_MIN_PCT unset -> 40 (a planner pivot at 8 % is raised to 40 % of the period)")
+pd = MossBaseAdapter(dof=3, port_factory=(lambda: FakeMossFirmware()), geometry=MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=100))
+check(pd._map_geometry.min_pct == 40, f"adapter: the mapping floor is the duty floor 40 while pulsing ({pd._map_geometry.min_pct})")
+os.environ["MOSS_PULSE_MIN_PCT"] = "0" # the wire test below counts the slices of a bare 20 %
fw7 = FakeMossFirmware()
f7 = lambda: fw7 # noqa: E731
f7.is_mock = True
@@ -469,6 +481,7 @@ check(pb._pulse_n == 0 and pb._map_geometry.min_pct == 100, "MOSS_PULSE_MS=0 ->
check(pb.connect() and not any(c.startswith("ramp") for c in fw8.commands), "...and no ramp line without MOSS_RAMP_MS")
pb.disconnect()
os.environ.pop("MOSS_PULSE_MS", None)
+os.environ.pop("MOSS_PULSE_MIN_PCT", None)
print(f"{OK} OK, {KO} KO")
print("TEST PASSED" if KO == 0 else "TEST FAILED")
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index 44d475a..7c81558 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -453,9 +453,9 @@ check("base: adapter 'moss' on the QT Py by-id", hw.adapter_type == "moss" and h
check("pad: the same MOSS envelope as moss-gamepad", atom(nav, "GamepadTeleop").kwargs.get("envelope") == teleop_envelope(g))
check("odometry without the gyro prior", atom(nav, "LidarOdometry").kwargs.get("use_gyro_prior") is False)
cam = atom(nav, "RealSenseCamera").kwargs
-check("RealSense 640x480 @ 5 fps, colour only",
+check("RealSense 640x480 @ 5 fps, colour + depth (aligned), no point cloud, no IMU",
(cam.get("width"), cam.get("height"), cam.get("fps"), cam.get("enable_depth"),
- cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, False, False, False))
+ cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, True, False, False))
t = cam["base_transform"]
check("camera mount from the profile: (0.156, 0, 0.075) m",
close(t.translation.x, 0.156) and close(t.translation.z, 0.075))
diff --git a/benches/bench_moss_cockpit_cold.py b/benches/bench_moss_cockpit_cold.py
index 6845034..08bd4a9 100644
--- a/benches/bench_moss_cockpit_cold.py
+++ b/benches/bench_moss_cockpit_cold.py
@@ -8,12 +8,15 @@ Every check feeds a value we know and reads back a value in physical units
pixel <-> metre formulas, even downsampling
B. demo server - /, /static, /state.json schema, /map.png (demo size, wall =
WHITE, floor = free colour), /map_meta roundtrip, /scan.json
- (<= 1500 points, every point on a wall), /camera.mjpg framing
+ (<= 1500 points, every point on a wall, no depth-camera point),
+ /camera.mjpg framing
and its 5 fps cap, POST /cmd/goal|stop|episode|stack, path
traversal refused, bad bodies refused
C. decoding - messages built with dimos_lcm and fed to a live cockpit's bus
cache: OccupancyGrid -> exact PNG pixels, PointCloud2 -> scan
- points (and the /pointcloud fallback), PoseStamped yaw 90 deg,
+ points (and the /pointcloud fallback), the `lidar` cloud split
+ into lidar returns (z = the profile's lidar height, the stack's
+ own override first) and depth-camera points, PoseStamped yaw 90 deg,
telemetry String -> battery %, Path, explore/goal state, a JPEG
image passed through byte for byte
D. commands - POST /cmd/* in live mode publish the known bytes on the known
@@ -30,7 +33,8 @@ Every check feeds a value we know and reads back a value in physical units
cache, a camera frame after a run change, a viewer that stops
reading frees its camera slot
H. the real page - tools/moss_cockpit/ served as shipped: index.html, every asset it
- and tokens.css reference, the run caption's namespace strip
+ and tokens.css reference, the run caption's namespace strip, the
+ depth-camera legend element the map writes to
F. hygiene - no dimos/serial/torch import, no LAN address in the server
C, D and the run-change checks of G need dimos_lcm (pure python, installed with
@@ -67,6 +71,9 @@ sys.path.insert(0, str(ROOT / "tools"))
import numpy as np # noqa: E402
import moss_cockpit as mc # noqa: E402
+from moss_dimos import robot_profile # noqa: E402 - pure python, no dimOS
+
+TAG = robot_profile.current().lidar_height_m # the z of every lidar return in the `lidar` cloud
try:
import dimos_lcm # noqa: F401
@@ -340,6 +347,9 @@ def on_wall(x, y):
check("/scan.json: world frame, 1..1500 points", scan["frame"] == "world" and 0 < len(pts) <= 1500,
f"{len(pts)} pts")
check("every scan point lies on a wall cell of the demo map", all(on_wall(x, y) for x, y in pts))
+check("demo /scan.json: lidar only -> camera [], camera_n 0, n >= points",
+ scan.get("camera") == [] and scan.get("camera_n") == 0 and scan.get("n", -1) >= len(pts),
+ f"camera_n {scan.get('camera_n')} n {scan.get('n')}")
wall_below = [p for p in pts if abs(p[0] - p0["x"]) < 0.2 and p[1] < p0["y"]]
check("the ray straight down from the pose hits the outer wall at y ~ -5.4",
bool(wall_below) and abs(max(p[1] for p in wall_below) + 5.4) < 0.06,
@@ -512,18 +522,52 @@ else:
check("/lidar never seen -> /pointcloud fallback, lidar frame, (0.5, 0.25)",
scan["frame"] == "lidar_link" and scan["source"] == "pointcloud" and scan["points"] == [[0.5, 0.25]],
json.dumps(scan)[:120])
- bus.ingest(mc.CHANNELS["lidar"], cloud([(1.0, 2.0, 0.37), (3.0, -4.0, 0.37)], "world"))
+ check("/pointcloud fallback (the C1's flat scan, z = 0): every point lidar, no camera key",
+ scan.get("n") == 1 and "camera" not in scan and "camera_n" not in scan, json.dumps(scan)[:120])
+ bus.ingest(mc.CHANNELS["lidar"], cloud([(1.0, 2.0, TAG), (3.0, -4.0, TAG)], "world"))
time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)
code, scan = lcli.json("/scan.json")
check("PointCloud2 with 2 points on /lidar -> [[1, 2], [3, -4]] world",
scan["frame"] == "world" and scan["points"] == [[1.0, 2.0], [3.0, -4.0]], json.dumps(scan)[:120])
- big = [(math.cos(a), math.sin(a), 0.0) for a in np.linspace(0, 2 * math.pi, 3000)]
+ check(f"lidar returns only (z = {TAG} m) -> n 2, camera [], camera_n 0",
+ scan.get("n") == 2 and scan.get("camera") == [] and scan.get("camera_n") == 0, json.dumps(scan)[:160])
+ big = [(math.cos(a), math.sin(a), TAG) for a in np.linspace(0, 2 * math.pi, 3000)]
bus.ingest(mc.CHANNELS["lidar"], cloud(big, "world"))
time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)
code, scan = lcli.json("/scan.json")
radii = [math.hypot(x, y) for x, y in scan["points"]]
check("3000-point ring of radius 1 m -> 1500 points, all at 1 m",
len(scan["points"]) == 1500 and max(abs(r - 1.0) for r in radii) < 2e-3, f"{len(scan['points'])}")
+ check("ring: n counts the cloud (3000), before the 1500-point cap; camera_n 0",
+ scan.get("n") == 3000 and scan.get("camera_n") == 0, f"n {scan.get('n')} camera_n {scan.get('camera_n')}")
+
+ # 3 lidar returns at the tag height + 2 depth-camera points at their true height (5 cm, 60 cm)
+ mixed = [(1.0, 0.0, TAG), (0.0, 1.5, TAG), (-2.0, -0.5, TAG), (0.4, 0.1, 0.05), (0.7, -0.2, 0.6)]
+ bus.ingest(mc.CHANNELS["lidar"], cloud(mixed, "world"))
+ time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)
+ code, scan = lcli.json("/scan.json")
+ check("3 points at the lidar height + 2 at 0.05 / 0.6 m -> n 3, camera_n 2",
+ scan.get("n") == 3 and scan.get("camera_n") == 2, json.dumps(scan)[:200])
+ check("points = the 3 lidar returns' xy (the mint scan unchanged)",
+ scan["points"] == [[1.0, 0.0], [0.0, 1.5], [-2.0, -0.5]], json.dumps(scan["points"]))
+ check("camera = the 2 depth points' xy [[0.4, 0.1], [0.7, -0.2]]",
+ scan.get("camera") == [[0.4, 0.1], [0.7, -0.2]], json.dumps(scan.get("camera")))
+ near = [(1.0, 0.0, TAG), (0.5, 0.5, TAG + 0.002)] # 2 mm above the scan plane: a camera point
+ bus.ingest(mc.CHANNELS["lidar"], cloud(near, "world"))
+ time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)
+ code, scan = lcli.json("/scan.json")
+ check("a depth point 2 mm above the lidar plane is a camera point (exact tag, not a band)",
+ scan["points"] == [[1.0, 0.0]] and scan.get("camera") == [[0.5, 0.5]], json.dumps(scan)[:200])
+
+ # the stack decides the tag: its own MOSS_LIDAR_HEIGHT_M, not the cockpit's environment
+ STACK_TALL = stand_in(MOSS_LIDAR_HEIGHT_M=str(TAG + 0.05))
+ register("run-a", "moss-dimos.moss-explore", pid=STACK_TALL.pid) # same run: the bus cache stays
+ bus.ingest(mc.CHANNELS["lidar"], cloud([(1.0, 0.0, TAG + 0.05), (2.0, 0.0, TAG)], "world"))
+ time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)
+ code, scan = lcli.json("/scan.json")
+ check(f"stack started with MOSS_LIDAR_HEIGHT_M={TAG + 0.05:g}: z {TAG + 0.05:g} -> lidar, z {TAG:g} -> camera",
+ scan["points"] == [[1.0, 0.0]] and scan.get("camera") == [[2.0, 0.0]], json.dumps(scan)[:200])
+ register("run-a", "moss-dimos.moss-explore")
pose = PoseStamped(header=hdr("world"), pose=pose_of(1.25, -0.5, qz=s45, qw=s45))
for _ in range(3):
@@ -1121,6 +1165,9 @@ strip = re.search(r'\(st\.blueprint \|\| "none"\)\.replace\(/(.+?)/, ""\)', js)
pattern = strip.group(1) if strip else None
got = {bp: re.sub(pattern, "", bp, count=1) for bp in ("moss-dimos.moss-explore", "other-dimos.explore", "moss-nav")} \
if pattern else {}
+page_html = (real_page / "index.html").read_text()
+check("the depth-camera legend: app.js writes camera_n to #hud-cam, and index.html carries it",
+ 'setText("hud-cam"' in js and "camera_n" in js and 'id="hud-cam"' in page_html)
check("the run caption strips any dimOS namespace: moss-dimos.moss-explore -> moss-explore, "
"other-dimos.explore -> explore, moss-nav unchanged",
got == {"moss-dimos.moss-explore": "moss-explore", "other-dimos.explore": "explore", "moss-nav": "moss-nav"},
diff --git a/docs/cockpit.md b/docs/cockpit.md
index 65531f2..fc343b3 100644
--- a/docs/cockpit.md
+++ b/docs/cockpit.md
@@ -70,8 +70,11 @@ firmware 300 ms after the last drive.
- lidar odometry (kiss-icp, `moss_dimos/lidar_odometry.py`), the pose source:
the encoders give metres only once `MOSS_TICKS_PER_M` is set, and no IMU
feeds the odometry;
-- the 2D scored costmap (`moss_dimos/costmap2d.py`), where the lidar writes obstacles;
-- the RealSense colour stream at 640x480, 5 fps, carried as JPEG on the bus
+- the 2D scored costmap (`moss_dimos/costmap2d.py`), where the lidar writes obstacles
+ and the depth camera adds the ones below the scan plane;
+- the RealSense colour stream at 640x480, 5 fps, carried as JPEG on the bus, and its
+ depth aligned to colour at the same rate, which `lidar_odometry` turns into obstacle
+ points from 12 cm (the camera's close-range depth) to 3 m, drawn in amber on the map
(`MOSS_CAMERA=0` leaves it out; `MOSS_RS_SERIAL=<serial>` picks one camera
when several are plugged in, and the flight check looks for the same one);
- the wrist camera (the UVC camera on the arm, found by its `/dev/v4l/by-id`
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index 50ca1b6..2e0b60e 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -32,7 +32,8 @@ module that fails to open it - never to another device.
Switches (env, read when moss-nav / moss-explore is built; robot_profile.env_on
parses them: 0, false, no or off = off, any other value = on, unset = the default):
- MOSS_CAMERA=0 no RealSense (default on: colour 640x480 @ 5 fps, JPEG on the bus)
+ MOSS_CAMERA=0 no RealSense (default on: colour 640x480 @ 5 fps, JPEG on the bus, plus depth aligned
+ to colour at the same rate, turned into map obstacles by lidar_odometry)
MOSS_WRIST=0 no wrist camera (default on: the UVC camera on the arm, 640x480 @ 5 fps,
JPEG on the bus as wrist_image; absent device = warning, module skipped)
MOSS_WRIST_DEV wrist camera: a /dev/videoN path or an index, instead of the by-id lookup
@@ -397,7 +398,9 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
x, y, z, qx, qy, qz, qw = camera_mount_xyzq(profile)
cam_kwargs = dict(width=CAMERA_WIDTH, height=CAMERA_HEIGHT, fps=CAMERA_FPS,
- enable_depth=False, enable_pointcloud=False, enable_imu=False,
+ # depth on, aligned to colour: lidar_odometry turns it into the obstacles the
+ # 2D lidar cannot see (below its scan plane); the point cloud stays off (not used)
+ enable_depth=True, enable_pointcloud=False, enable_imu=False,
base_transform=Transform(translation=Vector3(x, y, z),
rotation=Quaternion(qx, qy, qz, qw)))
serial = os.environ.get("MOSS_RS_SERIAL", "").strip()
diff --git a/moss_dimos/costmap2d.py b/moss_dimos/costmap2d.py
index 5e52b79..fe202fe 100644
--- a/moss_dimos/costmap2d.py
+++ b/moss_dimos/costmap2d.py
@@ -17,10 +17,14 @@ table legs. This module replaces it for a rover with a single-plane lidar:
* what the body drove over (body_clear) is cleared: that one is a physical
certainty, not a sensor reading.
-The lidar is the only sensor that writes here. lidar_odometry publishes each
+Two sensors write here, through one cloud. lidar_odometry publishes each
revolution in `world` with every lidar return at exactly the profile's
-lidar_height_m; `lidar_returns` keeps those points and drops anything else
-the cloud may carry.
+lidar_height_m, plus the depth camera's obstacle points at their true height
+(the things below the scan plane: a shoe, a step, a chair base). `lidar_returns`
+tells them apart. Lidar returns carve their rays free and hit their cell; the
+camera points only hit their cell (a camera obstacle proves nothing about the
+floor in front of it) under the same two-viewpoint rule, so a camera obstacle
+becomes a wall on approach like a leg does.
Mission gate: the map writes only once the mission has started. A rover parked
with a spinning lidar thickens the obstacles around its one viewpoint and
@@ -84,6 +88,7 @@ class ScoredGrid:
self.ox = centre[0] - span_m / 2.0
self.oy = centre[1] - span_m / 2.0
self.lidar = np.zeros((self.n, self.n), dtype=np.int8) # what the lidar saw in its scan plane
+ self.camera = np.zeros((self.n, self.n), dtype=np.int8) # what the depth camera saw below that plane: hit by it, freed by its floor samples, never carved by a lidar ray
self.seen = np.zeros((self.n, self.n), dtype=bool)
self._last_hit_xy = np.full((self.n, self.n, 2), np.nan, dtype=np.float32)
@@ -144,6 +149,25 @@ class ScoredGrid:
self._miss(self.lidar, cells[0], cells[1])
self._hit(self.lidar, hits_xy[:, 0], hits_xy[:, 1], from_xy)
+ def camera_hits(self, hits_xy: np.ndarray, from_xy: tuple[float, float]) -> int:
+ """World-frame obstacle points from the depth camera, seen from `from_xy`:
+ a hit on their cells (two-viewpoint rule as for lidar hits), no ray carved.
+ Returns the number of cells that took a hit."""
+ if len(hits_xy) == 0:
+ return 0
+ return self._hit(self.camera, hits_xy[:, 0], hits_xy[:, 1], from_xy)
+
+ def camera_floor(self, floor_xy: np.ndarray) -> None:
+ """World-frame bare-floor samples from the depth camera: a miss on the
+ camera layer (a shoe that was taken away stops being an obstacle once
+ the camera sees the floor where it stood). The lidar layer is untouched."""
+ if len(floor_xy) == 0:
+ return
+ gx, gy = self.cell(floor_xy[:, 0], floor_xy[:, 1])
+ if len(gx):
+ flat = np.unique(gy * self.n + gx)
+ self._miss(self.camera, flat % self.n, flat // self.n)
+
def _ray_cells(self, from_xy: tuple[float, float], hits_xy: np.ndarray):
"""Cells crossed by the rays from `from_xy` to each hit (capped at
RAY_MAX_M), stopping one cell short of the hit; one sample per cell."""
@@ -185,6 +209,7 @@ class ScoredGrid:
gx, gy = self.cell((x + c * bx - s * by).ravel(), (y + s * bx + c * by).ravel())
if len(gx):
self.lidar[gy, gx] = FREE_FLOOR
+ self.camera[gy, gx] = FREE_FLOOR
self.seen[gy, gx] = True
# ---- output ------------------------------------------------------------
@@ -193,6 +218,7 @@ class ScoredGrid:
out = np.full((self.n, self.n), -1, dtype=np.int8)
out[self.seen] = 0
out[self.lidar >= OCCUPIED_AT] = 100
+ out[self.camera >= OCCUPIED_AT] = 100
return out
def value_at(self, x: float, y: float) -> int:
@@ -233,6 +259,7 @@ class MossCostMap(Module):
listen to)."""
lidar: In[PointCloud2]
+ camera_floor: In[PointCloud2] # bare floor the depth camera saw (lidar_odometry): frees camera obstacles
odom: In[PoseStamped]
explore_done: In[Bool] # explorer2 found no reachable frontier left: logged, the map keeps writing
stop_explore_cmd: In[Bool] # exploration stopped on request (explore_ctl.py stop, the cockpit): logged, the map keeps writing
@@ -246,6 +273,8 @@ class MossCostMap(Module):
self._pose_xy: tuple[float, float] | None = None
self._pose_yaw = 0.0
self._revolutions = 0
+ self._camera_pts = 0 # camera obstacle points since the last publish (log only)
+ self._camera_cells = 0 # ...and the cells they hit
self._last_clear: tuple | None = None # (x, y, yaw) of the last body_clear
# The pre-mission gate, closed at construction: a rover parked with a
# spinning lidar would write the room it waits in, from one spot. It
@@ -328,18 +357,34 @@ class MossCostMap(Module):
self._mission_frozen = False
logger.info("costmap: explore_cmd received - map writing OPEN for the mission")
+ async def handle_camera_floor(self, msg: PointCloud2) -> None:
+ if self._grid is None or self._pose_xy is None or self._write_frozen():
+ return
+ pts = np.asarray(msg.as_numpy()[0], dtype=np.float64)
+ if len(pts):
+ self._grid.camera_floor(pts[:, :2])
+
async def handle_lidar(self, msg: PointCloud2) -> None:
if self._grid is None or self._pose_xy is None or self._write_frozen():
return
pts = np.asarray(msg.as_numpy()[0], dtype=np.float64)
if len(pts) == 0:
return
- hits = pts[lidar_returns(pts)]
+ tag = lidar_returns(pts)
+ hits = pts[tag]
if len(hits) == 0:
return # no lidar return on this cloud: not a revolution
self._grid.lidar_revolution(hits[:, :2], self._pose_xy)
+ cam = pts[~tag]
+ if len(cam):
+ self._camera_cells += self._grid.camera_hits(cam[:, :2], self._pose_xy)
+ self._camera_pts += len(cam)
self._revolutions += 1
if self._revolutions % PUBLISH_EVERY == 0:
+ if self._camera_pts:
+ logger.info(f"costmap: camera obstacles: {self._camera_pts} points, {self._camera_cells} cell hits "
+ f"over the last {PUBLISH_EVERY} revolutions")
+ self._camera_pts = self._camera_cells = 0
self._publish()
def _publish(self) -> None:
diff --git a/moss_dimos/lidar_odometry.py b/moss_dimos/lidar_odometry.py
index 53e129b..0f2df97 100644
--- a/moss_dimos/lidar_odometry.py
+++ b/moss_dimos/lidar_odometry.py
@@ -27,8 +27,8 @@ RealSense built with depth on), obstacles seen by the camera are added to the
next `lidar` clouds at their true height, and the bare floor it sees goes out
on `camera_floor` (z = 0). The camera mount comes from the profile
(camera_xyz_base_m, camera_pitch_deg). The MOSS blueprints build the RealSense
-without depth, so this path is idle there, and costmap2d maps lidar returns
-only.
+with depth on (aligned to colour, CAMERA_FPS), so this path is live there and
+costmap2d maps the camera obstacles next to the lidar returns.
Streams
pointcloud : In[PointCloud2] one revolution, lidar_link (rplidar_c1.py)
@@ -71,7 +71,8 @@ logger = setup_logger()
PLANE_THICKNESS_M = 0.05
DEPTH_STRIDE = 8 # 640x480 -> 80x60 samples per frame: what the map needs, not more
-DEPTH_EVERY = 2 # one depth frame in two (15 fps -> 7.5 Hz)
+DEPTH_EVERY = 1 # every depth frame: the camera runs at 5 fps on MOSS, nothing to skip
+CAMERA_MIN_RANGE_M = 0.12 # the D455's close-range depth (Min-Z update): nearer than this is noise or the rover's own nose
DEPTH_MAX_M = 3.0 # beyond that the floor noise (1-2 % of range) leaks into the obstacle band
OBSTACLE_MAX_M = 3.0 # camera obstacles are trusted up to this range; past it, depth reflections
# on a glossy floor arm phantom obstacles. Floor samples keep the full
@@ -270,7 +271,7 @@ class LidarOdometry(Module):
h, w = d.shape
vs, us = np.mgrid[0:h:DEPTH_STRIDE, 0:w:DEPTH_STRIDE]
z = d[vs, us].astype(np.float64) / 1000.0
- ok = (z > 0.30) & (z < DEPTH_MAX_M) # < 0.30 m = the rover's own front, not the world
+ ok = (z > CAMERA_MIN_RANGE_M) & (z < DEPTH_MAX_M)
if not ok.any():
return
z, us, vs = z[ok], us[ok], vs[ok]
diff --git a/moss_dimos/moss_adapter.py b/moss_dimos/moss_adapter.py
index ab96929..423e71f 100644
--- a/moss_dimos/moss_adapter.py
+++ b/moss_dimos/moss_adapter.py
@@ -66,6 +66,8 @@ Calibration (env, all optional):
MOSS_RAMP_MS "UP DOWN" sent as 'ramp UP DOWN' after connect (while pulsing, unset = "1 1")
MOSS_PULSE_MS period of the full-duty time slicing (default 500, 0 = off): a percent becomes a duty of
full-power slices over the period (pulse_tracks), the way MOSS's tracks can be steered
+ MOSS_PULSE_MIN_PCT duty floor of a non-zero track while pulsing (default 40)
+ MOSS_PIVOT_PCT every pivot (tracks in opposite directions) is sent at this percent, both tracks (default 100, 0 = as asked)
MOSS_MOCK_BUS any value but 0/false/no/off: drive the fake firmware (no motors)
"""
from __future__ import annotations
@@ -103,6 +105,11 @@ MOCK_BUS_ENV = "MOSS_MOCK_BUS"
PULSE_MS_DEFAULT = 500 # MOSS_PULSE_MS: period of the full-duty time slicing, 0 = off (then MOSS_MIN_PCT applies)
PULSE_MS_MAX = 2000
PULSE_RAMP = "1 1" # firmware ramp while pulsing: 1 ms per %, so a 100 ms slice reaches full duty
+PIVOT_PCT_DEFAULT = 100 # MOSS_PIVOT_PCT: a pivot (tracks in opposite directions) is sent at this percent on both
+ # tracks whatever the planner asked, 0 = off. The pad pivots at 100 % continuous and the
+ # tracks turn without jolts there; the planner's slow pivots (8 %) can only be jolts
+PULSE_MIN_PCT_DEFAULT = 40 # MOSS_PULSE_MIN_PCT: the lowest duty of a non-zero track while pulsing; a planner pivot at
+ # 8 % would be one 100 ms slice per second (seen on the floor: jerky and slow), 40 % moves the rover
def pulse_ms() -> int:
@@ -110,6 +117,25 @@ def pulse_ms() -> int:
return max(0, min(PULSE_MS_MAX, int(_env_float("MOSS_PULSE_MS", PULSE_MS_DEFAULT))))
+def pulse_min_pct() -> int:
+ """MOSS_PULSE_MIN_PCT clamped to 0..100: the duty floor of the pulses."""
+ return max(0, min(100, int(_env_float("MOSS_PULSE_MIN_PCT", PULSE_MIN_PCT_DEFAULT))))
+
+
+def pivot_pct() -> int:
+ """MOSS_PIVOT_PCT clamped to 0..100: the percent every pivot is sent at (0 = as asked)."""
+ return max(0, min(100, int(_env_float("MOSS_PIVOT_PCT", PIVOT_PCT_DEFAULT))))
+
+
+def pivot_tracks(want: tuple[int, int], pct: int) -> tuple[int, int]:
+ """A pivot (left and right non-zero, opposite signs) is sent at +-pct on both
+ tracks, the sign of each kept; anything else passes unchanged."""
+ left, right = want
+ if pct <= 0 or left == 0 or right == 0 or (left > 0) == (right > 0):
+ return want
+ return (pct if left > 0 else -pct, pct if right > 0 else -pct)
+
+
def pulse_tracks(want: tuple[int, int], tick: int, n_ticks: int, full: int = 100) -> tuple[int, int]:
"""Time-slice a (left, right) percent pair into full-duty pulses.
@@ -300,8 +326,9 @@ class MossBaseAdapter:
self._pulse_n = int(round(pulse_ms() / 1000.0 / KEEPALIVE_S)) if pulse_ms() > 0 else 0
self._ramp = "" # the ramp sent at connect ("" = the firmware default)
self._tick = 0
- # the mapping geometry: when pulsing, the static-friction floor is the pulse itself
- self._map_geometry = replace(self._geometry, min_pct=0) if self._pulse_n else self._geometry
+ self._pivot_pct = pivot_pct() # pivots sent at this percent on both tracks (0 = as asked)
+ # the mapping geometry: when pulsing, the static-friction floor becomes the duty floor of the pulses
+ self._map_geometry = replace(self._geometry, min_pct=pulse_min_pct()) if self._pulse_n else self._geometry
self._last_logged = None
self._reader = None
self._keepalive = None
@@ -535,7 +562,7 @@ class MossBaseAdapter:
vx, vy, wz = float(vx), float(vy), float(wz)
except Exception:
return False
- want = twist_to_tracks(vx, wz, self._map_geometry)
+ want = pivot_tracks(twist_to_tracks(vx, wz, self._map_geometry), self._pivot_pct)
if want != (0, 0) and not self._enabled:
return False
left, right = self._slice(want)
diff --git a/tools/moss-pulse-test.sh b/tools/moss-pulse-test.sh
index 902ac2c..6736a05 100755
--- a/tools/moss-pulse-test.sh
+++ b/tools/moss-pulse-test.sh
@@ -6,7 +6,9 @@
# tools/moss-pulse-test.sh sec # 100 ms slices at full duty, hard brake between (period 500 ms)
# tools/moss-pulse-test.sh moyen # same period, release over 300 ms instead of a hard brake
# tools/moss-pulse-test.sh doux # one cycle per second, release over 500 ms
+# tools/moss-pulse-test.sh fondu # 300 ms period, release over 600 ms: the slices blend into a near-continuous run
# tools/moss-pulse-test.sh doux --from 20 --to 100 --step 20 # any floor-probe option after the preset
+# tools/moss-pulse-test.sh doux --pivot # the same steps turning on the spot
#
# The preset is two variables; the same two on the launcher give the stack the
# same profile: MOSS_PULSE_MS=<ms> MOSS_RAMP_MS="<rise> <fall>" EXPLORE=1 tools/moss-fly.sh explore
@@ -16,7 +18,8 @@ case "${1:-}" in
sec) export MOSS_PULSE_MS=500 MOSS_RAMP_MS="1 1" ;;
moyen) export MOSS_PULSE_MS=500 MOSS_RAMP_MS="1 3" ;;
doux) export MOSS_PULSE_MS=1000 MOSS_RAMP_MS="1 5" ;;
- *) echo "usage: tools/moss-pulse-test.sh sec|moyen|doux [floor-probe options]"; exit 2 ;;
+ fondu) export MOSS_PULSE_MS=300 MOSS_RAMP_MS="1 6" ;; # slices blend: the release outlasts the period, turns run faster and smoother
+ *) echo "usage: tools/moss-pulse-test.sh sec|moyen|doux|fondu [floor-probe options]"; exit 2 ;;
esac
shift
echo "preset $MOSS_PULSE_MS ms period, ramp $MOSS_RAMP_MS ms per % (rise fall) -> for the stack: MOSS_PULSE_MS=$MOSS_PULSE_MS MOSS_RAMP_MS=\"$MOSS_RAMP_MS\" EXPLORE=1 tools/moss-fly.sh explore"
diff --git a/tools/moss_cockpit.py b/tools/moss_cockpit.py
index 0ccf620..d36313b 100644
--- a/tools/moss_cockpit.py
+++ b/tools/moss_cockpit.py
@@ -8,8 +8,8 @@
Why a separate process instead of a dimOS web module: dimOS's own viewers either
need a secure context, git LFS (the command center) or cost a core with no
client connected (inflate + distance transform on every costmap). This server
-imports lcm, dimos_lcm and numpy only - never dimos, torch or open3d - and does
-nothing until a page asks: LCM callbacks keep the latest bytes and a timestamp,
+imports lcm, dimos_lcm, numpy and moss_dimos.robot_profile (pure python) only -
+never dimos, torch or open3d - and does nothing until a page asks: LCM callbacks keep the latest bytes and a timestamp,
decoding happens on request and is cached by that timestamp (map at most 2 Hz,
scan at most 4 Hz). A camera channel is subscribed only while an MJPEG client
is watching it.
@@ -26,7 +26,8 @@ Endpoints (all Cache-Control: no-store; a failing request never kills the server
GET / /static/<file> the page and its assets (tools/moss_cockpit/)
GET /state.json stack, pose, topic rates, base, nav, recording, system
GET /map.png /map_meta latest /global_costmap as a PNG + the pixel <-> metre contract
- GET /scan.json latest lidar points, world frame (fallback: lidar frame)
+ GET /scan.json latest lidar points, world frame (fallback: lidar frame), and
+ apart the depth camera's obstacle points the same cloud carries
GET /path.json planner path
GET /log.json?n=40 tail of the running stack's main.jsonl
GET /preflight.json the last saved flight check
@@ -72,6 +73,13 @@ TOOLS_DIR = os.path.dirname(os.path.abspath(__file__))
REPO_DIR = os.path.dirname(TOOLS_DIR)
if TOOLS_DIR not in sys.path:
sys.path.insert(0, TOOLS_DIR) # explore_ctl, the shared out-of-stack publisher
+if REPO_DIR not in sys.path:
+ sys.path.append(REPO_DIR) # moss_dimos, when the venv does not install it already
+
+try:
+ from moss_dimos import robot_profile # pure: the geometry the stack flies (lidar tag height)
+except Exception: # noqa: BLE001 - tools/ copied without the package: no lidar/camera split
+ robot_profile = None
try:
import lcm # the bus; absent on a laptop: live mode says so, --demo does not need it
@@ -132,7 +140,8 @@ STATE_TOPICS = ("odom", "global_costmap", "lidar", "pointcloud", "path", "cmd_ve
MAP_MIN_INTERVAL_S = 0.5 # decode + PNG the costmap at most 2 Hz, whoever asks
SCAN_MIN_INTERVAL_S = 0.25 # scan at most 4 Hz
SCAN_FALLBACK_S = 5.0 # /lidar silent this long -> show /pointcloud (lidar frame)
-MAX_SCAN_POINTS = 1500
+MAX_SCAN_POINTS = 1500 # per list: the lidar returns, and apart the depth camera's points
+LIDAR_Z_TOL_M = 1e-6 # costmap2d's rule: a lidar return sits at EXACTLY the profile's lidar_height_m
HZ_WINDOW_S = 5.0
MJPEG_PERIOD_S = 0.2 # 5 fps: enough to see where the nose points, cheap on a small board
MAX_CAMERA_CLIENTS = 4
@@ -271,23 +280,61 @@ def decode_pose(raw: bytes) -> dict:
"frame": m.header.frame_id}
-def cloud_xy(raw: bytes) -> tuple[str, float, np.ndarray]:
- """(frame, stamp, Nx2 float32 x/y) of a PointCloud2, NaN/inf rows dropped.
- Field offsets read from the message (dimOS writes x,y,z float32 at 0/4/8 with a
- 16-byte step, more when per-point extras are attached)."""
+def cloud_xyz(raw: bytes) -> tuple[str, float, np.ndarray]:
+ """(frame, stamp, Nx3 float32 x/y/z) of a PointCloud2, rows with a NaN/inf
+ coordinate dropped. Field offsets read from the message (dimOS writes x,y,z
+ float32 at 0/4/8 with a 16-byte step, more when per-point extras are attached);
+ a cloud without a z field reads z = 0."""
m = LcmPointCloud2.lcm_decode(raw)
n = int(m.width) * int(m.height)
step = int(m.point_step)
offs = {f.name: int(f.offset) for f in m.fields}
if n == 0 or step < 8:
- return m.header.frame_id, _stamp(m.header), np.zeros((0, 2), np.float32)
+ return m.header.frame_id, _stamp(m.header), np.zeros((0, 3), np.float32)
end = ">" if m.is_bigendian else "<"
- dt = np.dtype({"names": ["x", "y"], "formats": [end + "f4", end + "f4"],
- "offsets": [offs.get("x", 0), offs.get("y", 4)], "itemsize": step})
+ zoff = offs.get("z", None if offs else 8)
+ names, where = ["x", "y"], [offs.get("x", 0), offs.get("y", 4)]
+ if zoff is not None and zoff + 4 <= step:
+ names.append("z")
+ where.append(zoff)
+ dt = np.dtype({"names": names, "formats": [end + "f4"] * len(names), "offsets": where, "itemsize": step})
data = m.data if isinstance(m.data, (bytes, bytearray)) else bytes(m.data)
arr = np.frombuffer(data, dtype=dt, count=min(n, len(data) // step))
- xy = np.column_stack((arr["x"], arr["y"])).astype(np.float32)
- return m.header.frame_id, _stamp(m.header), xy[np.isfinite(xy).all(axis=1)]
+ z = arr["z"] if "z" in names else np.zeros(len(arr), np.float32)
+ xyz = np.column_stack((arr["x"], arr["y"], z)).astype(np.float32)
+ return m.header.frame_id, _stamp(m.header), xyz[np.isfinite(xyz).all(axis=1)]
+
+
+def lidar_tag_height(pid=None) -> float | None:
+ """The z every lidar return carries in the `lidar` cloud: the profile's
+ lidar_height_m as the running stack reads it, with the MOSS_* overrides of
+ its own environment (/proc/<pid>/environ, as bus_env reads the bus); with no
+ stack, or one whose environment cannot be read, the cockpit's own. None
+ when moss_dimos does not import here."""
+ if robot_profile is None:
+ return None
+ env = proc_environ(pid) if pid else None
+ return float(robot_profile.current(os.environ if env is None else env).lidar_height_m)
+
+
+def scan_points(xyz: np.ndarray, tag_z: float | None) -> dict:
+ """The point lists of /scan.json for one cloud (Nx3). With tag_z (a `lidar`
+ cloud), the lidar returns - z within LIDAR_Z_TOL_M of tag_z, the rule of
+ costmap2d.lidar_returns - go to `points` and every other point (a depth-camera
+ obstacle, at its true height) to `camera`. Without tag_z (the C1 driver's
+ flat cloud, or no profile) every point is a lidar return and `camera` is left
+ out: this cloud says nothing about the depth camera. n and camera_n count the
+ points in the cloud, before each list is capped at MAX_SCAN_POINTS."""
+ if tag_z is None:
+ lidar, camera = xyz[:, :2], None
+ else:
+ is_lidar = np.abs(xyz[:, 2].astype(np.float64) - tag_z) < LIDAR_Z_TOL_M
+ lidar, camera = xyz[is_lidar, :2], xyz[~is_lidar, :2]
+ doc = {"points": np.round(downsample(lidar).astype(float), 3).tolist(), "n": int(len(lidar))}
+ if camera is not None:
+ doc["camera"] = np.round(downsample(camera).astype(float), 3).tolist()
+ doc["camera_n"] = int(len(camera))
+ return doc
def downsample(points: np.ndarray, limit: int = MAX_SCAN_POINTS) -> np.ndarray:
@@ -1123,7 +1170,8 @@ class LiveCockpit(Cockpit):
return result
def scan(self) -> dict | None:
- self._sync_run(stack_status())
+ stack = stack_status()
+ self._sync_run(stack)
now = time.monotonic()
raw, mono = self.bus.latest("lidar")
name = "lidar"
@@ -1136,10 +1184,14 @@ class LiveCockpit(Cockpit):
cached_key, cached, at = self._scan_cache
if cached is not None and (cached_key == (name, mono) or now - at < SCAN_MIN_INTERVAL_S):
return cached
- frame, stamp, xy = cloud_xy(raw)
- pts = np.round(downsample(xy).astype(float), 3).tolist()
+ frame, stamp, xyz = cloud_xyz(raw)
+ # only the `lidar` cloud carries depth-camera points; /pointcloud is the C1's own scan
+ tag = None
+ if name == "lidar":
+ tag = lidar_tag_height(stack.get("pid") if stack.get("running") else _dimos_run_pid())
doc = {"frame": frame or ("world" if name == "lidar" else "lidar_link"), "stamp": stamp,
- "source": name, "age_s": round(now - mono, 2), "points": pts}
+ "source": name, "age_s": round(now - mono, 2)}
+ doc.update(scan_points(xyz, tag))
self._scan_cache = ((name, mono), doc, now)
return doc
@@ -1582,9 +1634,11 @@ class DemoCockpit(Cockpit):
if not self.stack["running"]:
return None
with self._lock:
- pts = downsample(self.world.scan())
+ xy = self.world.scan()
+ # the demo world is lidar only: its depth camera sees no obstacle
return {"frame": "world", "stamp": now, "source": "demo", "age_s": 0.05,
- "points": np.round(pts.astype(float), 3).tolist()}
+ "points": np.round(downsample(xy).astype(float), 3).tolist(), "n": int(len(xy)),
+ "camera": [], "camera_n": 0}
def path(self) -> dict:
self._tick()
diff --git a/tools/moss_cockpit/app.js b/tools/moss_cockpit/app.js
index 7d1894b..0775847 100644
--- a/tools/moss_cockpit/app.js
+++ b/tools/moss_cockpit/app.js
@@ -23,6 +23,7 @@ var EP_FULL_S = 300; // the bottom progress line is full after 5 min of
var RUN_FULL_S = 1800; // ... or after 30 min of run when no episode records
var CAM_RETRY_MS = 3000; // reconnect the MJPEG stream this long after it ended or failed, forever
var CAM_GRACE_MS = 6000; // a stream younger than this is not judged dead (first frame wait + a state poll)
+var DEPTH_COLOR = "rgba(255,193,94,.95)"; // amber: depth-camera obstacles, apart from the mint lidar scan
var S = {
online: false, state: null,
@@ -491,12 +492,13 @@ function drawGrid() {
function pose() { return S.state && S.state.pose; }
-function scanWorld() {
- var sc = S.scan; if (!sc || !sc.points) return [];
- if (sc.frame === "world") return sc.points;
+// key: "points" (lidar returns, the default) or "camera" (depth-camera obstacle points)
+function scanWorld(key) {
+ var sc = S.scan, pts = sc && sc[key || "points"]; if (!pts) return [];
+ if (sc.frame === "world") return pts;
var p = pose(); // lidar frame: place it at the robot
var px = p ? p.x : 0, py = p ? p.y : 0, c = Math.cos(p ? p.yaw : 0), s = Math.sin(p ? p.yaw : 0);
- return sc.points.map(function (q) { return [px + c * q[0] - s * q[1], py + s * q[0] + c * q[1]]; });
+ return pts.map(function (q) { return [px + c * q[0] - s * q[1], py + s * q[0] + c * q[1]]; });
}
// The server keeps the last scan and path however old they are: only fresh ones
@@ -537,13 +539,27 @@ function draw() {
}
// scan: mint dots, only while the lidar speaks (an old scan is not where the robot is)
- var sc = scanLive() ? scanWorld() : [], r = Math.max(1.1, Math.min(2.4, S.view.scale / 40));
+ var live = scanLive(), sc = live ? scanWorld() : [], r = Math.max(1.1, Math.min(2.4, S.view.scale / 40));
ctx.fillStyle = "rgba(108,248,199,.9)";
for (var k = 0; k < sc.length; k++) {
var X = sx(sc[k][0]), Y = sy(sc[k][1]);
if (X < -4 || Y < -4 || X > w + 4 || Y > h + 4) continue;
ctx.fillRect(X - r / 2, Y - r / 2, r, r);
}
+ // depth camera: amber round dots over the scan, larger, so a single obstacle point shows
+ var cam = live ? scanWorld("camera") : [], rc = r / 2 + 1.6;
+ if (cam.length) {
+ ctx.fillStyle = DEPTH_COLOR;
+ ctx.beginPath();
+ for (var j = 0; j < cam.length; j++) {
+ var CX = sx(cam[j][0]), CY = sy(cam[j][1]);
+ if (CX < -6 || CY < -6 || CX > w + 6 || CY > h + 6) continue;
+ ctx.moveTo(CX + rc, CY); ctx.arc(CX, CY, rc, 0, 2 * Math.PI);
+ }
+ ctx.fill();
+ }
+ var cn = S.scan && S.scan.camera_n;
+ setText("hud-cam", "camera obstacles: " + (!live ? "--" : typeof cn === "number" ? String(cn) : "no depth"));
// goal: salmon ring (dashed while it waits for confirmation)
var nav = S.state && S.state.nav;
@@ -903,7 +919,7 @@ function pollLog() {
if ("ResizeObserver" in window) new ResizeObserver(function () { resize(); if (!S.fitted) fitView(); }).observe(wrap);
window.addEventListener("resize", resize);
resize();
-camConnect();
+camConnectAll();
renderCamera();
loadPreflight();
fetchJSON("/launch.json").then(function (d) { if (d && d.running) { S.launchBusy = true; watchLaunch(); } }, function () {});
diff --git a/tools/moss_cockpit/index.html b/tools/moss_cockpit/index.html
index 10632b5..206400d 100644
--- a/tools/moss_cockpit/index.html
+++ b/tools/moss_cockpit/index.html
@@ -142,6 +142,8 @@
#map-tools .btn { background: rgba(61, 87, 106, .7); backdrop-filter: blur(2px); }
#hud { position: absolute; top: 14px; left: 0; z-index: 2; font-size: 12.5px; color: var(--ui-dim);
font-variant-numeric: tabular-nums; pointer-events: none; }
+ #hud > span { display: block; }
+ #hud-cam { margin-top: 4px; color: #FFC15E; } /* amber, the colour of the depth-camera dots on the map */
#map-empty { position: absolute; inset: 0; display: flex; align-items: center; justify-content: center;
pointer-events: none; text-align: center; }
#map-empty[hidden] { display: none; }
@@ -353,7 +355,7 @@
</div>
<div id="map-wrap" data-live>
<canvas id="map" aria-label="Map. Drag to pan, wheel or pinch to zoom, tap to send a goal when navigation runs."></canvas>
- <div id="hud"><span id="hud-xy">x -- m y -- m</span></div>
+ <div id="hud"><span id="hud-xy">x -- m y -- m</span><span id="hud-cam">camera obstacles: --</span></div>
<div id="map-tools"><button class="btn" id="b-fit" type="button">Fit</button></div>
<div id="map-empty"><p class="dim" id="map-empty-why">No map yet. Launch a stack to build one.</p></div>
<div id="goal-chip" hidden role="dialog" aria-label="Confirm goal">
diff --git a/tools/moss_floor_probe.py b/tools/moss_floor_probe.py
index 6356e4b..42c5978 100755
--- a/tools/moss_floor_probe.py
+++ b/tools/moss_floor_probe.py
@@ -85,7 +85,7 @@ def _fresh_ticks(adapter: MossBaseAdapter, timeout: float) -> list[int] | None:
def run(adapter: MossBaseAdapter, pcts: list[int], hold: float, settle: float,
- min_ticks: int, stop_at_first: bool, say=print) -> list[dict]:
+ min_ticks: int, stop_at_first: bool, say=print, pivot: bool = False) -> list[dict]:
"""Drive each step and read the ticks around it. Returns the rows; the
adapter is stopped before returning, whatever happens."""
rows: list[dict] = []
@@ -102,8 +102,10 @@ def run(adapter: MossBaseAdapter, pcts: list[int], hold: float, settle: float,
t_end = time.monotonic() + hold
ok = True
while time.monotonic() < t_end:
- # geometry: mps_at_limit 1.0 and max_pct 100, so vx 0.35 m/s = 35 %
- ok = adapter.write_velocities([pct / 100.0, 0.0, 0.0]) and ok
+ # geometry: mps_at_limit 1.0 and max_pct 100, so vx 0.35 m/s = 35 %; a pivot
+ # at p % is wz = p/100 / (track/2) (left -p, right +p)
+ twist = [0.0, 0.0, pct / 100.0 / (adapter._geometry.track_m / 2.0)] if pivot else [pct / 100.0, 0.0, 0.0]
+ ok = adapter.write_velocities(twist) and ok
time.sleep(0.2)
adapter.write_stop()
time.sleep(settle)
@@ -137,10 +139,11 @@ def main(argv=None) -> int:
ap.add_argument("--min-ticks", type=int, default=20)
ap.add_argument("--port", default=None)
ap.add_argument("--mock", action="store_true", help="the fake firmware, no motor")
+ ap.add_argument("--pivot", action="store_true", help="turn on the spot (left back, right forward) instead of driving straight")
a = ap.parse_args(argv)
pcts = plan(a.start, a.stop, a.step)
- print(f"floor probe: {len(pcts)} steps {pcts[0] if pcts else '-'}..{pcts[-1] if pcts else '-'} % "
+ print(f"floor probe ({'pivot on the spot' if a.pivot else 'straight'}): {len(pcts)} steps {pcts[0] if pcts else '-'}..{pcts[-1] if pcts else '-'} % "
f"(hold {a.hold:.1f} s, settle {a.settle:.1f} s, motion = {a.min_ticks} ticks)")
if not a.go:
print("nothing moves without --go. Put the rover on blocks or give it a clear metre ahead.")
@@ -148,6 +151,7 @@ def main(argv=None) -> int:
if not a.mock and stack_running():
print("a dimOS stack is running: stop it first (tools/moss-fly.sh stop), this tool opens the motor board itself")
return 2
+ os.environ["MOSS_PIVOT_PCT"] = "0" # the percent printed is the percent sent, pivots included
kw = {"geometry": MossGeometry(mps_at_limit=1.0, max_pct=100, min_pct=0)}
if a.mock:
os.environ["MOSS_MOCK_BUS"] = "1"
@@ -158,7 +162,7 @@ def main(argv=None) -> int:
print("cannot connect to the firmware (see the lines above)")
return 2
try:
- rows = run(adapter, pcts, a.hold, a.settle, a.min_ticks, stop_at_first=not a.all)
+ rows = run(adapter, pcts, a.hold, a.settle, a.min_ticks, stop_at_first=not a.all, pivot=a.pivot)
finally:
adapter.disconnect()
if getattr(adapter, "_pulse_n", 0):