robot.map_corrected_odometry

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

logger

Classes

MapCorrectedOdometry

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

Module Contents

robot.map_corrected_odometry.logger
class robot.map_corrected_odometry.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.

Parameters:

  • inner – The underlying odometry instance (FusedOdometry / Stm32Odometry).

  • table_map – The loaded TableMap with field line/wall geometry.

  • start_x_cm – Robot center X on field at odometry origin (cm).

  • start_y_cm – Robot center Y on field at odometry origin (cm).

  • start_heading_rad – Robot heading on field at odometry origin (rad, 0 = +X, CCW+).

  • correction_gain – Fraction of perpendicular error applied per event (0–1). Lower values are more conservative. Default 0.6.

  • on_line_threshold – probabilityOfBlack() value above which the sensor is considered “on a line”. Default 0.7.

  • max_correction_cm – Clamp for a single correction step to avoid jumps from misidentified lines. Default 3.0 cm.

register_sensor(sensor: Any, position: raccoon.robot.geometry.SensorPosition) → None

Register an IR sensor for map-based correction.

Parameters:

  • sensor – An IRSensor instance (must have probabilityOfBlack()).

  • position – The sensor’s SensorPosition on the robot.

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.

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).

get_field_position() → Tuple[float, float]

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

Returns:

(x_cm, y_cm) on the Botball table.

get_field_heading() → float

Get the robot heading in field coordinates (radians).

Returns:

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

get_heading() → float
get_absolute_heading() → float
get_heading_error(target_heading_rad: float) → float
get_distance_from_origin() → Any
get_path_length() → float
reset() → None

Reset inner odometry and clear accumulated corrections.

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).

Parameters:

  • x_cm – New field X position (cm from left).

  • y_cm – New field Y position (cm from bottom).

  • heading_rad – New field heading (radians, 0 = +X, CCW+).

property correction_offset_cm: Tuple[float, float]

Current accumulated correction in field cm (for diagnostics).