| 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/CVehicleSimulVirtualBase.h> |
| 13 |
#include <mrpt/math/wrap2pi.h> |
| 14 |
#include <mrpt/random.h> |
| 15 |
|
| 16 |
using namespace mrpt::kinematics; |
| 17 |
|
| 18 |
CVehicleSimulVirtualBase::CVehicleSimulVirtualBase() = default; |
| 19 |
|
| 20 |
CVehicleSimulVirtualBase::~CVehicleSimulVirtualBase() = default; |
| 21 |
void CVehicleSimulVirtualBase::setCurrentGTPose(const mrpt::math::TPose2D& pose) |
| 22 |
{ |
| 23 |
m_GT_pose = pose; |
| 24 |
} |
| 25 |
|
| 26 |
void CVehicleSimulVirtualBase::simulateOneTimeStep(const double dt) |
| 27 |
{ |
| 28 |
using mrpt::math::TPose2D; |
| 29 |
const double final_t = m_time + dt; |
| 30 |
while (m_time <= final_t) |
| 31 |
{ |
| 32 |
// Simulate movement during At: |
| 33 |
TPose2D nextOdometry = m_odometry; |
| 34 |
nextOdometry.x += m_odometric_vel.vx * m_firmware_control_period; |
| 35 |
nextOdometry.y += m_odometric_vel.vy * m_firmware_control_period; |
| 36 |
nextOdometry.phi += m_odometric_vel.omega * m_firmware_control_period; |
| 37 |
mrpt::math::wrapToPiInPlace(nextOdometry.phi); |
| 38 |
|
| 39 |
TPose2D gtDelta = m_GT_vel * m_firmware_control_period; |
| 40 |
// Add some errors |
| 41 |
if (m_use_odo_error) |
| 42 |
{ |
| 43 |
auto& rng = mrpt::random::getRandomGenerator(); |
| 44 |
gtDelta.x *= 1.0 + m_Ax_err_bias + m_Ax_err_std * rng.drawGaussian1D_normalized(); |
| 45 |
gtDelta.y *= 1.0 + m_Ay_err_bias + m_Ay_err_std * rng.drawGaussian1D_normalized(); |
| 46 |
gtDelta.phi *= 1.0 + m_Aphi_err_bias + m_Aphi_err_std * rng.drawGaussian1D_normalized(); |
| 47 |
mrpt::math::wrapToPiInPlace(gtDelta.phi); |
| 48 |
} |
| 49 |
|
| 50 |
TPose2D nextGT = { |
| 51 |
m_GT_pose.x + gtDelta.x, m_GT_pose.y + gtDelta.y, m_GT_pose.phi + gtDelta.phi}; |
| 52 |
mrpt::math::wrapToPiInPlace(nextGT.phi); |
| 53 |
|
| 54 |
this->internal_simulControlStep(m_firmware_control_period); |
| 55 |
|
| 56 |
// Now rotate our current Odo velocity into GT coordinates |
| 57 |
m_GT_vel = getCurrentOdometricVelLocal(); |
| 58 |
m_GT_vel.rotate(m_GT_pose.phi); |
| 59 |
|
| 60 |
m_odometry = nextOdometry; |
| 61 |
m_GT_pose = nextGT; |
| 62 |
|
| 63 |
m_time += m_firmware_control_period; // Move forward |
| 64 |
} |
| 65 |
} |
| 66 |
|
| 67 |
void CVehicleSimulVirtualBase::resetStatus() |
| 68 |
{ |
| 69 |
m_GT_pose = mrpt::math::TPose2D(.0, .0, .0); |
| 70 |
m_GT_vel = mrpt::math::TTwist2D(.0, .0, .0); |
| 71 |
m_odometry = mrpt::math::TPose2D(.0, .0, .0); |
| 72 |
m_odometric_vel = mrpt::math::TTwist2D(.0, .0, .0); |
| 73 |
internal_clear(); |
| 74 |
} |
| 75 |
|
| 76 |
void CVehicleSimulVirtualBase::resetTime() { m_time = .0; } |
| 77 |
mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentGTVelLocal() const |
| 78 |
{ |
| 79 |
mrpt::math::TTwist2D tl = this->m_GT_vel; |
| 80 |
tl.rotate(-m_GT_pose.phi); |
| 81 |
return tl; |
| 82 |
} |
| 83 |
|
| 84 |
mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentOdometricVelLocal() const |
| 85 |
{ |
| 86 |
mrpt::math::TTwist2D tl = this->m_odometric_vel; |
| 87 |
tl.rotate(-m_odometry.phi); |
| 88 |
return tl; |
| 89 |
} |