MRPT  1.9.9
CVehicleSimul_Holo.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 
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 &&
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  {
47  }
48 
49  // Ramp rotvel until aligned:
50  const double Aang =
52  if (std::abs(Aang) < mrpt::DEG2RAD(1.0))
53  {
54  m_odometric_vel.omega = .0; // we are aligned.
55  }
56  else
57  {
58  const double wf =
59  mrpt::sign(Aang) * std::abs(m_vel_ramp_cmd.rot_speed);
60  if (t <= m_vel_ramp_cmd.ramp_time)
61  {
62  m_odometric_vel.omega = wi + t * (wf - wi) / T;
63  }
64  else
65  {
67  }
68  }
69  }
70 }
71 
74  double vel, double dir, double ramp_time, double rot_speed)
75 {
76  ASSERT_ABOVE_(ramp_time, 0);
77 
79  m_vel_ramp_cmd.ramp_time = ramp_time;
80  m_vel_ramp_cmd.rot_speed = rot_speed;
82  m_vel_ramp_cmd.target_vel_x = cos(dir) * vel;
83  m_vel_ramp_cmd.target_vel_y = sin(dir) * vel;
85 }
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
void sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
Sends a velocity cmd to the holonomic robot.
void internal_simulControlStep(const double dt) override
constexpr double DEG2RAD(const double x)
Degrees to radians.
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
double issue_time
time when the cmd was issued.
int sign(T x)
Returns the sign of X as "1" or "-1".
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void internal_clear() override
Resets all pending cmds.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
void resetTime()
Reset all simulator variables to 0 (except the.
TVelRampCmd m_vel_ramp_cmd
the last cmd received from the user.
double phi
Orientation (rads)
Definition: TPose2D.h:32
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020