MRPT  2.0.3
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 }
CVehicleVelCmd_Holo.h
mrpt::kinematics::CVehicleVelCmd_Holo::getVelCmdDescription
std::string getVelCmdDescription(const int index) const override
Get textual, human-readable description of each velocity command component.
Definition: CVehicleVelCmd_Holo.cpp:32
mrpt::kinematics::CVehicleVelCmd_Holo::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CVehicleVelCmd_Holo.cpp:118
mrpt::kinematics::CVehicleVelCmd_Holo::setToStop
void setToStop() override
Set to a command that means "do not move" / "stop".
Definition: CVehicleVelCmd_Holo.cpp:100
mrpt::kinematics::CVehicleVelCmd_Holo::cmdVel_scale
void cmdVel_scale(double vel_scale) override
Scale the velocity command encoded in this object.
Definition: CVehicleVelCmd_Holo.cpp:123
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams
Parameters that may be used by cmdVel_limits() in any derived classes.
Definition: CVehicleVelCmd.h:48
mrpt::kinematics::CVehicleVelCmd_Holo::vel
double vel
speed(m / s)
Definition: CVehicleVelCmd_Holo.h:24
mrpt::kinematics::CVehicleVelCmd_Holo::getVelCmdLength
size_t getVelCmdLength() const override
Get number of components in each velocity command.
Definition: CVehicleVelCmd_Holo.cpp:31
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::kinematics::CVehicleVelCmd_Holo::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CVehicleVelCmd_Holo.cpp:117
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt::kinematics::CVehicleVelCmd_Holo::setVelCmdElement
void setVelCmdElement(const int index, const double val) override
Set each velocity command component.
Definition: CVehicleVelCmd_Holo.cpp:74
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::kinematics::CVehicleVelCmd_Holo::ramp_time
double ramp_time
: Blending time between current and target time.
Definition: CVehicleVelCmd_Holo.h:29
val
int val
Definition: mrpt_jpeglib.h:957
mrpt::kinematics::CVehicleVelCmd_Holo::getVelCmdElement
double getVelCmdElement(const int index) const override
Get each velocity command component.
Definition: CVehicleVelCmd_Holo.cpp:53
mrpt::kinematics::CVehicleVelCmd_Holo::dir_local
double dir_local
: direction, relative to the current robot heading (radians).
Definition: CVehicleVelCmd_Holo.h:27
kinematics-precomp.h
mrpt::kinematics::CVehicleVelCmd_Holo::~CVehicleVelCmd_Holo
~CVehicleVelCmd_Holo() override
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::kinematics::CVehicleVelCmd_Holo::isStopCmd
bool isStopCmd() const override
Returns true if the command means "do not move" / "stop".
Definition: CVehicleVelCmd_Holo.cpp:95
mrpt::kinematics::CVehicleVelCmd_Holo::cmdVel_limits
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...
Definition: CVehicleVelCmd_Holo.cpp:132
mrpt::kinematics::CVehicleVelCmd_Holo::CVehicleVelCmd_Holo
CVehicleVelCmd_Holo()
mrpt::kinematics::CVehicleVelCmd_Holo
Kinematic model for.
Definition: CVehicleVelCmd_Holo.h:19
mrpt::kinematics::CVehicleVelCmd_Holo::rot_speed
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
Definition: CVehicleVelCmd_Holo.h:32
mrpt::kinematics
Definition: CKinematicChain.h:18
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::kinematics::CVehicleVelCmd
Virtual base for velocity commands of different kinematic models of planar mobile robot.
Definition: CVehicleVelCmd.h:20
mrpt::kinematics::CVehicleVelCmd_Holo::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CVehicleVelCmd_Holo.cpp:104



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020