Class CVehicleSimulVirtualBase

Inheritance Relationships

Derived Types

Class Documentation

class CVehicleSimulVirtualBase

This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile robot, including odometry errors and dynamics limitations. Main API methods are:

  • movementCommand: Call this for send a command to the robot. This command will be delayed and passed through a first order low-pass filter to simulate robot dynamics.

  • simulateInterval: Call this for run the simulator for the desired time period.

Subclassed by mrpt::kinematics::CVehicleSimul_DiffDriven, mrpt::kinematics::CVehicleSimul_Holo

State vector

double m_time

simulation running time

mrpt::math::TPose2D m_GT_pose

ground truth pose in world coordinates.

mrpt::math::TTwist2D m_GT_vel

Velocity in (x,y,omega)

mrpt::math::TTwist2D m_odometric_vel

Velocity in (x,y,omega)

mrpt::math::TPose2D m_odometry

Kinematic simulation and control interface

void simulateOneTimeStep(const double dt)

Runs the simulator during “dt” seconds. It will be split into periods of “m_firmware_control_period”.

inline const mrpt::math::TPose2D &getCurrentGTPose() const

Returns the instantaneous, ground truth pose in world coordinates

void setCurrentGTPose(const mrpt::math::TPose2D &pose)

Brute-force move robot to target coordinates (“teleport”)

inline const mrpt::math::TPose2D &getCurrentOdometricPose() const

Returns the current pose according to (noisy) odometry

template<typename T>
inline void setCurrentOdometricPose(const T &pose)

Brute-force overwrite robot odometry

inline const mrpt::math::TTwist2D &getCurrentGTVel() const

Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates

mrpt::math::TTwist2D getCurrentGTVelLocal() const

Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame

inline const mrpt::math::TTwist2D &getCurrentOdometricVel() const

Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates

mrpt::math::TTwist2D getCurrentOdometricVelLocal() const

Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame

inline double getTime() const

Get the current simulation time

virtual void sendVelCmd(const CVehicleVelCmd &cmd_vel) = 0

Sends a velocity command to the robot. The number of components and their meaning depends on the vehicle-kinematics derived class

virtual CVehicleVelCmd::Ptr getVelCmdType() const = 0

Gets an empty velocity command object that can be queried to find out the number of velcmd components,…

inline void setOdometryErrors(bool enabled, double Ax_err_bias = 1e-3, double Ax_err_std = 10e-3, double Ay_err_bias = 1e-3, double Ay_err_std = 10e-3, double Aphi_err_bias = mrpt::DEG2RAD(1e-3), double Aphi_err_std = mrpt::DEG2RAD(10e-3))

Enable/Disable odometry errors. Errors in odometry are 1 sigma Gaussian values per second

void resetStatus()
void resetTime()

simulation time).

Reset all simulator variables to 0 (except the

See also

resetTime Reset time counter

See also

resetStatus

Public Functions

CVehicleSimulVirtualBase()
virtual ~CVehicleSimulVirtualBase()

Protected Functions

virtual void internal_simulControlStep(const double dt) = 0
virtual void internal_clear() = 0

Resets all pending cmds

Protected Attributes

double m_firmware_control_period = {500e-6}

The period at which the low-level controller updates velocities (Default: 0.5 ms)

bool m_use_odo_error = {false}

Whether to corrupt odometry with noise

double m_Ax_err_bias
double m_Ax_err_std
double m_Ay_err_bias
double m_Ay_err_std
double m_Aphi_err_bias
double m_Aphi_err_std