Odometry2D Class Reference

Tracks 2D position and orientation of a robot using velocity and steering data from external sources. More...

#include <Odometry2D.h>

Inheritance diagram for Odometry2D:

Collaboration diagram for Odometry2D:

Public Member Functions
	Odometry2D (MessageSource &vehicle, ISpeedSource &speedSource, IOdometryModel2D &model)
bool	begin (Coordinate< DistanceM > initialPosition, float initialTheta=0.0f)
	Initialize the odometry state.
bool	begin (Transform2D transform)
bool	begin (const Frame2D &frame)
	Initialize the odometry state from a Frame2D.
void	end ()
void	update ()
	Update the odometry state with new speed and steering angle Call this method in your control loop.
Coordinate< DistanceM >	getPosition () const
	Get the current 2D position (meters)
Angle	getSteeringAngle () const
	Get the current steering angle (radians or angular velocity)
Angle	getHeading () const
	Get the current heading as an Angle (radians)
Speed	getSpeed () const
	Get the current speed (meters/second)
float	getTheta () const
	Get the current orientation (radians)
float	getLinearVelocity () const
	Get the current linear velocity (meters/second)
float	getAngularVelocity () const
	Get the current angular velocity (radians/second)
Distance	getTotalDistance () const
	Get the total distance traveled.
Delta2D	getLastDelta () const
	Get the last delta update (dx, dy, dtheta)
Speed3D	getSpeed3D () const
	Get the current speed as a Speed3D (x, y in world frame, z: 0)
void	setState (Coordinate< DistanceM > pos, float th)
	Set the odometry state (position and orientation)
MotionState3D	getMotionState () const
	Get the current motion state as a MotionState3D (with z=0 and pitch/roll=0)
Transform2D	getTransform () const
virtual Coordinate< DistanceM >	getPosition () const =0
virtual Angle	getHeading () const =0
virtual Speed	getSpeed () const =0
virtual Transform2D	getTransform () const =0
virtual bool	begin (Transform2D transform)=0
virtual void	end ()=0
Public Member Functions inherited from MessageSource
void	subscribe (MessageHandler &handler, MessageOrigin origin=MessageOrigin::Undefined, MessageContent content=MessageContent::Undefined)
	Subscribe a message handler to this source, with optional filtering.
void	unsubscribeAll ()
	Remove all registered message handlers.
void	sendMessage (Message< float > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< Coordinate< float > > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< GPSCoordinate > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< MotionState3D > &msg)
	Overload for MotionState3D messages.

Protected Member Functions
void	publish ()
void	update (uint32_t deltaTimeMs)
	Update the odometry state with new speed and steering angle.

Protected Attributes
Coordinate< float >	position
float	theta = 0.0f
float	totalDistance = 0.0f
Delta2D	lastDelta = {0.0f, 0.0f, 0.0f}
uint32_t	lastUpdateTimeMs = 0
IOdometryModel2D &	model
ISpeedSource &	speedSource
MessageSource &	vehicle
bool	is_differential = false
Protected Attributes inherited from MessageSource
std::vector< MessageHandlerEntry >	messageHandlers_

Detailed Description

Tracks 2D position and orientation of a robot using velocity and steering data from external sources.

This class provides 2D odometry for mobile robots, such as differential drive or Ackermann vehicles. It integrates velocity and steering angle over time to estimate the robot's position (x, y) in meters.

Supported Kinematics

• Differential drive (default model, via steering angle is provided, via IOdometryModel2D)

Inputs

• Speed (from an ISpeedSource, e.g., WheelEncoder)
• Steering angle (from messages or sensors)
• Time delta (in milliseconds, or uses millis() if not provided)

Outputs

• 2D position (x, y) in meters

Coordinate Frame

• All positions are in the robot's local/world frame, with x forward and y left/right.

Update Method

• Call update() in your control loop to update odometry using the latest speed and steering angle.
• Optionally provide delta time, or let the class compute it using millis().

Integration Method

• Uses simple Euler integration for position updates.

Construction

• Construct with references to an IMessageSource (vehicle), an ISpeedSource (e.g., WheelEncoder), and an IOdometryModel2D (kinematics model).
• The vehicle is subscribed to receive messages for speed/throttle/steering updates.

Limitations

• Assumes no wheel slip or drift unless compensated by the speed source.
• Not suitable for holonomic or omnidirectional robots.
• Orientation (theta) is not explicitly tracked in this class.

Example Usage

WheelEncoder encoder(...);
OdometryModelAckerman model(...);
Odometry2D odom(vehicle, encoder, model);
odom.begin(Coordinate<DistanceM>(0, 0));
// In your control loop:
odom.update();
auto pos = odom.getPosition();
Serial.printf("x=%.2f, y=%.2f\n", pos.x, pos.y);

Author

TinyRobotics contributors

Date

2026-03-30

Member Function Documentation

begin() [1/3]

bool begin ( const Frame2D &  frame )

inline

Initialize the odometry state from a Frame2D.

Parameters

frame

The Frame2D containing the initial position and orientation.

Returns

true on success

begin() [2/3]

bool begin ( Coordinate< DistanceM >  initialPosition, float  initialTheta = 0.0f  )

inline

Initialize the odometry state.

Parameters

initialPosition	The starting position of the robot (x, y) in meters.
initialTheta	The starting orientation (heading/yaw) in radians. Default is 0.0.

Returns

true on success

begin() [3/3]

bool begin ( Transform2D  transform )

inlinevirtual

Implements IMotionState2D.

end()

void end ( )

inlinevirtual

Implements IMotionState2D.

getHeading()

Angle getHeading ( ) const

inlinevirtual

Get the current heading as an Angle (radians)

Implements IMotionState2D.

getPosition()

Coordinate< DistanceM > getPosition ( ) const

inlinevirtual

Get the current 2D position (meters)

Implements IMotionState2D.

getSpeed()

Speed getSpeed ( ) const

inlinevirtual

Get the current speed (meters/second)

Implements IMotionState2D.

getTransform()

Transform2D getTransform ( ) const

inlinevirtual

Implements IMotionState2D.

update()

void update ( uint32_t  deltaTimeMs )

inlineprotected

Update the odometry state with new speed and steering angle.

Parameters

speed	The current speed of the robot (with units).
steeringAngle	The current steering angle (radians for Ackermann/rudder, or angular velocity for differential drive).
deltaTimeMs	Time since last update in milliseconds.

---

The documentation for this class was generated from the following file:

• src/TinyRobotics/odometry/Odometry2D.h