The Odometry class handles odometry readings (2D pose and velocity with related timestamp) More...

#include <odometry.h>

Public Types
typedef boost::function< void(double, double, double)>	IntegrationFunction
	Integration function, used to integrate the odometry:
Public Member Functions
double	getAngular () const
	angular velocity getter
double	getHeading () const
	heading getter
double	getLinearX () const
	linearX velocity getter
double	getLinearY () const
	linearY velocity getter
double	getX () const
	x position getter
double	getY () const
	y position getter
void	init (const ros::Time &time)
	Initialize the odometry.
	Odometry (size_t velocity_rolling_window_size=10)
	Constructor Timestamp will get the current time value Value will be set to zero.
void	setWheelsParams (double wheels_k, double wheels_radius)
	Sets the wheels parameters: mecanum geometric param and radius.
bool	update (double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time)
	Updates the odometry class with latest wheels position.
void	updateOpenLoop (double linearX, double linearY, double angular, const ros::Time &time)
	Updates the odometry class with latest velocity command.
Private Types
typedef bacc::accumulator_set < double, bacc::stats < bacc::tag::rolling_mean > >	RollingMeanAcc
	Rolling mean accumulator and window:
typedef bacc::tag::rolling_window	RollingWindow
Private Member Functions
void	integrateExact (double linearX, double linearY, double angular)
	Integrates the velocities (linear and angular) using exact method.
Private Attributes
double	angular_
RollingMeanAcc	angular_acc_
double	heading_
IntegrationFunction	integrate_fun_
	Integration funcion, used to integrate the odometry:
double	linearX_
	Current velocity:
RollingMeanAcc	linearX_acc_
double	linearY_
RollingMeanAcc	linearY_acc_
ros::Time	timestamp_
	Current timestamp:
size_t	velocity_rolling_window_size_
	Rolling mean accumulators for the linar and angular velocities:
double	wheels_k_
	Wheels kinematic parameters [m]:
double	wheels_radius_
double	x_
	Current pose:
double	y_

Detailed Description

The Odometry class handles odometry readings (2D pose and velocity with related timestamp)

Definition at line 60 of file odometry.h.

Member Typedef Documentation

typedef boost::function<void(double, double, double)> mecanum_drive_controller::Odometry::IntegrationFunction

Integration function, used to integrate the odometry:

Definition at line 65 of file odometry.h.

typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > mecanum_drive_controller::Odometry::RollingMeanAcc [private]

Rolling mean accumulator and window:

Definition at line 164 of file odometry.h.

typedef bacc::tag::rolling_window mecanum_drive_controller::Odometry::RollingWindow [private]

Definition at line 165 of file odometry.h.

Constructor & Destructor Documentation

mecanum_drive_controller::Odometry::Odometry ( size_t velocity_rolling_window_size = 10 )

Constructor Timestamp will get the current time value Value will be set to zero.

Parameters:

velocity_rolling_window_size Rolling window size used to compute the velocity mean

Definition at line 54 of file odometry.cpp.

Member Function Documentation

double mecanum_drive_controller::Odometry::getAngular ( ) const [inline]

angular velocity getter

Returns:: angular velocity [rad/s]

Definition at line 149 of file odometry.h.

double mecanum_drive_controller::Odometry::getHeading ( ) const [inline]

heading getter

Returns:: heading [rad]

Definition at line 104 of file odometry.h.

double mecanum_drive_controller::Odometry::getLinearX ( ) const [inline]

linearX velocity getter

Returns:: linearX velocity [m/s]

Definition at line 131 of file odometry.h.

double mecanum_drive_controller::Odometry::getLinearY ( ) const [inline]

linearY velocity getter

Returns:: linearY velocity [m/s]

Definition at line 140 of file odometry.h.

double mecanum_drive_controller::Odometry::getX ( ) const [inline]

x position getter

Returns:: x position [m]

Definition at line 113 of file odometry.h.

double mecanum_drive_controller::Odometry::getY ( ) const [inline]

y position getter

Returns:: y position [m]

Definition at line 122 of file odometry.h.

void mecanum_drive_controller::Odometry::init ( const ros::Time & time )

Initialize the odometry.

Parameters:

time	Current time

Definition at line 73 of file odometry.cpp.

void mecanum_drive_controller::Odometry::integrateExact	(	double	linearX,
		double	linearY,
		double	angular
	)		`[private]`

Integrates the velocities (linear and angular) using exact method.

Parameters:

linearX	Linear velocity along X [m] (linear displacement, i.e. m/s * dt) computed by encoders
linearY	Linear velocity along Y [m] (linear displacement, i.e. m/s * dt) computed by encoders
angular	Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders

Integrate angular velocity.

The odometry pose should be published in the /odom frame (unlike the odometry twist which is a body twist). Project the twist in the odometry basis (we cannot integrate linearX, linearY, angular 'as are' because they correspond to a body twist).

Integrate linear velocity.

Definition at line 132 of file odometry.cpp.

void mecanum_drive_controller::Odometry::setWheelsParams	(	double	wheels_k,
		double	wheels_radius
	)

Sets the wheels parameters: mecanum geometric param and radius.

Parameters:

wheels_k	Wheels geometric param (used in mecanum wheels' ik) [m]
wheels_radius	Wheels radius [m]

Definition at line 124 of file odometry.cpp.

bool mecanum_drive_controller::Odometry::update	(	double	wheel0_vel,
		double	wheel1_vel,
		double	wheel2_vel,
		double	wheel3_vel,
		const ros::Time &	time
	)

Updates the odometry class with latest wheels position.

Parameters:

wheel0_vel	Wheel velocity [rad]
wheel1_vel	Wheel velocity [rad]
wheel2_vel	Wheel velocity [rad]
wheel3_vel	Wheel velocity [rad]
time	Current time

Returns:: true if the odometry is actually updated

We cannot estimate the speed with very small time intervals:

Compute forward kinematics (i.e. compute mobile robot's body twist out of its wheels velocities): NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it difficult to interpret and compare behavior curves).

Integrate odometry.

Definition at line 85 of file odometry.cpp.