Main MRPT website > C++ reference for MRPT 1.9.9
CVehicleSimulVirtualBase.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, 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 
10 #include "kinematics-precomp.h" // Precompiled header
11 
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/random.h>
15 
16 using namespace mrpt::kinematics;
17 
19  : m_firmware_control_period(500e-6), m_use_odo_error(false)
20 {
21 }
22 
25 {
26  m_GT_pose = pose;
27 }
28 
30 {
31  using mrpt::math::TPose2D;
32  const double final_t = m_time + dt;
33  while (m_time <= final_t)
34  {
35  // Simulate movement during At:
36  TPose2D nextOdometry = m_odometry;
38  nextOdometry.y += m_odometric_vel.vy * m_firmware_control_period;
39  nextOdometry.phi += m_odometric_vel.omega * m_firmware_control_period;
40  mrpt::math::wrapToPiInPlace(nextOdometry.phi);
41 
42  TPose2D nextGT = m_GT_pose;
46  mrpt::math::wrapToPiInPlace(nextGT.phi);
47 
49 
50  // Now rotate our current Odo velocity into GT coordinates
53 
54  // Add some errors
55  if (m_use_odo_error)
56  {
57  nextGT.x += m_Ax_err_bias +
58  m_Ax_err_std *
61  nextGT.y += m_Ay_err_bias +
62  m_Ay_err_std *
65  nextGT.phi += m_Aphi_err_bias +
69  mrpt::math::wrapToPiInPlace(nextGT.phi);
70  }
71 
72  m_odometry = nextOdometry;
73  m_GT_pose = nextGT;
74 
75  m_time += m_firmware_control_period; // Move forward
76  }
77 }
78 
80 {
81  m_GT_pose = mrpt::math::TPose2D(.0, .0, .0);
82  m_GT_vel = mrpt::math::TTwist2D(.0, .0, .0);
83  m_odometry = mrpt::math::TPose2D(.0, .0, .0);
86 }
87 
90 {
91  mrpt::math::TTwist2D tl = this->m_GT_vel;
92  tl.rotate(-m_GT_pose.phi);
93  return tl;
94 }
95 
97  const
98 {
100  tl.rotate(-m_odometry.phi);
101  return tl;
102 }
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
double x
X,Y coordinates.
void simulateOneTimeStep(const double dt)
Runs the simulator during "dt" seconds.
mrpt::math::TPose2D m_GT_pose
ground truth pose in world coordinates.
virtual void internal_simulControlStep(const double dt)=0
mrpt::math::TTwist2D m_GT_vel
Velocity in (x,y,omega)
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:64
mrpt::math::TTwist2D getCurrentOdometricVelLocal() const
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame...
double vx
Velocity components: X,Y (m/s)
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.
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) ...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double phi
Orientation (rads)
double omega
Angular velocity (rad/s)
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
virtual void internal_clear()=0
Resets all pending cmds.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019