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: More...

Public Member Functions
double	getAngular () const
	angular velocity getter More...

double	getHeading () const
	heading getter More...

double	getLinearX () const
	linearX velocity getter More...

double	getLinearY () const
	linearY velocity getter More...

double	getX () const
	x position getter More...

double	getY () const
	y position getter More...

void	init (const ros::Time &time)
	Initialize the odometry. More...

	Odometry (size_t velocity_rolling_window_size=10)
	Constructor Timestamp will get the current time value Value will be set to zero. More...

void	setWheelsParams (double wheels_k, double wheels_radius)
	Sets the wheels parameters: mecanum geometric param and radius. More...

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

void	updateOpenLoop (double linearX, double linearY, double angular, const ros::Time &time)
	Updates the odometry class with latest velocity command. More...

Private Types
typedef bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > >	RollingMeanAcc
	Rolling mean accumulator and window: More...

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

Private Attributes
double	angular_

RollingMeanAcc	angular_acc_

double	heading_

IntegrationFunction	integrate_fun_
	Integration funcion, used to integrate the odometry: More...

double	linearX_
	Current velocity: More...

RollingMeanAcc	linearX_acc_

double	linearY_

RollingMeanAcc	linearY_acc_

ros::Time	timestamp_
	Current timestamp: More...

size_t	velocity_rolling_window_size_
	Rolling mean accumulators for the linar and angular velocities: More...

double	wheels_k_
	Wheels kinematic parameters [m]: More...

double	wheels_radius_

double	x_
	Current pose: More...

double	y_

Detailed Description

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

Definition at line 92 of file odometry.h.

Member Typedef Documentation

◆ IntegrationFunction

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

Integration function, used to integrate the odometry:

Definition at line 129 of file odometry.h.

◆ RollingMeanAcc

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 228 of file odometry.h.

◆ RollingWindow

typedef bacc::tag::rolling_window mecanum_drive_controller::Odometry::RollingWindow

private

Definition at line 229 of file odometry.h.

Constructor & Destructor Documentation

◆ Odometry()

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 86 of file odometry.cpp.

Member Function Documentation

◆ getAngular()

double mecanum_drive_controller::Odometry::getAngular ( ) const

inline

angular velocity getter

Returns: angular velocity [rad/s]

Definition at line 213 of file odometry.h.

◆ getHeading()

double mecanum_drive_controller::Odometry::getHeading ( ) const

inline

heading getter

Returns: heading [rad]

Definition at line 168 of file odometry.h.

◆ getLinearX()

double mecanum_drive_controller::Odometry::getLinearX ( ) const

inline

linearX velocity getter

Returns: linearX velocity [m/s]

Definition at line 195 of file odometry.h.

◆ getLinearY()

double mecanum_drive_controller::Odometry::getLinearY ( ) const

inline

linearY velocity getter

Returns: linearY velocity [m/s]

Definition at line 204 of file odometry.h.

◆ getX()

double mecanum_drive_controller::Odometry::getX ( ) const

inline

x position getter

Returns: x position [m]

Definition at line 177 of file odometry.h.

◆ getY()

double mecanum_drive_controller::Odometry::getY ( ) const

inline

y position getter

Returns: y position [m]

Definition at line 186 of file odometry.h.

◆ init()

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

Initialize the odometry.

Parameters

time	Current time

Definition at line 105 of file odometry.cpp.

◆ integrateExact()

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 computed by encoders
linearY	Linear velocity along Y m computed by encoders
angular	Angular velocity rad 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 164 of file odometry.cpp.

◆ setWheelsParams()

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 156 of file odometry.cpp.

◆ update()

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 117 of file odometry.cpp.

◆ updateOpenLoop()

void mecanum_drive_controller::Odometry::updateOpenLoop	(	double	linearX,
		double	linearY,
		double	angular,
		const ros::Time &	time
	)

Updates the odometry class with latest velocity command.

Parameters

linearX	Linear velocity [m/s]
angular	Angular velocity [rad/s]
time	Current time

Save last linear and angular velocity:

Integrate odometry:

Definition at line 142 of file odometry.cpp.

Member Data Documentation

◆ angular_

double mecanum_drive_controller::Odometry::angular_

private

Definition at line 250 of file odometry.h.

◆ angular_acc_

RollingMeanAcc mecanum_drive_controller::Odometry::angular_acc_

private

Definition at line 260 of file odometry.h.

◆ heading_

double mecanum_drive_controller::Odometry::heading_

private

Definition at line 245 of file odometry.h.

◆ integrate_fun_

IntegrationFunction mecanum_drive_controller::Odometry::integrate_fun_

private

Integration funcion, used to integrate the odometry:

Definition at line 263 of file odometry.h.

◆ linearX_

double mecanum_drive_controller::Odometry::linearX_

private

Current velocity:

Definition at line 248 of file odometry.h.

◆ linearX_acc_

RollingMeanAcc mecanum_drive_controller::Odometry::linearX_acc_

private

Definition at line 258 of file odometry.h.

◆ linearY_

double mecanum_drive_controller::Odometry::linearY_

private

Definition at line 249 of file odometry.h.

◆ linearY_acc_

RollingMeanAcc mecanum_drive_controller::Odometry::linearY_acc_

private

Definition at line 259 of file odometry.h.

◆ timestamp_

ros::Time mecanum_drive_controller::Odometry::timestamp_

private

Current timestamp:

Definition at line 240 of file odometry.h.

◆ velocity_rolling_window_size_

size_t mecanum_drive_controller::Odometry::velocity_rolling_window_size_

private

Rolling mean accumulators for the linar and angular velocities:

Definition at line 257 of file odometry.h.

◆ wheels_k_

double mecanum_drive_controller::Odometry::wheels_k_

private

Wheels kinematic parameters [m]:

Definition at line 253 of file odometry.h.

◆ wheels_radius_

double mecanum_drive_controller::Odometry::wheels_radius_

private

Definition at line 254 of file odometry.h.

◆ x_

double mecanum_drive_controller::Odometry::x_

private

Current pose:

Definition at line 243 of file odometry.h.

◆ y_

double mecanum_drive_controller::Odometry::y_

private

Definition at line 244 of file odometry.h.

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

Public Types

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ IntegrationFunction

◆ RollingMeanAcc

◆ RollingWindow

Constructor & Destructor Documentation

◆ Odometry()

Member Function Documentation

◆ getAngular()

◆ getHeading()

◆ getLinearX()

◆ getLinearY()

◆ getX()

◆ getY()

◆ init()

◆ integrateExact()

◆ setWheelsParams()

◆ update()

◆ updateOpenLoop()

Member Data Documentation

◆ angular_

◆ angular_acc_

◆ heading_

◆ integrate_fun_

◆ linearX_

◆ linearX_acc_

◆ linearY_

◆ linearY_acc_

◆ timestamp_

◆ velocity_rolling_window_size_

◆ wheels_k_

◆ wheels_radius_

◆ x_

◆ y_