Main MRPT website > C++ reference for MRPT 1.5.7
CVehicleVelCmd_Holo.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  vel(.0),
21  dir_local(.0),
22  ramp_time(.0),
23  rot_speed(.0)
24 {
25 }
26 
27 CVehicleVelCmd_Holo::CVehicleVelCmd_Holo(double vel, double dir_local, double ramp_time, double rot_speed) :
28  vel(vel),
29  dir_local(dir_local),
30  ramp_time(ramp_time),
31  rot_speed(rot_speed)
32 {
33 }
34 
36 {
37 }
38 
40 {
41  return 4;
42 }
43 
45 {
46  switch (index)
47  {
48  case 0: return "vel"; break;
49  case 1: return "dir_local"; break;
50  case 2: return "ramp_time"; break;
51  case 3: return "rot_speed"; break;
52  default:
53  THROW_EXCEPTION_FMT("index out of bounds: %i",index);
54  };
55 }
56 
58 {
59  switch (index)
60  {
61  case 0: return vel; break;
62  case 1: return dir_local; break;
63  case 2: return ramp_time; break;
64  case 3: return rot_speed; break;
65  default:
66  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
67  };
68 }
69 
70 void CVehicleVelCmd_Holo::setVelCmdElement(const int index, const double val)
71 {
72  switch (index)
73  {
74  case 0: vel=val; break;
75  case 1: dir_local = val; break;
76  case 2: ramp_time=val; break;
77  case 3: rot_speed=val; break;
78  default:
79  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
80  };
81 }
82 
84 {
85  return vel == 0 && rot_speed == 0;
86 }
87 
89 {
90  vel = dir_local = ramp_time = rot_speed = .0;
91 }
93 {
94  switch (version)
95  {
96  case 0:
97  in >> vel >> dir_local >> ramp_time >> rot_speed;
98  break;
99  default:
101  };
102 }
103 
105 {
106  if (version)
107  {
108  *version = 0;
109  return;
110  }
111  out << vel << dir_local << ramp_time << rot_speed;
112 }
113 
114 
115 void CVehicleVelCmd_Holo::cmdVel_scale(double vel_scale)
116 {
117  vel *= vel_scale; // |(vx,vy)|
118  // rot_speed *= vel_scale; // rot_speed
119  // Note: No need to scale "rot_speed" since a holonomic robot's path will be invariant
120  // ramp_time: leave unchanged
121 }
122 
123 double CVehicleVelCmd_Holo::cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params)
124 {
125  ASSERTMSG_(
126  params.robotMax_V_mps >= .0,
127  "[CVehicleVelCmd_Holo] `robotMax_V_mps` must be set to valid values: "
128  "either assign values programmatically or call loadConfigFile()");
129 
130  double f = 1.0;
131  if (vel>params.robotMax_V_mps) f = params.robotMax_V_mps / vel;
132 
133  vel *= f; // |(vx,vy)|
134  rot_speed *= f; // rot_speed
135  // ramp_time: leave unchanged
136  // Blending with "beta" not required, since the ramp_time already blends cmds for holo robots.
137 
138  return f;
139 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool isStopCmd() const MRPT_OVERRIDE
Returns true if the command 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.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
void setVelCmdElement(const int index, const double val) MRPT_OVERRIDE
Set each velocity command component.
Virtual base for velocity commands of different kinematic models of planar mobile robot...
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) MRPT_OVERRIDE
Updates this command, computing a blended version of beta (within [0,1]) of vel_cmd and 1-beta of pre...
double ramp_time
: Blending time between current and target time.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double getVelCmdElement(const int index) const MRPT_OVERRIDE
Get each velocity command component.
double dir_local
: direction, relative to the current robot heading (radians). 0 means forward.
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
GLsizei const GLchar ** string
Definition: glext.h:3919
void cmdVel_scale(double vel_scale) MRPT_OVERRIDE
Scale the velocity command encoded in this object.
void setToStop() MRPT_OVERRIDE
Set to a command that means "do not move" / "stop".
std::string getVelCmdDescription(const int index) const MRPT_OVERRIDE
Get textual, human-readable description of each velocity command component.
GLuint in
Definition: glext.h:6301
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...
Parameters that may be used by cmdVel_limits() in any derived classes.
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
Definition: glext.h:3514
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
size_t getVelCmdLength() const MRPT_OVERRIDE
Get number of components in each velocity command.



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