Debian Codegraph @

libs/kinematics/src/CVehicleSimul_DiffDriven.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/CVehicleSimul_DiffDriven.h>
13
14 using namespace mrpt::kinematics;
15
16 CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven()
17 {
18 resetStatus();
19 resetTime();
20 }
21 CVehicleSimul_DiffDriven::~CVehicleSimul_DiffDriven() = default;
22 void CVehicleSimul_DiffDriven::internal_clear()
23 {
24 Command_Time = .0;
25 Command_v = .0;
26 Command_w = .0;
27 m_v = m_w = 0;
28 }
29
30 void CVehicleSimul_DiffDriven::internal_simulControlStep(const double AAt)
31 {
32 // Change velocities:
33 // ----------------------------------------------------------------
34 double elapsed_time = this->m_time - Command_Time;
35 elapsed_time -= cDELAY;
36 elapsed_time = std::max(0.0, elapsed_time);
37
38 if (cTAU == 0 && cDELAY == 0)
39 {
40 m_v = Command_v;
41 m_w = Command_w;
42 }
43 else
44 {
45 m_v = Command_v0 + (Command_v - Command_v0) * (1 - exp(-elapsed_time / cTAU));
46 m_w = Command_w0 + (Command_w - Command_w0) * (1 - exp(-elapsed_time / cTAU));
47 }
48
49 // Local to global frame:
50 m_odometric_vel.vx = cos(m_odometry.phi) * m_v;
51 m_odometric_vel.vy = sin(m_odometry.phi) * m_v;
52 m_odometric_vel.omega = m_w;
53 }
54
55 /*************************************************************************
56 Gives a movement command to the robot:
57 This actually saves the command for execution later on, if we have
58 to take into account the robot low-pass behavior in velocities.
59 *************************************************************************/
60 void CVehicleSimul_DiffDriven::movementCommand(double lin_vel, double ang_vel)
61 {
62 // Just save the command:
63 Command_Time = m_time;
64 Command_v = lin_vel;
65 Command_w = ang_vel;
66
67 // Current values:
68 Command_v0 = m_v;
69 Command_w0 = m_w;
70 }