MRPT  2.0.4
CVehicleVelCmd_Holo.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 
18 
20 
22  double vel_, double dir_local_, double ramp_time_, double rot_speed_)
23  : vel(vel_),
24  dir_local(dir_local_),
25  ramp_time(ramp_time_),
26  rot_speed(rot_speed_)
27 {
28 }
29 
31 size_t CVehicleVelCmd_Holo::getVelCmdLength() const { return 4; }
32 std::string CVehicleVelCmd_Holo::getVelCmdDescription(const int index) const
33 {
34  switch (index)
35  {
36  case 0:
37  return "vel";
38  break;
39  case 1:
40  return "dir_local";
41  break;
42  case 2:
43  return "ramp_time";
44  break;
45  case 3:
46  return "rot_speed";
47  break;
48  default:
49  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
50  };
51 }
52 
53 double CVehicleVelCmd_Holo::getVelCmdElement(const int index) const
54 {
55  switch (index)
56  {
57  case 0:
58  return vel;
59  break;
60  case 1:
61  return dir_local;
62  break;
63  case 2:
64  return ramp_time;
65  break;
66  case 3:
67  return rot_speed;
68  break;
69  default:
70  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
71  };
72 }
73 
74 void CVehicleVelCmd_Holo::setVelCmdElement(const int index, const double val)
75 {
76  switch (index)
77  {
78  case 0:
79  vel = val;
80  break;
81  case 1:
82  dir_local = val;
83  break;
84  case 2:
85  ramp_time = val;
86  break;
87  case 3:
88  rot_speed = val;
89  break;
90  default:
91  THROW_EXCEPTION_FMT("index out of bounds: %i", index);
92  };
93 }
94 
96 {
97  return vel == 0 && rot_speed == 0;
98 }
99 
101 {
102  vel = dir_local = ramp_time = rot_speed = .0;
103 }
105  mrpt::serialization::CArchive& in, uint8_t version)
106 {
107  switch (version)
108  {
109  case 0:
110  in >> vel >> dir_local >> ramp_time >> rot_speed;
111  break;
112  default:
114  };
115 }
116 
117 uint8_t CVehicleVelCmd_Holo::serializeGetVersion() const { return 0; }
119 {
120  out << vel << dir_local << ramp_time << rot_speed;
121 }
122 
123 void CVehicleVelCmd_Holo::cmdVel_scale(double vel_scale)
124 {
125  vel *= vel_scale; // |(vx,vy)|
126  // rot_speed *= vel_scale; // rot_speed
127  // Note: No need to scale "rot_speed" since a holonomic robot's path will be
128  // invariant
129  // ramp_time: leave unchanged
130 }
131 
133  const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, const double beta,
134  const TVelCmdParams& params)
135 {
136  ASSERTMSG_(
137  params.robotMax_V_mps >= .0,
138  "[CVehicleVelCmd_Holo] `robotMax_V_mps` must be set to valid values: "
139  "either assign values programmatically or call loadConfigFile()");
140 
141  double f = 1.0;
142  if (vel > params.robotMax_V_mps) f = params.robotMax_V_mps / vel;
143 
144  vel *= f; // |(vx,vy)|
145  rot_speed *= f; // rot_speed
146  // ramp_time: leave unchanged
147  // Blending with "beta" not required, since the ramp_time already blends
148  // cmds for holo robots.
149 
150  return f;
151 }
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)
To be added to all CSerializable-classes implementation files.
void cmdVel_scale(double vel_scale) override
Scale the velocity command encoded in this object.
mrpt::vision::TStereoCalibParams params
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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).
int val
Definition: mrpt_jpeglib.h:957
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double getVelCmdElement(const int index) const override
Get each velocity command component.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
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...
bool isStopCmd() const override
Returns true if the command means "do not move" / "stop".
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
Parameters that may be used by cmdVel_limits() in any derived classes.
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020