Main MRPT website > C++ reference for MRPT 1.5.6
CVehicleVelCmd_DiffDriven.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
12 #include <mrpt/utils/CStream.h>
13 
14 using namespace mrpt::kinematics;
15 using namespace mrpt::utils;
16 
18 
20  lin_vel(.0),
21  ang_vel(.0)
22 {
23 }
25 {
26 }
28 {
29  return 2;
30 }
31 
33 {
34  switch (index)
35  {
36  case 0: return "lin_vel"; break;
37  case 1: return "ang_vel"; break;
38  default:
39  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
40  };
41 }
42 
44 {
45  switch (index)
46  {
47  case 0: return lin_vel; break;
48  case 1: return ang_vel; break;
49  default:
50  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
51  };
52 }
53 
55 {
56  switch (index)
57  {
58  case 0: lin_vel = val; break;
59  case 1: ang_vel = val; break;
60  default:
61  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
62  };
63 }
64 
66 {
67  return lin_vel == .0 && ang_vel == .0;
68 }
69 
71 {
72  lin_vel = ang_vel = .0;
73 }
74 
76 {
77  switch (version)
78  {
79  case 0:
80  in >> lin_vel >> ang_vel;
81  break;
82  default:
84  };
85 }
86 
88 {
89  if (version)
90  {
91  *version = 0;
92  return;
93  }
94  out << lin_vel << ang_vel;
95 }
96 
98 {
99  lin_vel *= vel_scale;
100  ang_vel *= vel_scale;
101 }
102 
104 {
105  ASSERT_(params.robotMax_V_mps>0);
106  ASSERT_(params.robotMax_W_radps>0);
107  const mrpt::kinematics::CVehicleVelCmd_DiffDriven *prevcmd = dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(&prev_vel_cmd);
108  ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`");
109 
110  double speed_scale = filter_max_vw(lin_vel, ang_vel, params);
111 
112  if (std::abs(lin_vel) < 0.01) // i.e. new behavior is nearly a pure rotation
113  { // thus, it's OK to blend the rotational component
114  ang_vel = beta*ang_vel + (1 - beta)*prevcmd->ang_vel;
115  }
116  else // there is a non-zero translational component
117  {
118  // must maintain the ratio of w to v (while filtering v)
119  float ratio = ang_vel / lin_vel;
120  lin_vel = beta*lin_vel + (1 - beta)*prevcmd->lin_vel; // blend new v value
121  ang_vel = ratio * lin_vel; // ensure new w implements expected path curvature
122 
123  speed_scale*= filter_max_vw(lin_vel, ang_vel, params);
124  }
125 
126  return speed_scale;
127 }
128 
130 {
131  double speed_scale = 1.0;
132  // Ensure maximum speeds:
133  if (std::abs(v) > p.robotMax_V_mps) {
134  // Scale:
135  const double F = std::abs(p.robotMax_V_mps / v);
136  v *= F;
137  w *= F;
138  speed_scale *= F;
139  }
140 
141  if (std::abs(w) > p.robotMax_W_radps) {
142  // Scale:
143  const double F = std::abs(p.robotMax_W_radps / w);
144  v *= F;
145  w *= F;
146  speed_scale *= F;
147  }
148  return speed_scale;
149 
150 }
151 
const GLdouble * v
Definition: glew.h:1296
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) MRPT_OVERRIDE
See base class docs.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double filter_max_vw(double &v, double &w, const TVelCmdParams &p)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
size_t getVelCmdLength() const MRPT_OVERRIDE
Get number of components in each velocity command.
GLuint in
Definition: glew.h:7146
Virtual base for velocity commands of different kinematic models of planar mobile robot...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
double robotMax_W_radps
Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set]...
int version
Definition: mrpt_jpeglib.h:898
void cmdVel_scale(double vel_scale) MRPT_OVERRIDE
See docs of method in base class.
GLfloat GLfloat p
Definition: glew.h:10113
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void setToStop() MRPT_OVERRIDE
Set to a command that means "do not move" / "stop".
GLsizei const GLcharARB ** string
Definition: glew.h:3293
GLuint index
Definition: glew.h:1721
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
double robotMax_V_mps
Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set]...
GLuint GLfloat * val
Definition: glew.h:7785
std::string getVelCmdDescription(const int index) const MRPT_OVERRIDE
Get textual, human-readable description of each velocity command component.
double getVelCmdElement(const int index) const MRPT_OVERRIDE
Get each velocity command component.
Parameters that may be used by cmdVel_limits() in any derived classes.
#define ASSERTMSG_(f, __ERROR_MSG)
void setVelCmdElement(const int index, const double val) MRPT_OVERRIDE
Set each velocity command component.
Kinematic model for Ackermann-like or differential-driven vehicles.
bool isStopCmd() const MRPT_OVERRIDE
Returns true if the command means "do not move" / "stop".



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018