MRPT  2.0.4
CVehicleVelCmd_DiffDriven.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 
14 
15 using namespace mrpt::kinematics;
16 
19 
21 size_t CVehicleVelCmd_DiffDriven::getVelCmdLength() const { return 2; }
23  const int index) const
24 {
25  switch (index)
26  {
27  case 0:
28  return "lin_vel";
29  break;
30  case 1:
31  return "ang_vel";
32  break;
33  default:
34  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
35  };
36 }
37 
38 double CVehicleVelCmd_DiffDriven::getVelCmdElement(const int index) const
39 {
40  switch (index)
41  {
42  case 0:
43  return lin_vel;
44  break;
45  case 1:
46  return ang_vel;
47  break;
48  default:
49  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
50  };
51 }
52 
54  const int index, const double val)
55 {
56  switch (index)
57  {
58  case 0:
59  lin_vel = val;
60  break;
61  case 1:
62  ang_vel = val;
63  break;
64  default:
65  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
66  };
67 }
68 
70 {
71  return lin_vel == .0 && ang_vel == .0;
72 }
73 
76  mrpt::serialization::CArchive& in, uint8_t version)
77 {
78  switch (version)
79  {
80  case 0:
81  in >> lin_vel >> ang_vel;
82  break;
83  default:
85  };
86 }
87 
91 {
92  out << lin_vel << ang_vel;
93 }
94 
96 {
97  lin_vel *= vel_scale;
98  ang_vel *= vel_scale;
99 }
100 
102  const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, const double beta,
103  const TVelCmdParams& params)
104 {
105  ASSERT_(params.robotMax_V_mps > 0);
106  ASSERT_(params.robotMax_W_radps > 0);
107  const auto* prevcmd =
108  dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(
109  &prev_vel_cmd);
110  ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`");
111 
112  double speed_scale = filter_max_vw(lin_vel, ang_vel, params);
113 
114  // i.e. new behavior is nearly a pure rotation
115  if (std::abs(lin_vel) < 0.01)
116  { // thus, it's OK to blend the rotational component
117  ang_vel = beta * ang_vel + (1 - beta) * prevcmd->ang_vel;
118  }
119  else // there is a non-zero translational component
120  {
121  // must maintain the ratio of w to v (while filtering v)
122  double ratio = ang_vel / lin_vel;
123  // blend new v value
124  lin_vel = beta * lin_vel + (1 - beta) * prevcmd->lin_vel;
125  // ensure new w implements expected path curvature
126  ang_vel = ratio * lin_vel;
127 
128  speed_scale *= filter_max_vw(lin_vel, ang_vel, params);
129  }
130 
131  return speed_scale;
132 }
133 
135  double& v, double& w, const TVelCmdParams& p)
136 {
137  double speed_scale = 1.0;
138  // Ensure maximum speeds:
139  if (std::abs(v) > p.robotMax_V_mps)
140  {
141  // Scale:
142  const double F = std::abs(p.robotMax_V_mps / v);
143  v *= F;
144  w *= F;
145  speed_scale *= F;
146  }
147 
148  if (std::abs(w) > p.robotMax_W_radps)
149  {
150  // Scale:
151  const double F = std::abs(p.robotMax_W_radps / w);
152  v *= F;
153  w *= F;
154  speed_scale *= F;
155  }
156  return speed_scale;
157 }
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
double filter_max_vw(double &v, double &w, const TVelCmdParams &p)
mrpt::vision::TStereoCalibParams params
Virtual base for velocity commands of different kinematic models of planar mobile robot...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
size_t getVelCmdLength() const override
Get number of components in each velocity command.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) override
See base class docs.
void setToStop() override
Set to a command that means "do not move" / "stop".
int val
Definition: mrpt_jpeglib.h:957
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void setVelCmdElement(const int index, const double val) override
Set each velocity command component.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
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:69
Parameters that may be used by cmdVel_limits() in any derived classes.
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020