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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019