robot.localization_fusion
=========================

.. py:module:: robot.localization_fusion

.. autoapi-nested-parse::

   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 :class:`Observation` carrying a
   :class:`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
-------

.. autoapisummary::

   robot.localization_fusion.ContinuousLocalizationFusion


Functions
---------

.. autoapisummary::

   robot.localization_fusion.start_localization_fusion


Module Contents
---------------

.. py:class:: 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.

   :param robot: the robot; provides ``localization``, ``all_sensors()`` and the
                 per-sensor :class:`SensorPosition` geometry.
   :param threshold: ``probabilityOfBlack()`` at/above which a sensor counts as
                     "on a line" (matches the ``on_black`` convention, default 0.7).
   :param hz: fusion rate; defaults to the localization tick rate (100 Hz). Lower
              it to reduce CPU; it never needs to exceed the predict rate.
   :param anchor_sigma_cm / anchor_sigma_deg: 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.
   :param line_sigma_cm: per-line-measurement sigma handed to the map likelihood.


   .. py:property:: sensor_count
      :type: int



   .. py:method:: 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.



   .. py:method:: 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.



   .. py:method:: 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.



.. py:function:: start_localization_fusion(robot: raccoon.robot.api.GenericRobot, **kwargs) -> ContinuousLocalizationFusion | None

   Launch :class:`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.