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

lidar odometry: deskew is opt-in (MOSS_DESKEW=1). On the rover kiss-icp's deskew fails at its first thread (pthread_create: Invalid argument), the odometry loop dies with it and the launcher stops the unproven stack; the default is the proven path

metrox5155ca950c95
diff --git a/benches/bench_lidar_odometry_cold.py b/benches/bench_lidar_odometry_cold.py
index 8ff936c..e25ef34 100644
--- a/benches/bench_lidar_odometry_cold.py
+++ b/benches/bench_lidar_odometry_cold.py
@@ -230,10 +230,12 @@ finally:
LO.Module.start = real_module_start
LO.logger = real_logger
cfg = getattr(o._kiss, "cfg", None)
-check("start() builds kiss-icp: max 12 m, min 0.35 m, deskew on, voxel 0.05 m, 30 points/voxel, threshold 0.3 m, min motion 0.02 m",
+check("start() builds kiss-icp: max 12 m, min 0.35 m, deskew OFF by default, voxel 0.05 m, 30 points/voxel, threshold 0.3 m, min motion 0.02 m",
cfg is not None and (cfg.data.max_range, cfg.data.min_range, cfg.data.deskew, cfg.mapping.voxel_size,
cfg.mapping.max_points_per_voxel, cfg.adaptive_threshold.initial_threshold,
- cfg.adaptive_threshold.min_motion_th) == (12.0, 0.35, True, 0.05, 30, 0.3, 0.02))
+ cfg.adaptive_threshold.min_motion_th) == (12.0, 0.35, False, 0.05, 30, 0.3, 0.02))
+check("deskew=True reaches the kiss-icp config (MOSS_DESKEW=1 in the blueprint)",
+ LO.LidarOdometry(deskew=True).deskew is True and LO.LidarOdometry().deskew is False)
up = [line for line in spy.lines if line.startswith("lidar odometry up")]
check("start() logs 'lidar odometry up (kiss-icp, ...' once (the launcher greps this prefix)",
len(up) == 1 and up[0].startswith("lidar odometry up (kiss-icp,") and up[0].endswith("-> world"),
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index 6fb147d..516eb4e 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -453,6 +453,7 @@ 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 (MOSS_GYRO unset = off)", atom(nav, "LidarOdometry").kwargs.get("use_gyro_prior") is False)
+check("odometry without deskew (MOSS_DESKEW unset = off: on the rover the deskew thread fails)", atom(nav, "LidarOdometry").kwargs.get("deskew") is False)
cam = atom(nav, "RealSenseCamera").kwargs
check("RealSense 640x480 @ 5 fps, colour + depth (aligned), no point cloud, no IMU (MOSS_GYRO off by default)",
(cam.get("width"), cam.get("height"), cam.get("fps"), cam.get("enable_depth"),
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index f23e6a7..c0ffd29 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -36,6 +36,8 @@ parses them: 0, false, no or off = off, any other value = on, unset = the defaul
to colour at the same rate, turned into map obstacles by lidar_odometry)
MOSS_GYRO=1 gyro prior for the lidar odometry from the RealSense's IMU (default OFF: the IMU
pipeline fails to open on the rover and takes the camera down with it; an experiment)
+ MOSS_DESKEW=1 kiss-icp straightens each lidar revolution by the motion prior (default OFF: on the
+ rover its first thread fails and the odometry stops; an experiment)
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;
@@ -432,6 +434,9 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
# which fails with 'No device connected' on the rover (the colour and depth streams then never
# start either), so the gyro prior is OFF unless asked for
gyro = robot_profile.env_on("MOSS_GYRO", False) and robot_profile.env_on("MOSS_CAMERA", True)
+ # MOSS_DESKEW=1 is an experiment too: kiss-icp's deskew (each point moved by its share of the motion
+ # prior) dies on the rover at its first thread, and the odometry loop with it
+ deskew = robot_profile.env_on("MOSS_DESKEW", False)
parts = [
_coordinator_blueprint(_moss_base(address=ports["base"])),
_gamepad_part(),
@@ -440,7 +445,7 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
# 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),
+ LidarOdometry.blueprint(use_gyro_prior=gyro, deskew=deskew),
MossCostMap.blueprint(),
]
if robot_profile.env_on("MOSS_CAMERA", True):
diff --git a/moss_dimos/lidar_odometry.py b/moss_dimos/lidar_odometry.py
index 5212f12..e1e10b3 100644
--- a/moss_dimos/lidar_odometry.py
+++ b/moss_dimos/lidar_odometry.py
@@ -147,7 +147,7 @@ class LidarOdometry(Module):
def __init__(self, max_range_m: float = 12.0, min_range_m: float = 0.35,
voxel_size_m: float = 0.05, initial_threshold_m: float = 0.3,
- gyro_axis: str = "-y", use_gyro_prior: bool = True, deskew: bool = True,
+ gyro_axis: str = "-y", use_gyro_prior: bool = True, deskew: bool = False,
world_frame: str = "world",
base_frame: str = "base_link", lidar_frame: str = "lidar_link",
log_every_s: float = 2.0, **kwargs: Any) -> None:
@@ -160,7 +160,9 @@ class LidarOdometry(Module):
# yaw_rate = -gyro.y. A camera mounted upside down flips the sign.
self.gyro_axis = gyro_axis
self.use_gyro_prior = use_gyro_prior
- self.deskew = deskew # kiss-icp straightens each revolution by the motion prior (sweep_times): a full-power pivot bends a 10 Hz scan by 25 degrees
+ self.deskew = deskew # kiss-icp straightens each revolution by the motion prior (sweep_times): a full-power pivot bends a 10 Hz
+ # scan by 25 degrees. OFF by default: on the rover kiss-icp's deskew dies at its first thread
+ # (pthread_create: Invalid argument) and the odometry with it; MOSS_DESKEW=1 is the experiment
self.world_frame, self.base_frame, self.lidar_frame = world_frame, base_frame, lidar_frame
self.log_every_s = log_every_s
# dimOS pickles module instances into their worker: no lock, no native