MRPT  1.9.9
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-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 
17 
19  : vel(.0), dir_local(.0), ramp_time(.0), rot_speed(.0)
20 {
21 }
22 
24  double vel_, double dir_local_, double ramp_time_, double rot_speed_)
25  : vel(vel_),
26  dir_local(dir_local_),
27  ramp_time(ramp_time_),
28  rot_speed(rot_speed_)
29 {
30 }
31 
33 size_t CVehicleVelCmd_Holo::getVelCmdLength() const { return 4; }
35 {
36  switch (index)
37  {
38  case 0:
39  return "vel";
40  break;
41  case 1:
42  return "dir_local";
43  break;
44  case 2:
45  return "ramp_time";
46  break;
47  case 3:
48  return "rot_speed";
49  break;
50  default:
51  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
52  };
53 }
54 
56 {
57  switch (index)
58  {
59  case 0:
60  return vel;
61  break;
62  case 1:
63  return dir_local;
64  break;
65  case 2:
66  return ramp_time;
67  break;
68  case 3:
69  return rot_speed;
70  break;
71  default:
72  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
73  };
74 }
75 
76 void CVehicleVelCmd_Holo::setVelCmdElement(const int index, const double val)
77 {
78  switch (index)
79  {
80  case 0:
81  vel = val;
82  break;
83  case 1:
84  dir_local = val;
85  break;
86  case 2:
87  ramp_time = val;
88  break;
89  case 3:
90  rot_speed = val;
91  break;
92  default:
93  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
94  };
95 }
96 
98 {
99  return vel == 0 && rot_speed == 0;
100 }
101 
103 {
104  vel = dir_local = ramp_time = rot_speed = .0;
105 }
108 {
109  switch (version)
110  {
111  case 0:
112  in >> vel >> dir_local >> ramp_time >> rot_speed;
113  break;
114  default:
116  };
117 }
118 
121 {
122  out << vel << dir_local << ramp_time << rot_speed;
123 }
124 
125 void CVehicleVelCmd_Holo::cmdVel_scale(double vel_scale)
126 {
127  vel *= vel_scale; // |(vx,vy)|
128  // rot_speed *= vel_scale; // rot_speed
129  // Note: No need to scale "rot_speed" since a holonomic robot's path will be
130  // invariant
131  // ramp_time: leave unchanged
132 }
133 
135  const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, const double beta,
136  const TVelCmdParams& params)
137 {
138  ASSERTMSG_(
139  params.robotMax_V_mps >= .0,
140  "[CVehicleVelCmd_Holo] `robotMax_V_mps` must be set to valid values: "
141  "either assign values programmatically or call loadConfigFile()");
142 
143  double f = 1.0;
144  if (vel > params.robotMax_V_mps) f = params.robotMax_V_mps / vel;
145 
146  vel *= f; // |(vx,vy)|
147  rot_speed *= f; // rot_speed
148  // ramp_time: leave unchanged
149  // Blending with "beta" not required, since the ramp_time already blends
150  // cmds for holo robots.
151 
152  return f;
153 }
std::string getVelCmdDescription(const int index) const override
Get textual, human-readable description of each velocity command component.
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)
This must be inserted in all CSerializable classes implementation files.
void cmdVel_scale(double vel_scale) override
Scale the velocity command encoded in this object.
size_t getVelCmdLength() const override
Get number of components in each velocity command.
void setToStop() override
Set to a command that means "do not move" / "stop".
Virtual base for velocity commands of different kinematic models of planar mobile robot...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
double ramp_time
: Blending time between current and target time.
void setVelCmdElement(const int index, const double val) override
Set each velocity command component.
double dir_local
: direction, relative to the current robot heading (radians).
GLuint index
Definition: glext.h:4054
int val
Definition: mrpt_jpeglib.h:955
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double getVelCmdElement(const int index) const override
Get each velocity command component.
GLsizei const GLchar ** string
Definition: glext.h:4101
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
double cmdVel_limits(const mrpt::kinematics::CVehicleVelCmd &prev_vel_cmd, const double beta, const TVelCmdParams &params) override
Updates this command, computing a blended version of beta (within [0,1]) of vel_cmd and 1-beta of pre...
GLuint in
Definition: glext.h:7274
bool isStopCmd() const override
Returns true if the command means "do not move" / "stop".
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
Parameters that may be used by cmdVel_limits() in any derived classes.
GLenum const GLfloat * params
Definition: glext.h:3534
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020