Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
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 
24 {
25 }
26 
28 {
29  m_GT_pose=pose;
30 }
31 
33 {
34  using mrpt::math::TPose2D;
35  const double final_t = m_time + dt;
36  while (m_time <= final_t)
37  {
38  // Simulate movement during At:
39  TPose2D nextOdometry = m_odometry;
43  mrpt::math::wrapToPiInPlace(nextOdometry.phi);
44 
45  TPose2D nextGT = m_GT_pose;
49  mrpt::math::wrapToPiInPlace(nextGT.phi);
50 
52 
53  //Now rotate our current Odo velocity into GT coordinates
56 
57  //Add some errors
58  if (m_use_odo_error)
59  {
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 }
73 
75 {
76  m_GT_pose= mrpt::math::TPose2D(.0,.0,.0);
77  m_GT_vel = mrpt::math::TTwist2D(.0,.0,.0);
78  m_odometry = mrpt::math::TPose2D(.0,.0,.0);
81 }
82 
84 {
85  m_time = .0;
86 }
87 
89 {
90  mrpt::math::TTwist2D tl = this->m_GT_vel;
91  tl.rotate(-m_GT_pose.phi);
92  return tl;
93 }
94 
96 {
98  tl.rotate(-m_odometry.phi);
99  return tl;
100 }
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)
void simulateOneTimeStep(const double dt)
Runs the simulator during "dt" seconds.
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::math::TPose2D m_GT_pose
ground truth pose in world coordinates.
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
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:61
mrpt::math::TTwist2D getCurrentOdometricVelLocal() const
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame...
double vy
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 simulation time).
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) ...
double phi
Orientation (rads)
double omega
Angular velocity (rad/s)
virtual void internal_clear()=0
Resets all pending cmds.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019