Detailed Description

Kinematic simulator of a holonomic 2D robot capable of moving in any direction, with "blended" velocity profiles.

See CVehicleSimul_Holo::sendVelCmd() for a description of the velocity commands in this kinematic model.

Definition at line 22 of file CVehicleSimul_Holo.h.

#include <mrpt/kinematics/CVehicleSimul_Holo.h>

Inheritance diagram for mrpt::kinematics::CVehicleSimul_Holo:

Classes
struct	TVelRampCmd

Public Types
using	kinematic_cmd_t = CVehicleVelCmd_Holo

Public Member Functions
	CVehicleSimul_Holo ()

void	sendVelRampCmd (double vel, double dir, double ramp_time, double rot_speed)
	Sends a velocity cmd to the holonomic robot. More...

void	sendVelCmd (const CVehicleVelCmd &cmd_vel) override
	Sends a velocity command to the robot. More...

CVehicleVelCmd::Ptr	getVelCmdType () const override
	Gets an empty velocity command object that can be queried to find out the number of velcmd components,... More...

Kinematic simulation and control interface
void	simulateOneTimeStep (const double dt)
	Runs the simulator during "dt" seconds. More...

const mrpt::math::TPose2D &	getCurrentGTPose () const
	Returns the instantaneous, ground truth pose in world coordinates. More...

void	setCurrentGTPose (const mrpt::math::TPose2D &pose)
	Brute-force move robot to target coordinates ("teleport") More...

const mrpt::math::TPose2D &	getCurrentOdometricPose () const
	Returns the current pose according to (noisy) odometry. More...

template<typename T >
void	setCurrentOdometricPose (const T &pose)
	Brute-force overwrite robot odometry. More...

const mrpt::math::TTwist2D &	getCurrentGTVel () const
	Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates. More...

mrpt::math::TTwist2D	getCurrentGTVelLocal () const
	Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame. More...

const mrpt::math::TTwist2D &	getCurrentOdometricVel () const
	Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates. More...

mrpt::math::TTwist2D	getCurrentOdometricVelLocal () const
	Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame. More...

double	getTime () const
	Get the current simulation time. More...

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

void	resetStatus ()

void	resetTime ()
	Reset all simulator variables to 0 (except the. More...

Protected Attributes
double	m_firmware_control_period
	The period at which the low-level controller updates velocities (Default: 0.5 ms) More...

bool	m_use_odo_error
	Whether to corrupt odometry with noise. More...

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

State vector
double	m_time
	simulation running time More...

mrpt::math::TPose2D	m_GT_pose
	ground truth pose in world coordinates. More...

mrpt::math::TTwist2D	m_GT_vel
	Velocity in (x,y,omega) More...

mrpt::math::TTwist2D	m_odometric_vel
	Velocity in (x,y,omega) More...

mrpt::math::TPose2D	m_odometry

Private Member Functions
void	internal_simulControlStep (const double dt) override

void	internal_clear () override
	Resets all pending cmds. More...

Private Attributes
Vel ramp cmds
TVelRampCmd	m_vel_ramp_cmd
	the last cmd received from the user. More...

Member Typedef Documentation

◆ kinematic_cmd_t

using mrpt::kinematics::CVehicleSimul_Holo::kinematic_cmd_t = CVehicleVelCmd_Holo

Definition at line 25 of file CVehicleSimul_Holo.h.

Constructor & Destructor Documentation

◆ CVehicleSimul_Holo()

CVehicleSimul_Holo::CVehicleSimul_Holo ( )

Definition at line 17 of file CVehicleSimul_Holo.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), and mrpt::kinematics::CVehicleSimulVirtualBase::resetTime().

Member Function Documentation

◆ getCurrentGTPose()

const mrpt::math::TPose2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTPose ( ) const

inlineinherited

Returns the instantaneous, ground truth pose in world coordinates.

Definition at line 44 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds(), and run_rnav_test().

◆ getCurrentGTVel()

const mrpt::math::TTwist2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVel ( ) const

inlineinherited

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

Definition at line 63 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().

◆ getCurrentGTVelLocal()

mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentGTVelLocal ( ) const

inherited

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

Definition at line 89 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel, mrpt::math::TPose2D::phi, and mrpt::math::TTwist2D::rotate().

◆ getCurrentOdometricPose()

const mrpt::math::TPose2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricPose ( ) const

inlineinherited

Returns the current pose according to (noisy) odometry.

See also: setOdometryErrors

Definition at line 50 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().

◆ getCurrentOdometricVel()

const mrpt::math::TTwist2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVel ( ) const

inlineinherited

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

Definition at line 70 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel.

◆ getCurrentOdometricVelLocal()

mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentOdometricVelLocal ( ) const

inherited

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

Definition at line 96 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry, mrpt::math::TPose2D::phi, and mrpt::math::TTwist2D::rotate().

Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().

◆ getTime()

double mrpt::kinematics::CVehicleSimulVirtualBase::getTime ( ) const

inlineinherited

Get the current simulation time.

Definition at line 79 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_time.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getNavigationTime(), mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getNavigationTime(), mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::resetNavigationTimer(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::resetNavigationTimer().

◆ getVelCmdType()

CVehicleVelCmd::Ptr mrpt::kinematics::CVehicleSimul_Holo::getVelCmdType ( ) const

inlineoverridevirtual

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

Implements mrpt::kinematics::CVehicleSimulVirtualBase.

Definition at line 51 of file CVehicleSimul_Holo.h.

◆ internal_clear()

void CVehicleSimul_Holo::internal_clear ( )

overrideprivatevirtual

Resets all pending cmds.

Implements mrpt::kinematics::CVehicleSimulVirtualBase.

Definition at line 72 of file CVehicleSimul_Holo.cpp.

References m_vel_ramp_cmd.

◆ internal_simulControlStep()

void CVehicleSimul_Holo::internal_simulControlStep ( const double dt )

overrideprivatevirtual

Implements mrpt::kinematics::CVehicleSimulVirtualBase.

Definition at line 23 of file CVehicleSimul_Holo.cpp.

◆ resetStatus()

void CVehicleSimulVirtualBase::resetStatus ( )

inherited

Definition at line 79 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::internal_clear(), mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, and mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.

Referenced by mrpt::kinematics::CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven(), and CVehicleSimul_Holo().

◆ resetTime()

void CVehicleSimulVirtualBase::resetTime ( )

inherited

Reset all simulator variables to 0 (except the.

simulation time).

See also: resetTime Reset time counter; resetStatus

Definition at line 88 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_time.

Referenced by mrpt::kinematics::CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven(), and CVehicleSimul_Holo().

◆ sendVelCmd()

void mrpt::kinematics::CVehicleSimul_Holo::sendVelCmd ( const CVehicleVelCmd & cmd_vel )

inlineoverridevirtual

Sends a velocity command to the robot.

The number of components and their meaning depends on the vehicle-kinematics derived class

Implements mrpt::kinematics::CVehicleSimulVirtualBase.

Definition at line 39 of file CVehicleSimul_Holo.h.

References ASSERTMSG_, mrpt::kinematics::CVehicleVelCmd_Holo::dir_local, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry, mrpt::math::TPose2D::phi, mrpt::kinematics::CVehicleVelCmd_Holo::ramp_time, mrpt::kinematics::CVehicleVelCmd_Holo::rot_speed, sendVelRampCmd(), and mrpt::kinematics::CVehicleVelCmd_Holo::vel.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::changeSpeeds().

◆ sendVelRampCmd()

void CVehicleSimul_Holo::sendVelRampCmd	(	double	vel,
		double	dir,
		double	ramp_time,
		double	rot_speed
	)

Sends a velocity cmd to the holonomic robot.

Parameters

[in]	vel	Linear speed (m/s)
[in]	dir	Direction (rad) (In the odometry frame of reference)
[in]	ramp_time	Blend the cmd during this period (seconds)
[in]	rot_speed	Rotational speed while there is heading error and to this constant (rad/s)

Definition at line 73 of file CVehicleSimul_Holo.cpp.

Referenced by sendVelCmd(), and mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::stop().

◆ setCurrentGTPose()

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

inherited

Brute-force move robot to target coordinates ("teleport")

Definition at line 24 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose.

◆ setCurrentOdometricPose()

template<typename T >

void mrpt::kinematics::CVehicleSimulVirtualBase::setCurrentOdometricPose ( const T & pose )

inlineinherited

Brute-force overwrite robot odometry.

Definition at line 56 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.

◆ setOdometryErrors()

void mrpt::kinematics::CVehicleSimulVirtualBase::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)`
	)