MRPT  1.9.9
CVehicleSimulVirtualBase.cpp
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 
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 
22 {
23  m_GT_pose = pose;
24 }
25 
27 {
28  using mrpt::math::TPose2D;
29  const double final_t = m_time + dt;
30  while (m_time <= final_t)
31  {
32  // Simulate movement during At:
33  TPose2D nextOdometry = m_odometry;
35  nextOdometry.y += m_odometric_vel.vy * m_firmware_control_period;
36  nextOdometry.phi += m_odometric_vel.omega * m_firmware_control_period;
37  mrpt::math::wrapToPiInPlace(nextOdometry.phi);
38 
39  TPose2D nextGT = m_GT_pose;
43  mrpt::math::wrapToPiInPlace(nextGT.phi);
44 
46 
47  // Now rotate our current Odo velocity into GT coordinates
50 
51  // Add some errors
52  if (m_use_odo_error)
53  {
54  nextGT.x += m_Ax_err_bias +
57  nextGT.y += m_Ay_err_bias +
60  nextGT.phi += m_Aphi_err_bias +
63  mrpt::math::wrapToPiInPlace(nextGT.phi);
64  }
65 
66  m_odometry = nextOdometry;
67  m_GT_pose = nextGT;
68 
69  m_time += m_firmware_control_period; // Move forward
70  }
71 }
72 
74 {
75  m_GT_pose = mrpt::math::TPose2D(.0, .0, .0);
76  m_GT_vel = mrpt::math::TTwist2D(.0, .0, .0);
77  m_odometry = mrpt::math::TPose2D(.0, .0, .0);
80 }
81 
84 {
85  mrpt::math::TTwist2D tl = this->m_GT_vel;
86  tl.rotate(-m_GT_pose.phi);
87  return tl;
88 }
89 
91  const
92 {
94  tl.rotate(-m_odometry.phi);
95  return tl;
96 }
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.
Definition: TPose2D.h:30
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)
Definition: TTwist2D.h:19
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
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)
Definition: TTwist2D.h:26
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) ...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double phi
Orientation (rads)
Definition: TPose2D.h:32
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
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: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019