Debian Codegraph @

libs/kinematics/src/CVehicleVelCmd_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/CVehicleVelCmd_Holo.h>
13 #include <mrpt/serialization/CArchive.h>
14
15 using namespace mrpt::kinematics;
16
17 IMPLEMENTS_SERIALIZABLE(CVehicleVelCmd_Holo, CVehicleVelCmd, mrpt::kinematics)
18
19 CVehicleVelCmd_Holo::CVehicleVelCmd_Holo() = default;
20
21 CVehicleVelCmd_Holo::CVehicleVelCmd_Holo(
22 double vel_, double dir_local_, double ramp_time_, double rot_speed_) :
23 vel(vel_), dir_local(dir_local_), ramp_time(ramp_time_), rot_speed(rot_speed_)
24 {
25 }
26
27 CVehicleVelCmd_Holo::~CVehicleVelCmd_Holo() = default;
28 size_t CVehicleVelCmd_Holo::getVelCmdLength() const { return 4; }
29 std::string CVehicleVelCmd_Holo::getVelCmdDescription(const int index) const
30 {
31 switch (index)
32 {
33 case 0:
34 return "vel";
35 break;
36 case 1:
37 return "dir_local";
38 break;
39 case 2:
40 return "ramp_time";
41 break;
42 case 3:
43 return "rot_speed";
44 break;
45 default:
46 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
47 };
48 }
49
50 double CVehicleVelCmd_Holo::getVelCmdElement(const int index) const
51 {
52 switch (index)
53 {
54 case 0:
55 return vel;
56 break;
57 case 1:
58 return dir_local;
59 break;
60 case 2:
61 return ramp_time;
62 break;
63 case 3:
64 return rot_speed;
65 break;
66 default:
67 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
68 };
69 }
70
71 void CVehicleVelCmd_Holo::setVelCmdElement(const int index, const double val)
72 {
73 switch (index)
74 {
75 case 0:
76 vel = val;
77 break;
78 case 1:
79 dir_local = val;
80 break;
81 case 2:
82 ramp_time = val;
83 break;
84 case 3:
85 rot_speed = val;
86 break;
87 default:
88 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
89 };
90 }
91
92 bool CVehicleVelCmd_Holo::isStopCmd() const { return vel == 0 && rot_speed == 0; }
93
94 void CVehicleVelCmd_Holo::setToStop() { vel = dir_local = ramp_time = rot_speed = .0; }
95 void CVehicleVelCmd_Holo::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
96 {
97 switch (version)
98 {
99 case 0:
100 in >> vel >> dir_local >> ramp_time >> rot_speed;
101 break;
102 default:
103 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
104 };
105 }
106
107 uint8_t CVehicleVelCmd_Holo::serializeGetVersion() const { return 0; }
108 void CVehicleVelCmd_Holo::serializeTo(mrpt::serialization::CArchive& out) const
109 {
110 out << vel << dir_local << ramp_time << rot_speed;
111 }
112
113 void CVehicleVelCmd_Holo::cmdVel_scale(double vel_scale)
114 {
115 vel *= vel_scale; // |(vx,vy)|
116 // rot_speed *= vel_scale; // rot_speed
117 // Note: No need to scale "rot_speed" since a holonomic robot's path will be
118 // invariant
119 // ramp_time: leave unchanged
120 }
121
122 double CVehicleVelCmd_Holo::cmdVel_limits(
123 const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd,
124 const double beta,
125 const TVelCmdParams& params)
126 {
127 ASSERTMSG_(
128 params.robotMax_V_mps >= .0,
129 "[CVehicleVelCmd_Holo] `robotMax_V_mps` must be set to valid values: "
130 "either assign values programmatically or call loadConfigFile()");
131
132 double f = 1.0;
133 if (vel > params.robotMax_V_mps) f = params.robotMax_V_mps / vel;
134
135 vel *= f; // |(vx,vy)|
136 rot_speed *= f; // rot_speed
137 // ramp_time: leave unchanged
138 // Blending with "beta" not required, since the ramp_time already blends
139 // cmds for holo robots.
140
141 return f;
142 }