Main MRPT website > C++ reference for MRPT 1.5.7
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 
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) MRPT_OVERRIDE
See base class docs.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#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,...)
size_t getVelCmdLength() const MRPT_OVERRIDE
Get number of components in each velocity command.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
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
GLuint index
Definition: glext.h:3891
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
int val
Definition: mrpt_jpeglib.h:953
int version
Definition: mrpt_jpeglib.h:898
void cmdVel_scale(double vel_scale) MRPT_OVERRIDE
See docs of method in base class.
GLsizei const GLchar ** string
Definition: glext.h:3919
void setToStop() MRPT_OVERRIDE
Set to a command that means "do not move" / "stop".
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
const GLdouble * v
Definition: glext.h:3603
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
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...
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)
GLfloat GLfloat p
Definition: glext.h:5587
GLenum const GLfloat * params
Definition: glext.h:3514
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019