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
31.4 KB
Raw Blame
"""Lidar odometry for MOSS: KISS-ICP scan-to-map on the RPLIDAR C1.

dimOS ships no odometry for a 2D lidar, so this module supplies the pose. Each
revolution from RPLidarC1 (a flat cloud in `lidar_link`) is registered against
kiss-icp's local map, and the result is projected on SE(2) and published.

A planar scan leaves z/roll/pitch unobservable for a 3D ICP: the cloud is
thickened (copies at +-PLANE_THICKNESS_M) and the pose is projected on SE(2).

The prior of each registration is kiss-icp's own constant-velocity model, or a
gyro's yaw rate when `use_gyro_prior` is on and `imu` messages arrive. The
wheel (track) odometry never feeds the prior: tracks slip in every turn, and a
scan stamped at a slipping pose doubles the walls. It is only logged, as a
sanity signal. Rotation without a gyro is the known weak spot: on C1 scans of
~340 points, kiss-icp alone was measured under-estimating turns by 20-35 %.
Bench the turn tracking before trusting long runs.

A contact shakes the lidar: the revolution swept during it comes in with a
fraction of its points and registers at a wrong heading (measured: +6.4 deg
in one revolution of 97 points where the gyro moved +0.5 deg), and every
later revolution keeps that heading. So a contact on `bump` (the IMU's
BumpImu, the base's stall guard, the explorer's back-off) is remembered: a
revolution within MOSS_CONTACT_WINDOW_S of it that carries fewer than
MOSS_CONTACT_DROP_RATIO times the running median point count is not
registered (the pose is carried by the gyro, or held without it, and its
cloud never reaches the map), and the next MOSS_RELOC_SWEEPS registered
revolutions search with their sigma times MOSS_RELOC_SIGMA_GAIN.

Frames and heights:
  * `world` is kiss-icp's own frame: every run starts at the origin, facing +x.
  * `odom` is published on the floor plane (z = 0) - see handle_pointcloud.
  * the lidar returns are lifted to exactly the profile's lidar_height_m, in
    the `lidar` cloud and in the base_link->lidar_link tf. costmap2d recognises
    lidar returns by that exact height, so both read the same profile value.

Optional depth camera: when `depth_image` and `camera_info` are fed (a
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
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)
  imu                     : In[Imu]          gyro (angular_velocity) for the optional rotation prior
  bump                    : In[Bool]         a contact: the shaken revolution after it is dropped
  coordinator_joint_state : In[JointState]   base/vx, base/vy, base/wz positions = wheel odometry, logged only
  depth_image             : In[Image]        optional depth camera, DEPTH16 mm, aligned to colour
  camera_info             : In[CameraInfo]   its colour intrinsics
  odom   : Out[PoseStamped]  base pose in `world`, z = 0
  lidar  : Out[PointCloud2]  the revolution re-expressed in `world` (costmap2d input)
  camera_floor : Out[PointCloud2]  world-frame floor samples from the depth camera, z = 0
  tf     : Out[TFMessage]    world->base_link, base_link->lidar_link
"""
from __future__ import annotations

import math
import os
import threading
import time
from typing import Any

import numpy as np

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.Transform import Transform
from dimos.msgs.geometry_msgs.Vector3 import Vector3
from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo
from dimos.msgs.sensor_msgs.Image import Image
from dimos.msgs.sensor_msgs.Imu import Imu
from dimos.msgs.sensor_msgs.JointState import JointState
from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
from dimos.msgs.tf2_msgs.TFMessage import TFMessage
from dimos.utils.logging_config import setup_logger
from dimos_lcm.std_msgs import Bool   # the SAME Bool class as BumpImu / the coordinator / the costmap: one 'bump' stream type

from moss_dimos.robot_profile import RobotProfile, current as _current_profile

logger = setup_logger()

PLANE_THICKNESS_M = 0.05
DEPTH_STRIDE = 8               # 640x480 -> 80x60 samples per frame: what the map needs, not more
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
                               # DEPTH_MAX_M range: erasing stays cheap.
OBSTACLE_Z_M = (0.12, 1.30)    # world z band the camera turns into floor obstacles. Upper bound 1.30, not 0.70: a table top is a BLOCK - the rover goes around tables, never between the legs


def _env_float(name: str, default: float, lo: float, hi: float) -> float:
    """A number from the environment, read once at import. Unset or empty ->
    `default`; not a number -> `default`, said in the log; outside [lo, hi]
    -> the nearest bound, said in the log."""
    raw = os.environ.get(name, "").strip()
    if not raw:
        return default
    try:
        value = float(raw)
    except ValueError:
        logger.warning(f"lidar odometry: {name}={raw!r} is not a number - using {default}")
        return default
    if not lo <= value <= hi:
        bounded = hi if value > hi else lo      # below lo, or nan
        logger.warning(f"lidar odometry: {name}={raw!r} is outside [{lo}, {hi}] - using {bounded}")
        return bounded
    return value


# A contact (`bump`) shakes the lidar: the revolution swept during it comes in thin and
# registers at a wrong heading, and the map jumps. These four decide what happens next.
CONTACT_WINDOW_S = _env_float("MOSS_CONTACT_WINDOW_S", 0.4, 0.0, 5.0)        # a revolution this soon after a contact may be the shaken one
CONTACT_DROP_RATIO = _env_float("MOSS_CONTACT_DROP_RATIO", 0.5, 0.0, 1.0)    # ... and is dropped under this share of the median point count (0 = never)
RELOC_SWEEPS = int(round(_env_float("MOSS_RELOC_SWEEPS", 5, 0, 50)))        # registered revolutions after a contact that search wider (0 = none)
RELOC_SIGMA_GAIN = _env_float("MOSS_RELOC_SIGMA_GAIN", 2.0, 1.0, 10.0)      # their sigma (kernel, and 3 sigma of correspondence distance) times this
COUNT_HISTORY = 20             # registered revolutions in the running median of the point count


def _profile() -> RobotProfile:
    """The geometry the tf, the lidar lift and the depth projection use, read
    at call time so a MOSS_* override takes effect without a restart."""
    return _current_profile()


def _yaw_quat(yaw: float) -> Quaternion:
    return Quaternion(0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))


def _se2(dx: float, dy: float, dyaw: float) -> np.ndarray:
    c, s = math.cos(dyaw), math.sin(dyaw)
    return np.array([[c, -s, 0.0, dx], [s, c, 0.0, dy], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])


def sweep_times(pts: np.ndarray) -> np.ndarray:
    """When, within one revolution, each point was taken: 0 for the first, 1 for
    the last, from the unwrapped bearing of the points in scan order (the C1
    publishes a revolution in sweep order; its bearing runs one way round). A
    revolution whose bearings do not progress (fewer than a few degrees of span)
    falls back to the point order. kiss-icp's deskew moves each point by that
    fraction of the motion prior, so a scan bent by a fast pivot comes straight."""
    n = len(pts)
    if n < 2:
        return np.zeros(n)
    ang = np.unwrap(np.arctan2(pts[:, 1], pts[:, 0]))
    span = ang[-1] - ang[0]
    if abs(span) < math.radians(5.0):
        return np.linspace(0.0, 1.0, n)
    t = (ang - ang[0]) / span
    return np.clip(t, 0.0, 1.0)


def split_floor_and_obstacles(bx: np.ndarray, by: np.ndarray, bz: np.ndarray,
                              floor_z: np.ndarray, cell_m: float = 0.05) -> tuple[np.ndarray, np.ndarray]:
    """Masks (obstacle, floor) over base-frame depth points.

    Obstacle = above the floor threshold and below OBSTACLE_Z_M[1] (1.30 m:
    table tops included, so a table is a block the rover drives around).
    Floor = at floor level. Under a table the camera sees BOTH the top and
    the floor between the legs on the same ground cell; the obstacle wins,
    so floor samples on a cell that also holds an obstacle are dropped -
    otherwise hit and miss cancel and the table vanishes from the costmap.
    """
    obst = (bz > floor_z) & (bz < OBSTACLE_Z_M[1])
    floor = (bz <= floor_z) & (bz > -0.10)
    if obst.any() and floor.any():
        keys_o = set(map(tuple, np.floor(np.stack([bx[obst], by[obst]], 1) / cell_m).astype(np.int64)))
        fk = np.floor(np.stack([bx, by], 1) / cell_m).astype(np.int64)
        under = np.array([tuple(k) in keys_o for k in fk])
        floor = floor & ~under
    return obst, floor


class LidarOdometry(Module):
    pointcloud: In[PointCloud2]
    imu: In[Imu]
    bump: In[Bool]                  # a contact (BumpImu, the stall guard, the explorer's back-off): the shaken revolution is dropped
    coordinator_joint_state: In[JointState]
    depth_image: In[Image]          # depth camera (aligned to colour), DEPTH16 mm
    camera_info: In[CameraInfo]     # colour intrinsics (= aligned depth intrinsics)
    odom: Out[PoseStamped]
    lidar: Out[PointCloud2]
    camera_floor: Out[PointCloud2]   # world-frame floor samples (z = 0) the depth camera saw bare
    tf: Out[TFMessage]

    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 = False,
                 world_frame: str = "world",
                 base_frame: str = "base_link", lidar_frame: str = "lidar_link",
                 log_every_s: float = 2.0, **kwargs: Any) -> None:
        super().__init__(**kwargs)
        self.max_range_m, self.min_range_m, self.voxel_size_m = max_range_m, min_range_m, voxel_size_m
        self.initial_threshold_m = initial_threshold_m
        # which IMU axis carries the robot's yaw rate, with its sign. A D455F
        # motion module reports in the camera's optical frame (x right, y
        # down, z forward): a level camera looking forward gives
        # 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. 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
        # object before start().
        self._kiss: Any = None
        self._dropped = 0                  # revolutions lost to a registration that failed twice
        self._lock: Any = None
        self._n = 0
        self._last_log = 0.0
        self._last_ms = 0.0
        self.pose2d = (0.0, 0.0, 0.0)
        # gyro integration since the last scan (+ totals per axis for calibration)
        self._gyro_last_ts: float | None = None
        self._gyro_acc = 0.0
        self._gyro_seen = False
        self._gyro_totals = np.zeros(3)
        self._wheel: tuple[float, float, float] | None = None   # latest wheel odometry: logged only
        self._prior_used = "cv"
        self._K: tuple[float, float, float, float] | None = None
        self._pose_hist: list[tuple[float, float, float, float]] = []   # (wall ts, x, y, yaw), last ~3 s
        self._depth_n = 0
        self._depth_pts_last = 0
        self._pending_cam_pts = None
        # contacts: the last one's monotonic stamp, the point counts of the last
        # registered revolutions, and the relocation that follows a contact
        self._contact_t: float | None = None
        self._dropped_contact = 0          # revolutions dropped as shaken by a contact
        self._counts: list[int] = []
        self._reloc_armed_t: float | None = None   # the contact stamp that armed the current relocation
        self._reloc_left = 0               # registered revolutions still to search wider
        self._reloc_n = 0                  # ... done so far in this relocation
        self._reloc_yaw = 0.0              # the yaw the registration corrected over them, rad
        self._reloc_announce = False

    # ── lifecycle ──────────────────────────────────────────────────────
    @rpc
    def start(self) -> None:
        super().start()
        from kiss_icp.config import KISSConfig
        from kiss_icp.kiss_icp import KissICP
        cfg = KISSConfig()
        cfg.data.max_range = self.max_range_m
        cfg.data.min_range = self.min_range_m
        cfg.data.deskew = self.deskew
        cfg.mapping.voxel_size = self.voxel_size_m
        cfg.mapping.max_points_per_voxel = 30
        # kiss-icp defaults are tuned for cars (2 m initial threshold)
        cfg.adaptive_threshold.initial_threshold = self.initial_threshold_m
        cfg.adaptive_threshold.min_motion_th = 0.02
        self._kiss = KissICP(cfg)
        self._lock = threading.Lock()
        self._warm_up()
        logger.info(f"lidar odometry up (kiss-icp, voxel {self.voxel_size_m} m, range "
                    f"{self.min_range_m}-{self.max_range_m} m, prior: gyro={self.use_gyro_prior} "
                    f"axis {self.gyro_axis}, wheels NEVER) -> {self.world_frame}")
        logger.info(f"lidar odometry contacts: a revolution within {CONTACT_WINDOW_S:g} s of a bump under "
                    f"{CONTACT_DROP_RATIO:g} x the median point count is dropped; then {RELOC_SWEEPS} "
                    f"revolutions register with sigma x{RELOC_SIGMA_GAIN:g}")

    @rpc
    def stop(self) -> None:
        super().stop()

    # ── registration, with the first-call failure absorbed ──────────────
    def _warm_up(self) -> None:
        """One registration on a synthetic ring before the first scan. On this
        board the first call into kiss-icp's registration can fail once with
        'pthread_create has failed: Invalid argument' (its thread pool starting
        up); the next call works. Taking that failure here keeps the first real
        revolution and keeps the error out of the run log. The map is not
        touched (registration only)."""
        k = self._kiss
        ang = np.linspace(0.0, 2.0 * math.pi, 64, endpoint=False)
        ring = np.stack([2.0 * np.cos(ang), 2.0 * np.sin(ang), np.zeros_like(ang)], 1)
        try:
            source, _ = k.voxelize(ring)
            sigma = k.adaptive_threshold.get_threshold()
            self._register(k, source, np.eye(4), sigma, where="warm-up")
        except Exception as exc:  # noqa: BLE001 - a warm-up never stops the module
            logger.warning(f"lidar odometry: registration warm-up skipped ({type(exc).__name__}: {exc})")

    @staticmethod
    def _register(k, source, initial_guess, sigma, where: str = "scan"):
        """align_points_to_map with ONE retry: a RuntimeError from the first
        attempt (thread pool start-up) is logged and the call repeated; a second
        failure propagates to the caller, which drops that revolution."""
        try:
            return k.registration.align_points_to_map(
                points=source, voxel_map=k.local_map, initial_guess=initial_guess,
                max_correspondance_distance=3 * sigma, kernel=sigma)
        except RuntimeError as exc:
            logger.warning(f"lidar odometry: registration failed once at {where} ({exc}) - retrying")
            return k.registration.align_points_to_map(
                points=source, voxel_map=k.local_map, initial_guess=initial_guess,
                max_correspondance_distance=3 * sigma, kernel=sigma)

    # ── priors ─────────────────────────────────────────────────────────
    async def handle_imu(self, msg: Imu) -> None:
        w = msg.angular_velocity
        ts = float(getattr(msg, "ts", 0.0) or time.time())
        if self._gyro_last_ts is not None:
            dt = ts - self._gyro_last_ts
            if 0.0 < dt < 0.1:
                v = np.array([w.x, w.y, w.z], dtype=float)
                self._gyro_totals += v * dt
                axis = {"x": 0, "y": 1, "z": 2}[self.gyro_axis[-1]]
                sign = -1.0 if self.gyro_axis.startswith("-") else 1.0
                self._gyro_acc += sign * v[axis] * dt
                self._gyro_seen = True
        self._gyro_last_ts = ts

    async def handle_bump(self, msg: Bool) -> None:
        """A contact: stamped, nothing else here. The next revolutions read the
        stamp (handle_pointcloud): the shaken one is dropped, the relocation
        is armed."""
        if bool(getattr(msg, "data", False)):
            self._contact_t = time.monotonic()

    async def handle_coordinator_joint_state(self, msg: JointState) -> None:
        names = list(msg.name); pos = list(msg.position)
        try:
            ix, iy, ith = (names.index(n) for n in ("base/vx", "base/vy", "base/wz"))
        except ValueError:
            return
        self._wheel = (float(pos[ix]), float(pos[iy]), float(pos[ith]))

    async def handle_camera_info(self, msg: CameraInfo) -> None:
        K = list(msg.K or [])
        if len(K) == 9 and K[0] > 0:
            self._K = (float(K[0]), float(K[4]), float(K[2]), float(K[5]))

    async def handle_depth_image(self, msg: Image) -> None:
        """Sparse obstacle cloud from the depth image, in `world`, onto `lidar`.

        The 2D lidar sees one plane: it misses objects lower than its scan and
        thin dark legs. The depth camera sees them: every DEPTH_STRIDE-th pixel
        is back-projected, placed in the world with the pose at capture time,
        cropped to the rover's height band, voxel-deduplicated and attached to
        the next `lidar` clouds."""
        self._depth_n += 1
        if self._K is None or self._depth_n % DEPTH_EVERY:
            return
        d = np.asarray(msg.data)
        if d.ndim == 3:
            d = d[..., 0]
        fx, fy, cx, cy = self._K
        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 > CAMERA_MIN_RANGE_M) & (z < DEPTH_MAX_M)
        if not ok.any():
            return
        z, us, vs = z[ok], us[ok], vs[ok]
        xo = (us - cx) * z / fx          # optical x (right)
        yo = (vs - cy) * z / fy          # optical y (down)
        # optical (x right, y down, z forward) -> base (x forward, y left, z up),
        # camera pitched down by the profile's camera_pitch_deg: forward axis =
        # (cos, 0, -sin), down axis = (-sin, 0, -cos); then the mount offset
        prof = _profile()
        pitch = math.radians(prof.camera_pitch_deg)
        cam = prof.camera_xyz_base_m
        cp, sp = math.cos(pitch), math.sin(pitch)
        bx = z * cp - yo * sp + cam[0]
        by = -xo + cam[1]
        bz = -z * sp - yo * cp + cam[2]
        # floor threshold grows with range (depth noise ~1-2 % of range): a 5 cm
        # chair base is an obstacle at 1 m, floor noise is not an obstacle at 3 m
        floor_z = 0.03 + 0.03 * np.clip(bx - 1.0, 0.0, None)
        obst, floor = split_floor_and_obstacles(bx, by, bz, floor_z)
        if not obst.any() and not floor.any():
            return
        fx, fy = bx[floor], by[floor]          # bare floor: published separately, z = 0
        near = np.hypot(bx, by) < OBSTACLE_MAX_M
        bx, by, bz = bx[obst & near], by[obst & near], bz[obst & near]
        # pose at the frame's capture time: the depth handler runs 50-200 ms
        # late, and at 17 deg/s the latest pose would smear the camera points
        # by several degrees per frame
        fts = float(getattr(msg, "ts", 0.0) or 0.0)
        pose = self._pose_at(fts) if fts > 0 else self.pose2d
        x, y, yaw = pose
        c, s_ = math.cos(yaw), math.sin(yaw)
        if len(fx):
            fwx, fwy = c * fx - s_ * fy + x, s_ * fx + c * fy + y
            fpts = np.stack([fwx, fwy, np.zeros_like(fwx)], axis=1)
            fkeys = np.floor(fpts / 0.05).astype(np.int64)
            _, fidx = np.unique(fkeys, axis=0, return_index=True)
            self.camera_floor.publish(PointCloud2.from_numpy(fpts[fidx].astype(np.float32), frame_id=self.world_frame, timestamp=time.time()))
        if len(bx) == 0:
            return
        wx, wy = c * bx - s_ * by + x, s_ * bx + c * by + y
        pts = np.stack([wx, wy, bz], axis=1)
        # voxel-deduplicate at the map resolution
        keys = np.floor(pts / 0.05).astype(np.int64)
        _, idx = np.unique(keys, axis=0, return_index=True)
        pts = pts[idx].astype(np.float32)
        self._depth_pts_last = len(pts)
        # attached to EVERY lidar revolution until replaced (0.5 s cap): the
        # 10 Hz lidar against 7.5 Hz depth would otherwise leave one revolution
        # in four without camera points, and the camera points would blink.
        self._pending_cam_pts = (pts, time.monotonic())

    def _pose_at(self, ts: float) -> tuple[float, float, float]:
        """Pose interpolated at wall time ts from the recent history (else the latest)."""
        h = self._pose_hist
        if len(h) < 2 or ts <= h[0][0]:
            return self.pose2d if not h else (h[0][1], h[0][2], h[0][3])
        if ts >= h[-1][0]:
            return (h[-1][1], h[-1][2], h[-1][3])
        for a, b in zip(h, h[1:]):
            if a[0] <= ts <= b[0]:
                f = (ts - a[0]) / max(b[0] - a[0], 1e-6)
                dyaw = math.atan2(math.sin(b[3] - a[3]), math.cos(b[3] - a[3]))
                return (a[1] + f * (b[1] - a[1]), a[2] + f * (b[2] - a[2]), a[3] + f * dyaw)
        return self.pose2d

    def _prior_delta(self) -> np.ndarray:
        """Motion since the last scan, in the previous lidar frame (SE(2) as 4x4)."""
        last_delta = np.asarray(self._kiss.last_delta)
        dx, dy = float(last_delta[0, 3]), float(last_delta[1, 3])
        dyaw = math.atan2(last_delta[1, 0], last_delta[0, 0])
        used = []
        # the wheels are NEVER consulted here - see the module docstring
        if self.use_gyro_prior and self._gyro_seen:
            dyaw = self._gyro_acc
            used.append("gyro")
        self._prior_used = "+".join(used) or "cv"
        return _se2(dx, dy, dyaw)

    # ── contacts ───────────────────────────────────────────────────────
    def _arm_relocation(self) -> None:
        """A contact not seen yet arms the relocation: the next RELOC_SWEEPS
        registered revolutions search wider. A contact during a relocation
        starts its count again; the yaw correction keeps adding up."""
        stamp = self._contact_t
        if stamp is None or stamp == self._reloc_armed_t:
            return
        self._reloc_armed_t = stamp
        if RELOC_SWEEPS > 0:
            self._reloc_left = RELOC_SWEEPS
            self._reloc_announce = True

    def _shaken(self, n_pts: int, t0: float) -> float | None:
        """The running median point count when this revolution is a shaken
        sweep to drop (within CONTACT_WINDOW_S of a contact, under
        CONTACT_DROP_RATIO times the median of the last registered ones),
        else None."""
        stamp = self._contact_t
        if stamp is None or t0 - stamp > CONTACT_WINDOW_S or not self._counts:
            return None
        median = float(np.median(self._counts))
        return median if n_pts < CONTACT_DROP_RATIO * median else None

    def _carry(self, k, n_pts: int, median: float) -> np.ndarray:
        """The pose for a dropped revolution: the last one turned by the gyro
        integral when the prior is on and fed, else the last one unchanged.
        kiss-icp's last pose follows it (the next prior starts from there);
        its map and its threshold are not touched."""
        last = np.asarray(k.last_pose)
        turned = self.use_gyro_prior and self._gyro_seen
        pose = last @ _se2(0.0, 0.0, self._gyro_acc) if turned else last
        k.last_pose = pose
        self._gyro_acc, self._gyro_seen = 0.0, False
        self._dropped_contact += 1
        if turned:
            how = "pose carried by the gyro"
        elif self.use_gyro_prior:      # the prior is on but the imu stream went quiet
            how = "pose held (no gyro sample since the last revolution)"
        else:
            how = "pose held (no gyro prior)"
        logger.warning(f"lidar odom contact: revolution dropped ({n_pts} pts, median {median:.0f}), {how}; "
                       f"dropped by contacts so far: {self._dropped_contact}")
        return pose

    # ── the work ───────────────────────────────────────────────────────
    async def handle_pointcloud(self, msg: PointCloud2) -> None:
        out = msg.as_numpy()
        pts = np.asarray(out[0] if isinstance(out, tuple) else out, dtype=np.float64)
        if pts.ndim != 2 or pts.shape[0] < 30:
            return
        t0 = time.monotonic()
        thick = np.concatenate([pts, pts + [0, 0, PLANE_THICKNESS_M], pts - [0, 0, PLANE_THICKNESS_M]])
        with self._lock:
            k = self._kiss
            self._arm_relocation()
            median = self._shaken(len(pts), t0)
            shaken = median is not None
            if shaken:
                new_pose = self._carry(k, len(pts), median)
            else:
                new_pose = self._register_revolution(k, pts, thick)
                if new_pose is None:
                    return
        pose = np.asarray(new_pose)
        R, t = pose[:3, :3], pose[:3, 3]
        x, y, yaw = float(t[0]), float(t[1]), math.atan2(R[1, 0], R[0, 0])
        now_wall = time.time()
        self._pose_hist.append((now_wall, x, y, yaw))
        self._pose_hist = [h for h in self._pose_hist if now_wall - h[0] < 3.0]
        self.pose2d = (x, y, yaw)
        c, s = math.cos(yaw), math.sin(yaw)
        R2 = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
        lidar_h = _profile().lidar_height_m    # costmap2d tags lidar returns by this exact z
        world_pts = (pts @ R2.T) + np.array([x, y, lidar_h])
        ts = time.time(); q = _yaw_quat(yaw)
        # odom on the FLOOR PLANE (z=0): dimOS's planner measures 3D distances
        # (goal_tolerance 0.2 m, path checks down to 0.01 m) - an odom published
        # at lidar height keeps the robot lidar_height_m away from every z=0
        # goal FOREVER. The lidar height lives in the tf child and in the
        # world-cloud transform, never in the pose.
        self.odom.publish(PoseStamped(ts, self.world_frame, position=Vector3(x, y, 0.0), orientation=q))
        if not shaken:      # a shaken sweep: its pose goes out, its cloud never reaches the map
            cam = self._pending_cam_pts
            if cam is not None and time.monotonic() - cam[1] < 0.5:
                world_pts = np.vstack([world_pts, cam[0]])
            self.lidar.publish(PointCloud2.from_numpy(world_pts.astype(np.float32), frame_id=self.world_frame, timestamp=ts))
        self.tf.publish(TFMessage(
            Transform(translation=Vector3(x, y, 0.0), rotation=q, frame_id=self.world_frame, child_frame_id=self.base_frame, ts=ts),
            Transform(translation=Vector3(0.0, 0.0, lidar_h), rotation=Quaternion(0, 0, 0, 1),
                      frame_id=self.base_frame, child_frame_id=self.lidar_frame, ts=ts)))
        if shaken:
            return
        self._n += 1
        self._last_ms = (time.monotonic() - t0) * 1000.0
        if t0 - self._last_log >= self.log_every_s:
            self._last_log = t0
            g = np.degrees(self._gyro_totals); w = self._wheel
            logger.info(f"lidar odom #{self._n}: x={x:+.3f} y={y:+.3f} yaw={math.degrees(yaw):+.1f}deg "
                        f"({len(pts)} pts, {self._last_ms:.1f} ms, prior {self._prior_used}, depth cloud "
                        f"{self._depth_pts_last} pts) | gyro integral "
                        f"x={g[0]:+.1f} y={g[1]:+.1f} z={g[2]:+.1f}deg | wheels "
                        + (f"x={w[0]:+.3f} y={w[1]:+.3f} th={math.degrees(w[2]):+.1f}deg" if w else "none"))

    def _register_revolution(self, k, pts: np.ndarray, thick: np.ndarray) -> np.ndarray | None:
        """Register one revolution into kiss-icp and update its state; the new
        pose, or None when the registration failed twice (revolution dropped).
        Called under the lock. During a relocation the sigma is RELOC_SIGMA_GAIN
        times the adaptive one, from the same prior (the gyro's heading when
        the prior is on)."""
        prior = self._prior_delta()
        frame = k.preprocessor.preprocess(thick, np.tile(sweep_times(pts), 3) if self.deskew else np.zeros(len(thick)), prior)
        source, frame_down = k.voxelize(frame)
        sigma = k.adaptive_threshold.get_threshold()
        relocating = self._reloc_left > 0
        if relocating:
            sigma *= RELOC_SIGMA_GAIN       # after a contact: search wider, from the prior's heading
            if self._reloc_announce:
                self._reloc_announce = False
                logger.info(f"lidar odom contact: relocating over {self._reloc_left} revolutions "
                            f"(sigma x{RELOC_SIGMA_GAIN:.1f})")
        initial_guess = np.asarray(k.last_pose) @ prior
        try:
            new_pose = self._register(k, source, initial_guess, sigma)
        except RuntimeError as exc:
            self._dropped += 1
            logger.warning(f"lidar odometry: registration failed twice, revolution dropped ({exc}); "
                           f"dropped so far: {self._dropped}")
            return None
        corr = np.linalg.inv(initial_guess) @ np.asarray(new_pose)
        k.adaptive_threshold.update_model_deviation(corr)
        k.local_map.update(frame_down, new_pose)
        k.last_delta = np.linalg.inv(np.asarray(k.last_pose)) @ new_pose
        k.last_pose = new_pose
        # consume the priors
        self._gyro_acc, self._gyro_seen = 0.0, False
        self._counts.append(len(pts))
        del self._counts[:-COUNT_HISTORY]
        if relocating:
            self._reloc_yaw += math.atan2(corr[1, 0], corr[0, 0])
            self._reloc_n += 1
            self._reloc_left -= 1
            if self._reloc_left == 0:
                logger.info(f"lidar odom contact: relocation done over {self._reloc_n} revolutions, "
                            f"the registration corrected the yaw by {math.degrees(self._reloc_yaw):+.1f} deg in total")
                self._reloc_n, self._reloc_yaw = 0, 0.0
        return new_pose