MRPT  1.9.9
CPTG_DiffDrive_CCS.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 "nav-precomp.h" // Precomp header
12 #include <mrpt/system/os.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::nav;
17 using namespace mrpt::system;
18 
21 
23  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
24 {
26 
27  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K, double, cfg, sSection);
28 
29  // The constant curvature turning radius used in this PTG:
30  R = V_MAX / W_MAX;
31 }
33  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
34 {
36  const int WN = 25, WV = 30;
38 
39  cfg.write(
40  sSection, "K", K, WN, WV,
41  "K=+1 forward paths; K=-1 for backwards paths.");
42 
43  MRPT_END
44 }
45 
48 {
50 
51  switch (version)
52  {
53  case 0:
54  in >> K;
55  break;
56  default:
58  };
59 }
60 
63 {
65  out << K;
66 }
67 
69 {
70  char str[100];
71  os::sprintf(str, 100, "CPTG_DiffDrive_CCS,K=%i", (int)K);
72  return std::string(str);
73 }
74 
76  float alpha, float t, float x, float y, float phi, float& v, float& w) const
77 {
78  MRPT_UNUSED_PARAM(phi);
81  float u = fabs(alpha) * 0.5f; // 0.14758362f; // u = atan(0.5)* alpha /
82  // PI;
83 
84  if (t < u * R / V_MAX)
85  {
86  // l-
87  v = -V_MAX;
88  w = W_MAX;
89  }
90  else if (t < (u + M_PI / 2) * R / V_MAX)
91  {
92  // l+ pi/2
93  v = V_MAX;
94  w = W_MAX;
95  }
96  else
97  {
98  // s+:
99  v = V_MAX;
100  w = 0;
101  }
102 
103  // Turn in the opposite direction??
104  if (alpha < 0) w *= -1;
105 
106  v *= K;
107  w *= K;
108 }
109 
110 bool CPTG_DiffDrive_CCS::PTG_IsIntoDomain(double x, double y) const
111 {
112  // If signs of K and X are different, it is into the domain:
113  if ((K * x) < 0) return true;
114 
115  if (fabs(y) >= R)
116  {
117  // Segmento de arriba:
118  return (fabs(x) <= R);
119  }
120  else
121  {
122  // The circle at (0,R):
123  return (square(x) + square(fabs(y) - R)) <= R;
124  }
125 }
126 
128 {
130  K = +1.0;
131 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_CCS, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_CCS
void internal_readFromStream(mrpt::serialization::CArchive &in) override
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
T square(const T x)
Inline function for the square of a number.
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
GLsizei const GLchar ** string
Definition: glext.h:4101
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
const float R
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
GLenum GLint GLint y
Definition: glext.h:3538
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
A PTG for optimal paths of type "C|C,S" (as named in PTG papers).
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3538
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
void ptgDiffDriveSteeringFunction(float alpha, float t, float x, float y, float phi, float &v, float &w) const override
The main method to be implemented in derived classes: it defines the differential-driven differential...
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:189
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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