Debian Codegraph @

libs/kinematics/src/CVehicleVelCmd_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/CVehicleVelCmd_DiffDriven.h>
13 #include <mrpt/serialization/CArchive.h>
14
15 using namespace mrpt::kinematics;
16
17 IMPLEMENTS_SERIALIZABLE(CVehicleVelCmd_DiffDriven, CVehicleVelCmd, mrpt::kinematics)
18
19 CVehicleVelCmd_DiffDriven::~CVehicleVelCmd_DiffDriven() = default;
20 size_t CVehicleVelCmd_DiffDriven::getVelCmdLength() const { return 2; }
21 std::string CVehicleVelCmd_DiffDriven::getVelCmdDescription(const int index) const
22 {
23 switch (index)
24 {
25 case 0:
26 return "lin_vel";
27 break;
28 case 1:
29 return "ang_vel";
30 break;
31 default:
32 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
33 };
34 }
35
36 double CVehicleVelCmd_DiffDriven::getVelCmdElement(const int index) const
37 {
38 switch (index)
39 {
40 case 0:
41 return lin_vel;
42 break;
43 case 1:
44 return ang_vel;
45 break;
46 default:
47 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
48 };
49 }
50
51 void CVehicleVelCmd_DiffDriven::setVelCmdElement(const int index, const double val)
52 {
53 switch (index)
54 {
55 case 0:
56 lin_vel = val;
57 break;
58 case 1:
59 ang_vel = val;
60 break;
61 default:
62 THROW_EXCEPTION_FMT("index out of bounds: %i", index);
63 };
64 }
65
66 bool CVehicleVelCmd_DiffDriven::isStopCmd() const { return lin_vel == .0 && ang_vel == .0; }
67
68 void CVehicleVelCmd_DiffDriven::setToStop() { lin_vel = ang_vel = .0; }
69 void CVehicleVelCmd_DiffDriven::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
70 {
71 switch (version)
72 {
73 case 0:
74 in >> lin_vel >> ang_vel;
75 break;
76 default:
77 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
78 };
79 }
80
81 uint8_t CVehicleVelCmd_DiffDriven::serializeGetVersion() const { return 0; }
82 void CVehicleVelCmd_DiffDriven::serializeTo(mrpt::serialization::CArchive& out) const
83 {
84 out << lin_vel << ang_vel;
85 }
86
87 void CVehicleVelCmd_DiffDriven::cmdVel_scale(double vel_scale)
88 {
89 lin_vel *= vel_scale;
90 ang_vel *= vel_scale;
91 }
92
93 double CVehicleVelCmd_DiffDriven::cmdVel_limits(
94 const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd,
95 const double beta,
96 const TVelCmdParams& params)
97 {
98 ASSERT_(params.robotMax_V_mps > 0);
99 ASSERT_(params.robotMax_W_radps > 0);
100 const auto* prevcmd =
101 dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(&prev_vel_cmd);
102 ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`");
103
104 double speed_scale = filter_max_vw(lin_vel, ang_vel, params);
105
106 // i.e. new behavior is nearly a pure rotation
107 if (std::abs(lin_vel) < 0.01)
108 { // thus, it's OK to blend the rotational component
109 ang_vel = beta * ang_vel + (1 - beta) * prevcmd->ang_vel;
110 }
111 else // there is a non-zero translational component
112 {
113 // must maintain the ratio of w to v (while filtering v)
114 double ratio = ang_vel / lin_vel;
115 // blend new v value
116 lin_vel = beta * lin_vel + (1 - beta) * prevcmd->lin_vel;
117 // ensure new w implements expected path curvature
118 ang_vel = ratio * lin_vel;
119
120 speed_scale *= filter_max_vw(lin_vel, ang_vel, params);
121 }
122
123 return speed_scale;
124 }
125
126 double CVehicleVelCmd_DiffDriven::filter_max_vw(double& v, double& w, const TVelCmdParams& p)
127 {
128 double speed_scale = 1.0;
129 // Ensure maximum speeds:
130 if (std::abs(v) > p.robotMax_V_mps)
131 {
132 // Scale:
133 const double F = std::abs(p.robotMax_V_mps / v);
134 v *= F;
135 w *= F;
136 speed_scale *= F;
137 }
138
139 if (std::abs(w) > p.robotMax_W_radps)
140 {
141 // Scale:
142 const double F = std::abs(p.robotMax_W_radps / w);
143 v *= F;
144 w *= F;
145 speed_scale *= F;
146 }
147 return speed_scale;
148 }