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