| 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 |
} |