robot.map_corrected_odometry
============================

.. py:module:: robot.map_corrected_odometry

.. autoapi-nested-parse::

   Map-corrected odometry using IR sensor readings and known field geometry.

   Wraps an existing odometry source and applies lateral corrections when IR
   sensors detect line crossings.  The IMU heading is trusted; only the
   translational (x, y) position is nudged.

   Correction strategy
   -------------------
   Each update the wrapper:

   1. Reads every registered IR sensor's ``probabilityOfBlack()``.
   2. For each sensor whose probability crosses the **on-line threshold**
      (rising edge), it projects the sensor's expected field position using
      the current odometry pose + robot geometry.
   3. It finds the **nearest map line** to that projected point.
   4. It computes the signed perpendicular error from the projected sensor
      position to the line's centerline.
   5. It applies a fraction (``correction_gain``) of that error as a lateral
      nudge to the stored position offset, perpendicular to the line.

   Because heading is accurate, the perpendicular correction fixes the dominant
   drift mode (sideways slip) without perturbing distance-along-travel.

   Coordinate frames
   -----------------
   * **Odometry frame** — origin at the point of last ``reset()``, meters,
     heading 0 = initial forward direction.
   * **Field frame** — origin at bottom-left of the Botball table, centimeters,
     heading 0 = +X (right), positive CCW.

   The wrapper converts between them using the configured ``start_pose``
   (field position + heading at the moment odometry was last reset).

   Example::

       from raccoon.robot.map_corrected_odometry import MapCorrectedOdometry

       mc = MapCorrectedOdometry(
           inner=robot.odometry,
           table_map=robot.table_map,
           start_x_cm=90.0,
           start_y_cm=47.0,
           start_heading_rad=0.0,
       )
       mc.register_sensor(robot.defs.front_ir, robot.sensor_position(robot.defs.front_ir))
       mc.register_sensor(robot.defs.rear_ir, robot.sensor_position(robot.defs.rear_ir))

       # In your update loop (or motion step):
       mc.update(dt)
       pose = mc.get_pose()           # corrected pose in odometry frame
       field_pos = mc.get_field_position()  # (x_cm, y_cm) on the table



Attributes
----------

.. autoapisummary::

   robot.map_corrected_odometry.logger


Classes
-------

.. autoapisummary::

   robot.map_corrected_odometry.MapCorrectedOdometry


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

.. py:data:: logger

.. py:class:: MapCorrectedOdometry(inner: Any, table_map: raccoon.robot.table_map.TableMap, start_x_cm: float, start_y_cm: float, start_heading_rad: float = 0.0, correction_gain: float = 0.6, on_line_threshold: float = 0.7, max_correction_cm: float = 3.0)

   Odometry wrapper that corrects position using IR sensor + map data.

   Delegates all standard odometry calls to the *inner* odometry and
   maintains a correction offset that is applied transparently.

   :param inner: The underlying odometry instance (FusedOdometry / Stm32Odometry).
   :param table_map: The loaded TableMap with field line/wall geometry.
   :param start_x_cm: Robot center X on field at odometry origin (cm).
   :param start_y_cm: Robot center Y on field at odometry origin (cm).
   :param start_heading_rad: Robot heading on field at odometry origin (rad, 0 = +X, CCW+).
   :param correction_gain: Fraction of perpendicular error applied per event (0–1).
                           Lower values are more conservative.  Default 0.6.
   :param on_line_threshold: ``probabilityOfBlack()`` value above which the
                             sensor is considered "on a line".  Default 0.7.
   :param max_correction_cm: Clamp for a single correction step to avoid
                             jumps from misidentified lines.  Default 3.0 cm.


   .. py:method:: register_sensor(sensor: Any, position: raccoon.robot.geometry.SensorPosition) -> None

      Register an IR sensor for map-based correction.

      :param sensor: An IRSensor instance (must have ``probabilityOfBlack()``).
      :param position: The sensor's ``SensorPosition`` on the robot.



   .. py:method:: update(dt: float) -> None

      Update inner odometry and apply map-based corrections.

      Call this at the same rate as the inner odometry's update loop.



   .. py:method:: get_pose() -> Any

      Get corrected pose in odometry frame.

      The returned Pose has the correction baked into the position.
      Heading is unchanged (trusted from IMU).



   .. py:method:: get_field_position() -> Tuple[float, float]

      Get the corrected robot position in field coordinates (cm).

      :returns: (x_cm, y_cm) on the Botball table.



   .. py:method:: get_field_heading() -> float

      Get the robot heading in field coordinates (radians).

      :returns: Heading in radians (0 = +X, CCW positive).



   .. py:method:: get_heading() -> float


   .. py:method:: get_absolute_heading() -> float


   .. py:method:: get_heading_error(target_heading_rad: float) -> float


   .. py:method:: get_distance_from_origin() -> Any


   .. py:method:: get_path_length() -> float


   .. py:method:: reset() -> None

      Reset inner odometry and clear accumulated corrections.



   .. py:method:: reset_with_field_pose(x_cm: float, y_cm: float, heading_rad: float = 0.0) -> None

      Reset odometry and set a new field-frame starting pose.

      Use this when you know exactly where the robot is (e.g. after
      aligning against a wall or starting position).

      :param x_cm: New field X position (cm from left).
      :param y_cm: New field Y position (cm from bottom).
      :param heading_rad: New field heading (radians, 0 = +X, CCW+).



   .. py:property:: correction_offset_cm
      :type: Tuple[float, float]


      Current accumulated correction in field cm (for diagnostics).