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

odometry: the RealSense gyro as the rotation prior (MOSS_GYRO, on by default: the map drifted through one-sided full-power pivots); sounds: battery thresholds read under load (10.8 V low, 10.4 V critical, 11.2 V clear)

metrox877658d7966d
diff --git a/benches/bench_moss_adapter_cold.py b/benches/bench_moss_adapter_cold.py
index 3805654..2289427 100644
--- a/benches/bench_moss_adapter_cold.py
+++ b/benches/bench_moss_adapter_cold.py
@@ -512,24 +512,27 @@ class Turning(FakeMossFirmware):
return super()._telemetry_line()
-# one track frozen while the other turns: an arc's inner track under its floor, not a stall
+# one track frozen while the other turns: an arc's inner track under its floor is not a stall,
+# a track frozen at the HIGHER command (something holds it) is
class OneSided(FakeMossFirmware):
def _telemetry_line(self):
if self.mode == DRIVE:
- self.ticks[1] += 80
+ self.ticks[1] += 80 # only the right track turns
return super()._telemetry_line()
-fw11 = OneSided(auto_time=True)
-f11 = lambda: fw11 # noqa: E731
-f11.is_mock = True
-sc = MossBaseAdapter(dof=3, port_factory=f11, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=100, min_pct=0))
-check(sc.connect() and sc.write_enable(True), "a fake with only the right encoder turning")
-t_start = time.monotonic()
-while time.monotonic() - t_start < STALL_S + 0.6:
- sc.write_velocities([0.06, 0.0, 0.1]); settle(0.1)
-check(not sc.stalled and sc.stall_count == 0 and fw11.mode == DRIVE, f"one track frozen, the other turning: not a stall (count {sc.stall_count}, mode {fw11.mode})")
-sc.write_stop(); sc.disconnect()
+for twist, label, want_stall in (([0.06, 0.0, 0.1], "left frozen at the lower command (an arc turning left): not a stall", False),
+ ([0.06, 0.0, -0.1], "left frozen at the HIGHER command (something holds it): a stall", True)):
+ fw11 = OneSided(auto_time=True)
+ f11 = lambda fw=fw11: fw # noqa: E731
+ f11.is_mock = True
+ sc = MossBaseAdapter(dof=3, port_factory=f11, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=100, min_pct=0))
+ sc.connect(); sc.write_enable(True)
+ t_start = time.monotonic()
+ while time.monotonic() - t_start < STALL_S + 0.6 and not sc.stalled:
+ sc.write_velocities(twist); settle(0.1)
+ check((sc.stall_count == 1) == want_stall, f"{label} (count {sc.stall_count}, L/R {sc._want})")
+ sc.write_stop(); sc.disconnect()
fw10 = Turning(auto_time=True)
f10 = lambda: fw10 # noqa: E731
f10.is_mock = True
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index abbb48d..cd29fe3 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -452,11 +452,11 @@ check("lidar on the CP2102N by-id at 460800, frame lidar_link",
hw = atom(nav, "MossControlCoordinator").kwargs["hardware"][0]
check("base: adapter 'moss' on the QT Py by-id", hw.adapter_type == "moss" and hw.address == os.path.join(byid, QTPY))
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)
+check("odometry with the gyro prior (MOSS_GYRO unset = on)", atom(nav, "LidarOdometry").kwargs.get("use_gyro_prior") is True)
cam = atom(nav, "RealSenseCamera").kwargs
-check("RealSense 640x480 @ 5 fps, colour + depth (aligned), no point cloud, no IMU",
+check("RealSense 640x480 @ 5 fps, colour + depth (aligned), no point cloud, IMU on (the gyro prior)",
(cam.get("width"), cam.get("height"), cam.get("fps"), cam.get("enable_depth"),
- cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, True, False, False))
+ cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, True, False, True))
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/docs/cockpit.md b/docs/cockpit.md
index 303ae62..83a1cd1 100644
--- a/docs/cockpit.md
+++ b/docs/cockpit.md
@@ -80,6 +80,9 @@ firmware 300 ms after the last drive.
table top the rover has turned away from keeps its wall until the camera looks
at that spot again and sees nothing. The knobs are `MOSS_HIT_CAP`,
`MOSS_FREE_FLOOR` and `MOSS_RAY_MAX_M` (below);
+- the lidar odometry's rotation prior from the RealSense's gyro (`MOSS_GYRO=0` turns it
+ off): a full-power pivot turns the rover 25 degrees between two scans, more than the
+ constant-velocity prior follows, and the map drifts; the gyro says where the rover turned;
- the sound module (`moss_dimos/sounds.py`): a pack of short synthesized cues in
`assets/sounds` played through the robot user's PulseAudio session (a Bluetooth
speaker once connected, the default sink otherwise) on confirmed events only: the
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index 5851cf0..1cf9ed1 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -34,6 +34,8 @@ 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, plus depth aligned
to colour at the same rate, turned into map obstacles by lidar_odometry)
+ MOSS_GYRO=0 no gyro prior for the lidar odometry (default on, with the camera: the RealSense's IMU
+ feeds the odometry's rotation prior; without a camera there is no gyro)
MOSS_SOUNDS=0 no sound module (default on when pw-play or paplay is on the PATH: the sound pack
in assets/sounds plays boot_ready, bump_surprise, recovery_success, resume,
mission_complete, battery_low / battery_critical on the stack's events;
@@ -426,12 +428,16 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
from moss_dimos.lidar_odometry import LidarOdometry
from moss_dimos.rplidar_c1 import RPLidarC1
+ gyro = robot_profile.env_on("MOSS_GYRO", True) and robot_profile.env_on("MOSS_CAMERA", True)
parts = [
_coordinator_blueprint(_moss_base(address=ports["base"])),
_gamepad_part(),
RPLidarC1.blueprint(port=ports["lidar"], baudrate=LIDAR_BAUDRATE, frame_id="lidar_link"),
# no IMU wired: kiss-icp on its constant-velocity prior
- LidarOdometry.blueprint(use_gyro_prior=False),
+ # the rotation prior: the RealSense's gyro when MOSS_GYRO is on (default), which holds the
+ # registration through a full-power pivot (254 deg/s measured on the floor: 25 deg between
+ # two scans, where the constant-velocity prior let the map drift); constant velocity otherwise
+ LidarOdometry.blueprint(use_gyro_prior=gyro),
MossCostMap.blueprint(),
]
if robot_profile.env_on("MOSS_CAMERA", True):
@@ -444,7 +450,7 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
cam_kwargs = dict(width=CAMERA_WIDTH, height=CAMERA_HEIGHT, fps=CAMERA_FPS,
# 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,
+ enable_depth=True, enable_pointcloud=False, enable_imu=gyro,
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/moss_adapter.py b/moss_dimos/moss_adapter.py
index 480767b..edf72b0 100644
--- a/moss_dimos/moss_adapter.py
+++ b/moss_dimos/moss_adapter.py
@@ -701,13 +701,17 @@ class MossBaseAdapter:
want_l, want_r = self._want
stalled_l = want_l != 0 and abs(l_new - l_old) < STALL_MIN_TICKS
stalled_r = want_r != 0 and abs(r_new - r_old) < STALL_MIN_TICKS
- commanded = [stalled for want, stalled in ((want_l, stalled_l), (want_r, stalled_r)) if want != 0]
- # a stall is the rover pushing against something: EVERY commanded track frozen. One
- # track frozen while the other turns is an arc whose inner track sits under its
- # friction floor, or a tread dragging on a step: the rover still moves, the planner's
- # own stuck detection owns that case, and a false bump would back it off for nothing.
- if not commanded or not all(commanded):
+ if not (stalled_l or stalled_r):
return False
+ # Both commanded tracks frozen = the rover pushes against something. One track frozen
+ # while the other turns is a contact only when the frozen track was commanded at least
+ # as hard as the moving one (it should have turned at least as much: something holds
+ # it); frozen at the LOWER command it is an arc's inner track under its friction floor,
+ # which still moves the rover and is the planner's stuck detection's case, not a bump.
+ if stalled_l != stalled_r:
+ frozen_cmd, moving_cmd = (abs(want_l), abs(want_r)) if stalled_l else (abs(want_r), abs(want_l))
+ if frozen_cmd < moving_cmd:
+ return False
self._send("stop")
self._stall_count += 1
self._stalled = True
diff --git a/moss_dimos/sounds.py b/moss_dimos/sounds.py
index 6203e5b..e42d662 100644
--- a/moss_dimos/sounds.py
+++ b/moss_dimos/sounds.py
@@ -50,9 +50,10 @@ logger = setup_logger()
ASSETS_DIR = Path(__file__).resolve().parent.parent / "assets" / "sounds"
BACKENDS = ("paplay", "pw-play") # PulseAudio first: on the Jetson the Bluetooth speaker is a PulseAudio sink (module-bluez5),
# PipeWire there has no Bluetooth; MOSS_SOUND_BACKEND forces one of the two
-BATTERY_LOW_MV = 11000 # 3S pack: low below this at rest...
-BATTERY_LOW_CLEAR_MV = 11300 # ...and back to normal above this (hysteresis: a sag under load does not toggle it)
-BATTERY_CRITICAL_MV = 10600 # the flight check refuses below 10.8 V; critical is lower still
+BATTERY_LOW_MV = 10800 # 3S pack, read UNDER LOAD (the telemetry comes while the tracks turn: a half-full
+ # pack sags to about 11.0 V): low below this...
+BATTERY_LOW_CLEAR_MV = 11200 # ...and back to normal above this (hysteresis: one sag does not toggle it)
+BATTERY_CRITICAL_MV = 10400 # the flight check refuses to launch below 10.8 V at rest; critical under load is lower
BATTERY_SAMPLES = 6 # consecutive telemetry lines (3 s at 2 Hz) below a threshold before it counts
RECOVERY_WINDOW_S = 20.0 # a stall followed by motion inside this window = a recovery
RECOVERY_MIN_TICKS = 100 # encoder ticks that count as "moving again" after a stall