Main MRPT website > C++ reference for MRPT 1.5.9
CVehicleSimul_Holo.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "kinematics-precomp.h" // Precompiled header
11 
13 #include <mrpt/math/wrap2pi.h>
14 
15 using namespace mrpt::kinematics;
16 
18 {
19  resetStatus();
20  resetTime();
21 }
22 
24 {
25  // Control:
26  if (m_vel_ramp_cmd.issue_time>=0 && m_time>m_vel_ramp_cmd.issue_time) // are we executing any cmd?
27  {
28  const double t = m_time - m_vel_ramp_cmd.issue_time;
29  const double T = m_vel_ramp_cmd.ramp_time;
30  const double vxi = m_vel_ramp_cmd.init_vel.vx;
31  const double vyi = m_vel_ramp_cmd.init_vel.vy;
32  const double wi = m_vel_ramp_cmd.init_vel.omega;
33  const double vxf = m_vel_ramp_cmd.target_vel_x;
34  const double vyf = m_vel_ramp_cmd.target_vel_y;
35 
36  // "Blending" for vx,vy
38  {
39  m_odometric_vel.vx = vxi + t*(vxf-vxi) /T;
40  m_odometric_vel.vy = vyi + t*(vyf-vyi) /T;
41  }
42  else
43  {
46  }
47 
48  // Ramp rotvel until aligned:
49  const double Aang = mrpt::math::wrapToPi(m_vel_ramp_cmd.dir - m_odometry.phi);
50  if (std::abs(Aang) < mrpt::utils::DEG2RAD(1.0)) {
51  m_odometric_vel.omega = .0; // we are aligned.
52  }
53  else
54  {
55  const double wf = mrpt::utils::sign(Aang) * std::abs(m_vel_ramp_cmd.rot_speed);
56  if (t <= m_vel_ramp_cmd.ramp_time)
57  {
58  m_odometric_vel.omega = wi + t*(wf - wi) / T;
59  }
60  else
61  {
63  }
64  }
65 
66  }
67 }
68 
70 {
72 }
73 
74 
75 void CVehicleSimul_Holo::sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
76 {
77  ASSERT_ABOVE_(ramp_time,0);
78 
80  m_vel_ramp_cmd.ramp_time = ramp_time;
81  m_vel_ramp_cmd.rot_speed = rot_speed;
83  m_vel_ramp_cmd.target_vel_x = cos(dir) * vel;
84  m_vel_ramp_cmd.target_vel_y = sin(dir) * vel;
85  m_vel_ramp_cmd.dir = dir;
86 }
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
GLdouble GLdouble t
Definition: glext.h:3610
#define ASSERT_ABOVE_(__A, __B)
double DEG2RAD(const double x)
Degrees to radians.
int sign(T x)
Returns the sign of X as "1" or "-1".
void sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
Sends a velocity cmd to the holonomic robot.
double issue_time
time when the cmd was issued. (<0: invalid, means there are no pending cmds to execute) ...
double vy
Velocity components: X,Y (m/s)
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
void resetTime()
Reset all simulator variables to 0 (except the simulation time).
void internal_simulControlStep(const double dt) MRPT_OVERRIDE
TVelRampCmd m_vel_ramp_cmd
the last cmd received from the user.
void internal_clear() MRPT_OVERRIDE
Resets all pending cmds.
double phi
Orientation (rads)
double omega
Angular velocity (rad/s)



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020