Debian Codegraph @

libs/kinematics/src/CVehicleSimulVirtualBase.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/kinematics/CVehicleSimulVirtualBase.h>
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/random.h>
15
16 using namespace mrpt::kinematics;
17
18 CVehicleSimulVirtualBase::CVehicleSimulVirtualBase() = default;
19
20 CVehicleSimulVirtualBase::~CVehicleSimulVirtualBase() = default;
21 void CVehicleSimulVirtualBase::setCurrentGTPose(const mrpt::math::TPose2D& pose)
22 {
23 m_GT_pose = pose;
24 }
25
26 void CVehicleSimulVirtualBase::simulateOneTimeStep(const double dt)
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;
34 nextOdometry.x += m_odometric_vel.vx * m_firmware_control_period;
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 gtDelta = m_GT_vel * m_firmware_control_period;
40 // Add some errors
41 if (m_use_odo_error)
42 {
43 auto& rng = mrpt::random::getRandomGenerator();
44 gtDelta.x *= 1.0 + m_Ax_err_bias + m_Ax_err_std * rng.drawGaussian1D_normalized();
45 gtDelta.y *= 1.0 + m_Ay_err_bias + m_Ay_err_std * rng.drawGaussian1D_normalized();
46 gtDelta.phi *= 1.0 + m_Aphi_err_bias + m_Aphi_err_std * rng.drawGaussian1D_normalized();
47 mrpt::math::wrapToPiInPlace(gtDelta.phi);
48 }
49
50 TPose2D nextGT = {
51 m_GT_pose.x + gtDelta.x, m_GT_pose.y + gtDelta.y, m_GT_pose.phi + gtDelta.phi};
52 mrpt::math::wrapToPiInPlace(nextGT.phi);
53
54 this->internal_simulControlStep(m_firmware_control_period);
55
56 // Now rotate our current Odo velocity into GT coordinates
57 m_GT_vel = getCurrentOdometricVelLocal();
58 m_GT_vel.rotate(m_GT_pose.phi);
59
60 m_odometry = nextOdometry;
61 m_GT_pose = nextGT;
62
63 m_time += m_firmware_control_period; // Move forward
64 }
65 }
66
67 void CVehicleSimulVirtualBase::resetStatus()
68 {
69 m_GT_pose = mrpt::math::TPose2D(.0, .0, .0);
70 m_GT_vel = mrpt::math::TTwist2D(.0, .0, .0);
71 m_odometry = mrpt::math::TPose2D(.0, .0, .0);
72 m_odometric_vel = mrpt::math::TTwist2D(.0, .0, .0);
73 internal_clear();
74 }
75
76 void CVehicleSimulVirtualBase::resetTime() { m_time = .0; }
77 mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentGTVelLocal() const
78 {
79 mrpt::math::TTwist2D tl = this->m_GT_vel;
80 tl.rotate(-m_GT_pose.phi);
81 return tl;
82 }
83
84 mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentOdometricVelLocal() const
85 {
86 mrpt::math::TTwist2D tl = this->m_odometric_vel;
87 tl.rotate(-m_odometry.phi);
88 return tl;
89 }