Computes odometry values based on a set of kinematic properties and wheel speeds for a Mecanum-drive vehicle. More...
#include <MecanumDriveOdometry.hh>
Public Types | |
using | clock = std::chrono::steady_clock |
Public Member Functions | |
MecanumDriveOdometry (size_t _windowSize=10) | |
Constructor. More... | |
~MecanumDriveOdometry () | |
Destructor. More... | |
const Angle & | AngularVelocity () const |
Get the angular velocity. More... | |
const Angle & | Heading () const |
Get the heading. More... | |
void | Init (const clock::time_point &_time) |
Initialize the odometry. More... | |
bool | Initialized () const |
Get whether Init has been called. More... | |
double | LateralVelocity () const |
Get the lateral velocity. More... | |
double | LeftWheelRadius () const |
Get the left wheel radius. More... | |
double | LinearVelocity () const |
Get the linear velocity. More... | |
double | RightWheelRadius () const |
Get the rightwheel radius. More... | |
void | SetVelocityRollingWindowSize (size_t _size) |
Set the velocity rolling window size. More... | |
void | SetWheelParams (double _wheelSeparation, double _wheelBase, double _leftWheelRadius, double _rightWheelRadius) |
Set the wheel parameters including the radius and separation. More... | |
bool | Update (const Angle &_frontLeftPos, const Angle &_frontRightPos, const Angle &_backLeftPos, const Angle &_backRightPos, const clock::time_point &_time) |
Updates the odometry class with latest wheels and steerings position. More... | |
double | WheelBase () const |
Get the wheel base. More... | |
double | WheelSeparation () const |
Get the wheel separation. More... | |
double | X () const |
Get the X position. More... | |
double | Y () const |
Get the Y position. More... | |
Detailed Description
Computes odometry values based on a set of kinematic properties and wheel speeds for a Mecanum-drive vehicle.
gz/math/MecanumDriveOdometry.hh
Note: when computing velocity the math currently assumes that all wheels have a radius of 1.0.
A vehicle with a heading of zero degrees has a local reference frame according to the diagram below.
Y ^ | | O--->X(forward)
Member Typedef Documentation
◆ clock
using clock = std::chrono::steady_clock |
Constructor & Destructor Documentation
◆ MecanumDriveOdometry()
|
explicit |
Constructor.
- Parameters
-
[in] _windowSize Rolling window size used to compute the velocity mean
◆ ~MecanumDriveOdometry()
~MecanumDriveOdometry | ( | ) |
Destructor.
Member Function Documentation
◆ AngularVelocity()
const Angle& AngularVelocity | ( | ) | const |
Get the angular velocity.
- Returns
- The angular velocity in radian/second.
◆ Heading()
const Angle& Heading | ( | ) | const |
Get the heading.
- Returns
- The heading in radians.
◆ Init()
void Init | ( | const clock::time_point & | _time | ) |
Initialize the odometry.
- Parameters
-
[in] _time Current time.
◆ Initialized()
bool Initialized | ( | ) | const |
Get whether Init has been called.
- Returns
- True if Init has been called, false otherwise.
◆ LateralVelocity()
double LateralVelocity | ( | ) | const |
Get the lateral velocity.
- Returns
- The lateral velocity in meter/second.
◆ LeftWheelRadius()
double LeftWheelRadius | ( | ) | const |
Get the left wheel radius.
- Returns
- Left wheel radius in meters.
◆ LinearVelocity()
double LinearVelocity | ( | ) | const |
Get the linear velocity.
- Returns
- The linear velocity in meter/second.
◆ RightWheelRadius()
double RightWheelRadius | ( | ) | const |
Get the rightwheel radius.
- Returns
- Right wheel radius in meters.
◆ SetVelocityRollingWindowSize()
void SetVelocityRollingWindowSize | ( | size_t | _size | ) |
Set the velocity rolling window size.
- Parameters
-
[in] _size The Velocity rolling window size.
◆ SetWheelParams()
void SetWheelParams | ( | double | _wheelSeparation, |
double | _wheelBase, | ||
double | _leftWheelRadius, | ||
double | _rightWheelRadius | ||
) |
Set the wheel parameters including the radius and separation.
- Parameters
-
[in] _wheelSeparation Distance between left and right wheels. [in] _wheelBase Distance between front and back wheels. [in] _leftWheelRadius Radius of the left wheel. [in] _rightWheelRadius Radius of the right wheel.
◆ Update()
bool Update | ( | const Angle & | _frontLeftPos, |
const Angle & | _frontRightPos, | ||
const Angle & | _backLeftPos, | ||
const Angle & | _backRightPos, | ||
const clock::time_point & | _time | ||
) |
Updates the odometry class with latest wheels and steerings position.
- Parameters
-
[in] _frontLeftPos Left wheel position in radians. [in] _frontRightPos Right wheel postion in radians. [in] _backLeftPos Left wheel position in radians. [in] _backRightPos Right wheel postion in radians. [in] _time Current time point.
- Returns
- True if the odometry is actually updated.
◆ WheelBase()
double WheelBase | ( | ) | const |
Get the wheel base.
- Returns
- Distance between front and back wheels in meters.
◆ WheelSeparation()
double WheelSeparation | ( | ) | const |
Get the wheel separation.
- Returns
- Distance between left and right wheels in meters.
◆ X()
double X | ( | ) | const |
Get the X position.
- Returns
- The X position in meters
◆ Y()
double Y | ( | ) | const |
Get the Y position.
- Returns
- The Y position in meters.
The documentation for this class was generated from the following file: