Main MRPT website > C++ reference for MRPT 1.5.7
CVehicleSimulVirtualBase.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/system/datetime.h>
16 
17 namespace mrpt
18 {
19 namespace kinematics
20 {
21  /** This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile robot, including odometry errors and dynamics limitations.
22  * Main API methods are:
23  * - movementCommand: Call this for send a command to the robot. This comamnd will be
24  * delayed and passed throught a first order low-pass filter to simulate
25  * robot dynamics.
26  * - simulateInterval: Call this for run the simulator for the desired time period.
27  *
28  * \ingroup mrpt_kinematics_grp
29  */
31  {
32  public:
34  virtual ~CVehicleSimulVirtualBase();
35 
36  /** @name Kinematic simulation and control interface
37  * @{ */
38  /** Runs the simulator during "dt" seconds. It will be split into periods of "m_firmware_control_period". */
39  void simulateOneTimeStep(const double dt);
40 
41  /** Returns the instantaneous, ground truth pose in world coordinates */
42  const mrpt::math::TPose2D & getCurrentGTPose() const { return m_GT_pose; }
43  /** Brute-force move robot to target coordinates ("teleport") */
44  void setCurrentGTPose(const mrpt::math::TPose2D &pose);
45 
46 
47  /** Returns the current pose according to (noisy) odometry \sa setOdometryErrors */
48  const mrpt::math::TPose2D & getCurrentOdometricPose() const { return m_odometry; }
49  /** Brute-force overwrite robot odometry */
50  template<typename T>
51  void setCurrentOdometricPose(const T &pose) { m_odometry = mrpt::math::TPose2D(pose); }
52 
53  /** Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates */
54  const mrpt::math::TTwist2D & getCurrentGTVel() const { return m_GT_vel; }
55  /** Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame */
56  mrpt::math::TTwist2D getCurrentGTVelLocal() const;
57 
58  /** Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates */
59  const mrpt::math::TTwist2D & getCurrentOdometricVel() const { return m_odometric_vel; }
60  /** Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame */
61  mrpt::math::TTwist2D getCurrentOdometricVelLocal() const;
62 
63 
64  /** Get the current simulation time */
65  double getTime() const { return m_time;}
66 
67  /** Sends a velocity command to the robot. The number of components and their meaning depends
68  * on the vehicle-kinematics derived class */
69  virtual void sendVelCmd(const CVehicleVelCmd &cmd_vel) = 0;
70  /** Gets an empty velocity command object that can be queried to find out the number of velcmd components,... */
71  virtual CVehicleVelCmdPtr getVelCmdType() const = 0;
72 
73  /** Enable/Disable odometry errors. Errors in odometry are 1 sigma Gaussian values per second */
75  bool enabled,
76  double Ax_err_bias = 1e-3,
77  double Ax_err_std = 10e-3,
78  double Ay_err_bias = 1e-3,
79  double Ay_err_std = 10e-3,
80  double Aphi_err_bias = mrpt::utils::DEG2RAD(1e-3),
81  double Aphi_err_std = mrpt::utils::DEG2RAD(10e-3)
82  )
83  {
84  m_use_odo_error=enabled;
85  m_Ax_err_bias=Ax_err_bias;
86  m_Ax_err_std=Ax_err_std;
87  m_Ay_err_bias=Ay_err_bias;
88  m_Ay_err_std=Ay_err_std;
89  m_Aphi_err_bias=Aphi_err_bias;
90  m_Aphi_err_std=Aphi_err_std;
91  }
92 
93  void resetStatus(); //! Reset all simulator variables to 0 (except the simulation time). \sa resetTime
94  void resetTime(); //!< Reset time counter \sa resetStatus
95 
96  /** @} */
97 
98  protected:
99  /** @name State vector
100  * @{ */
101  double m_time; //!< simulation running time
102  mrpt::math::TPose2D m_GT_pose; //!< ground truth pose in world coordinates.
103  mrpt::math::TTwist2D m_GT_vel; //!< Velocity in (x,y,omega)
104  mrpt::math::TTwist2D m_odometric_vel; //!< Velocity in (x,y,omega)
106  /** @} */
107  double m_firmware_control_period; //!< The period at which the low-level controller updates velocities (Default: 0.5 ms)
108 
109  bool m_use_odo_error; //!< Whether to corrupt odometry with noise
110  double m_Ax_err_bias, m_Ax_err_std;
111  double m_Ay_err_bias, m_Ay_err_std;
112  double m_Aphi_err_bias, m_Aphi_err_std;
113 
114  virtual void internal_simulControlStep(const double dt) = 0;
115  virtual void internal_clear() =0; //!< Resets all pending cmds
116  private:
117 
118  };
119 
120  } // End of namespace
121 } // End of namespace
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile...
double DEG2RAD(const double x)
Degrees to radians.
mrpt::math::TPose2D m_GT_pose
ground truth pose in world coordinates.
const mrpt::math::TTwist2D & getCurrentOdometricVel() const
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates.
void setCurrentOdometricPose(const T &pose)
Brute-force overwrite robot odometry.
mrpt::math::TTwist2D m_GT_vel
Velocity in (x,y,omega)
const mrpt::math::TTwist2D & getCurrentGTVel() const
Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates.
const mrpt::math::TPose2D & getCurrentGTPose() const
Returns the instantaneous, ground truth pose in world coordinates.
Virtual base for velocity commands of different kinematic models of planar mobile robot...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
double getTime() const
Get the current simulation time.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
bool m_use_odo_error
Whether to corrupt odometry with noise.
double m_firmware_control_period
The period at which the low-level controller updates velocities (Default: 0.5 ms) ...
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::utils::DEG2RAD(1e-3), double Aphi_err_std=mrpt::utils::DEG2RAD(10e-3))
Enable/Disable odometry errors.
const mrpt::math::TPose2D & getCurrentOdometricPose() const
Returns the current pose according to (noisy) odometry.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019