robot.localization_fusion

Continuous, automatic line-sensor fusion into the localization particle filter.

The C++ Localization background thread only predicts — it integrates the odometry delta every tick and never looks at a sensor. All map-based correction has historically come from an explicit localization.observe() call inside a resync step (find_line_resync / align_to_wall_resync). That means during ordinary driving the filter is pure dead-reckoning and only snaps back to the map at the rare moments a resync step runs.

This service closes that gap: it runs as an always-on background loop that, every tick, reads the robot’s line sensors, builds one Observation carrying a SurfaceMeasurement per sensor (detected / not-detected against the map), and feeds it to the filter. The filter then continuously weights its particles against the table map’s line geometry — real Monte-Carlo localization, not just dead-reckoning.

Resync steps are NOT replaced: they remain the manual “I am confident I am on axis X at 210 cm” hint (a hard, axis-snapping pose observation). This service is the soft, automatic, every-tick correction underneath them.

The fusion pose channel anchors weakly on the filter’s OWN current estimate (NO ground truth is ever used), so unobservable degrees of freedom (e.g. the along-a- straight-line axis when only crossing perpendicular lines) stay pinned to odometry instead of drifting, while the surface measurements pull the observable axes onto the map. The C++ filter only resamples when its effective sample size drops (resample_effective_sample_ratio), so feeding an observation every tick does not deplete particles.

Classes

ContinuousLocalizationFusion

Always-on background fuser of line sensors → localization.

Functions

start_localization_fusion(...)

Launch ContinuousLocalizationFusion on its own daemon thread.

Module Contents

class robot.localization_fusion.ContinuousLocalizationFusion(robot: raccoon.robot.api.GenericRobot, *, threshold: float = 0.7, hz: float = 100.0, anchor_sigma_cm: float = 8.0, anchor_sigma_deg: float = 8.0, line_sigma_cm: float = 1.0)

Always-on background fuser of line sensors → localization.

Parameters:

  • robot – the robot; provides localization, all_sensors() and the per-sensor SensorPosition geometry.

  • threshold – probabilityOfBlack() at/above which a sensor counts as “on a line” (matches the on_black convention, default 0.7).

  • hz – fusion rate; defaults to the localization tick rate (100 Hz). Lower it to reduce CPU; it never needs to exceed the predict rate.

  • anchor_sigma_deg (anchor_sigma_cm /) – the weak pose-prior sigma (the filter is told “you are roughly where you currently think you are”). Small enough to pin unobservable axes, large enough to let the line measurements move the observable ones. ~8 cm works well in practice.

  • line_sigma_cm – per-line-measurement sigma handed to the map likelihood.

property sensor_count: int
tick() → bool

Read sensors once and push the observation. Returns False (no-op) when there is no localization or no line sensors. Exposed for tests and manual stepping; the loop calls it every period.

start() → ContinuousLocalizationFusion | None

Spawn the dedicated daemon thread that runs the fusion loop.

Returns self (a stop-handle) or None if there are no line sensors (nothing to fuse). The thread is a daemon so it can never block process shutdown, and it is COMPLETELY off the asyncio motion loop — localization work never runs on, and can never block, the hot path. Idempotent.

stop(timeout: float = 1.0) → None

Signal the loop to exit and join the thread. Safe to call more than once and from any thread. Never raises.

robot.localization_fusion.start_localization_fusion(robot: raccoon.robot.api.GenericRobot, **kwargs) → ContinuousLocalizationFusion | None

Launch ContinuousLocalizationFusion on its own daemon thread.

Returns the running fusion (call .stop() to shut it down) or None if the robot has no line sensors. All localization work — sensor reads, building observations, observe() — happens on that thread, NEVER on the caller’s asyncio event loop, so it can never block the motion hot path. Safe to call once at robot start; idempotent guard is the caller’s responsibility.