Debian Codegraph @

libs/kinematics/src/CVehicleSimul_Holo.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_Holo.h>
13 #include <mrpt/math/wrap2pi.h>
14
15 using namespace mrpt::kinematics;
16
17 CVehicleSimul_Holo::CVehicleSimul_Holo()
18 {
19 resetStatus();
20 resetTime();
21 }
22
23 void CVehicleSimul_Holo::internal_simulControlStep(const double dt)
24 {
25 // Control:
26 if (m_vel_ramp_cmd.issue_time >= 0 &&
27 m_time > m_vel_ramp_cmd.issue_time) // are we executing any cmd?
28 {
29 const double t = m_time - m_vel_ramp_cmd.issue_time;
30 const double T = m_vel_ramp_cmd.ramp_time;
31 const double vxi = m_vel_ramp_cmd.init_vel.vx;
32 const double vyi = m_vel_ramp_cmd.init_vel.vy;
33 const double wi = m_vel_ramp_cmd.init_vel.omega;
34 const double vxf = m_vel_ramp_cmd.target_vel_x;
35 const double vyf = m_vel_ramp_cmd.target_vel_y;
36
37 // "Blending" for vx,vy
38 if (t <= m_vel_ramp_cmd.ramp_time)
39 {
40 m_odometric_vel.vx = vxi + t * (vxf - vxi) / T;
41 m_odometric_vel.vy = vyi + t * (vyf - vyi) / T;
42 }
43 else
44 {
45 m_odometric_vel.vx = m_vel_ramp_cmd.target_vel_x;
46 m_odometric_vel.vy = m_vel_ramp_cmd.target_vel_y;
47 }
48
49 // Ramp rotvel until aligned:
50 const double Aang = mrpt::math::wrapToPi(m_vel_ramp_cmd.dir - m_odometry.phi);
51 if (std::abs(Aang) < mrpt::DEG2RAD(1.0))
52 {
53 m_odometric_vel.omega = .0; // we are aligned.
54 }
55 else
56 {
57 const double wf = mrpt::sign(Aang) * std::abs(m_vel_ramp_cmd.rot_speed);
58 if (t <= m_vel_ramp_cmd.ramp_time)
59 {
60 m_odometric_vel.omega = wi + t * (wf - wi) / T;
61 }
62 else
63 {
64 m_odometric_vel.omega = wf;
65 }
66 }
67 }
68 }
69
70 void CVehicleSimul_Holo::internal_clear() { m_vel_ramp_cmd = TVelRampCmd(); }
71 void CVehicleSimul_Holo::sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
72 {
73 ASSERT_GT_(ramp_time, 0);
74
75 m_vel_ramp_cmd.issue_time = m_time;
76 m_vel_ramp_cmd.ramp_time = ramp_time;
77 m_vel_ramp_cmd.rot_speed = rot_speed;
78 m_vel_ramp_cmd.init_vel = m_odometric_vel;
79 m_vel_ramp_cmd.target_vel_x = cos(dir) * vel;
80 m_vel_ramp_cmd.target_vel_y = sin(dir) * vel;
81 m_vel_ramp_cmd.dir = dir;
82 }