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 }
mrpt::kinematics::CVehicleSimulVirtualBase::m_firmware_control_period
double m_firmware_control_period
The period at which the low-level controller updates velocities (Default: 0.5 ms)
Definition: CVehicleSimulVirtualBase.h:129
mrpt::math::TTwist2D::rotate
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
Definition: lightweight_geom_data.cpp:117
mrpt::kinematics::CVehicleSimulVirtualBase::~CVehicleSimulVirtualBase
virtual ~CVehicleSimulVirtualBase()
Definition: CVehicleSimulVirtualBase.cpp:23
mrpt::random::CRandomGenerator::drawGaussian1D_normalized
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
Definition: RandomGenerator.cpp:42
mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_std
double m_Aphi_err_std
Definition: CVehicleSimulVirtualBase.h:135
mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_std
double m_Ay_err_std
Definition: CVehicleSimulVirtualBase.h:134
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:195
mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus
void resetStatus()
Definition: CVehicleSimulVirtualBase.cpp:79
wrap2pi.h
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition: lightweight_geom_data.h:2152
mrpt::math::TTwist2D::vy
double vy
Definition: lightweight_geom_data.h:2152
mrpt::math::wrapToPiInPlace
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:64
random.h
CVehicleSimulVirtualBase.h
mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose
mrpt::math::TPose2D m_GT_pose
ground truth pose in world coordinates.
Definition: CVehicleSimulVirtualBase.h:120
kinematics-precomp.h
mrpt::kinematics::CVehicleSimulVirtualBase::CVehicleSimulVirtualBase
CVehicleSimulVirtualBase()
Definition: CVehicleSimulVirtualBase.cpp:18
mrpt::kinematics::CVehicleSimulVirtualBase::resetTime
void resetTime()
Reset all simulator variables to 0 (except the.
Definition: CVehicleSimulVirtualBase.cpp:88
mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_bias
double m_Aphi_err_bias
Definition: CVehicleSimulVirtualBase.h:135
mrpt::kinematics::CVehicleSimulVirtualBase::internal_simulControlStep
virtual void internal_simulControlStep(const double dt)=0
mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel
mrpt::math::TTwist2D m_GT_vel
Velocity in (x,y,omega)
Definition: CVehicleSimulVirtualBase.h:122
mrpt::kinematics::CVehicleSimulVirtualBase::setCurrentGTPose
void setCurrentGTPose(const mrpt::math::TPose2D &pose)
Brute-force move robot to target coordinates ("teleport")
Definition: CVehicleSimulVirtualBase.cpp:24
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:186
mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_std
double m_Ax_err_std
Definition: CVehicleSimulVirtualBase.h:133
mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVelLocal
mrpt::math::TTwist2D getCurrentOdometricVelLocal() const
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame.
Definition: CVehicleSimulVirtualBase.cpp:96
mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVelLocal
mrpt::math::TTwist2D getCurrentGTVelLocal() const
Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame.
Definition: CVehicleSimulVirtualBase.cpp:89
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
mrpt::kinematics::CVehicleSimulVirtualBase::internal_clear
virtual void internal_clear()=0
Resets all pending cmds.
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
Definition: CVehicleSimulVirtualBase.h:124
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry
mrpt::math::TPose2D m_odometry
Definition: CVehicleSimulVirtualBase.h:125
mrpt::kinematics
Definition: CKinematicChain.h:19
mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_bias
double m_Ay_err_bias
Definition: CVehicleSimulVirtualBase.h:134
mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_bias
double m_Ax_err_bias
Definition: CVehicleSimulVirtualBase.h:133
mrpt::math::TTwist2D::omega
double omega
Angular velocity (rad/s)
Definition: lightweight_geom_data.h:2154
mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep
void simulateOneTimeStep(const double dt)
Runs the simulator during "dt" seconds.
Definition: CVehicleSimulVirtualBase.cpp:29
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:193
mrpt::kinematics::CVehicleSimulVirtualBase::m_time
double m_time
simulation running time
Definition: CVehicleSimulVirtualBase.h:118
mrpt::math::TTwist2D
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: lightweight_geom_data.h:2145
mrpt::kinematics::CVehicleSimulVirtualBase::m_use_odo_error
bool m_use_odo_error
Whether to corrupt odometry with noise.
Definition: CVehicleSimulVirtualBase.h:132



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST