Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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
13 
14 using namespace mrpt::kinematics;
15 
18 
20 size_t CVehicleVelCmd_DiffDriven::getVelCmdLength() const { return 2; }
22  const int index) const
23 {
24  switch (index)
25  {
26  case 0:
27  return "lin_vel";
28  break;
29  case 1:
30  return "ang_vel";
31  break;
32  default:
33  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
34  };
35 }
36 
38 {
39  switch (index)
40  {
41  case 0:
42  return lin_vel;
43  break;
44  case 1:
45  return ang_vel;
46  break;
47  default:
48  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
49  };
50 }
51 
53  const int index, const double val)
54 {
55  switch (index)
56  {
57  case 0:
58  lin_vel = val;
59  break;
60  case 1:
61  ang_vel = val;
62  break;
63  default:
64  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
65  };
66 }
67 
69 {
70  return lin_vel == .0 && ang_vel == .0;
71 }
72 
76 {
77  switch (version)
78  {
79  case 0:
80  in >> lin_vel >> ang_vel;
81  break;
82  default:
84  };
85 }
86 
90 {
91  out << lin_vel << ang_vel;
92 }
93 
95 {
96  lin_vel *= vel_scale;
97  ang_vel *= vel_scale;
98 }
99 
101  const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, const double beta,
102  const TVelCmdParams& params)
103 {
104  ASSERT_(params.robotMax_V_mps > 0);
105  ASSERT_(params.robotMax_W_radps > 0);
107  dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(
108  &prev_vel_cmd);
109  ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`");
110 
111  double speed_scale = filter_max_vw(lin_vel, ang_vel, params);
112 
113  if (std::abs(lin_vel) <
114  0.01) // i.e. new behavior is nearly a pure rotation
115  { // thus, it's OK to blend the rotational component
116  ang_vel = beta * ang_vel + (1 - beta) * prevcmd->ang_vel;
117  }
118  else // there is a non-zero translational component
119  {
120  // must maintain the ratio of w to v (while filtering v)
121  float ratio = ang_vel / lin_vel;
122  lin_vel = beta * lin_vel +
123  (1 - beta) * prevcmd->lin_vel; // blend new v value
124  ang_vel =
125  ratio * lin_vel; // ensure new w implements expected path curvature
126 
127  speed_scale *= filter_max_vw(lin_vel, ang_vel, params);
128  }
129 
130  return speed_scale;
131 }
132 
134  double& v, double& w, const TVelCmdParams& p)
135 {
136  double speed_scale = 1.0;
137  // Ensure maximum speeds:
138  if (std::abs(v) > p.robotMax_V_mps)
139  {
140  // Scale:
141  const double F = std::abs(p.robotMax_V_mps / v);
142  v *= F;
143  w *= F;
144  speed_scale *= F;
145  }
146 
147  if (std::abs(w) > p.robotMax_W_radps)
148  {
149  // Scale:
150  const double F = std::abs(p.robotMax_W_radps / w);
151  v *= F;
152  w *= F;
153  speed_scale *= F;
154  }
155  return speed_scale;
156 }
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double filter_max_vw(double &v, double &w, const TVelCmdParams &p)
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
Virtual base for velocity commands of different kinematic models of planar mobile robot...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) override
See base class docs.
GLuint index
Definition: glext.h:4054
void setToStop() override
Set to a command that means "do not move" / "stop".
int val
Definition: mrpt_jpeglib.h:955
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void setVelCmdElement(const int index, const double val) override
Set each velocity command component.
GLsizei const GLchar ** string
Definition: glext.h:4101
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
const GLdouble * v
Definition: glext.h:3678
IMPLEMENTS_SERIALIZABLE(CVehicleVelCmd_DiffDriven, CVehicleVelCmd, mrpt::kinematics) CVehicleVelCmd_DiffDriven
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
size_t getVelCmdLength() const override
Get number of components in each velocity command.
GLuint in
Definition: glext.h:7274
bool isStopCmd() const override
Returns true if the command means "do not move" / "stop".
double getVelCmdElement(const int index) const override
Get each velocity command component.
std::string getVelCmdDescription(const int index) const override
Get textual, human-readable description of each velocity command component.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
Parameters that may be used by cmdVel_limits() in any derived classes.
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void cmdVel_scale(double vel_scale) override
See docs of method in base class.
Kinematic model for Ackermann-like or differential-driven vehicles.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019