Main MRPT website > C++ reference for MRPT 1.5.7
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-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 "nav-precomp.h" // Precomp header
12 #include <mrpt/system/os.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::nav;
16 using namespace mrpt::system;
17 using namespace mrpt::utils;
18 
20 
21 void CPTG_DiffDrive_CCS::loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection)
22 {
24 
25  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K,double, cfg,sSection);
26 
27  // The constant curvature turning radius used in this PTG:
28  R = V_MAX / W_MAX;
29 }
31 {
33  const int WN = 25, WV = 30;
35 
36  cfg.write(sSection,"K",K, WN,WV, "K=+1 forward paths; K=-1 for backwards paths.");
37 
38  MRPT_END
39 }
40 
42 {
44 
45  switch (version)
46  {
47  case 0:
48  in >> K;
49  break;
50  default:
52  };
53 }
54 
56 {
57  if (version)
58  {
59  *version = 0;
60  return;
61  }
62 
64  out << K;
65 }
66 
68 {
69  char str[100];
70  os::sprintf(str,100,"CPTG_DiffDrive_CCS,K=%i",(int)K);
71  return std::string(str);
72 }
73 
74 void CPTG_DiffDrive_CCS::ptgDiffDriveSteeringFunction( float alpha, float t,float x, float y, float phi, float &v, float &w ) const
75 {
77  float u = fabs(alpha) * 0.5f; //0.14758362f; // u = atan(0.5)* alpha / PI;
78 
79  if (t< u*R/V_MAX)
80  {
81  // l-
82  v = -V_MAX;
83  w = W_MAX;
84  }
85  else
86  if (t< (u+M_PI/2)*R/V_MAX)
87  {
88  // l+ pi/2
89  v = V_MAX;
90  w = W_MAX;
91  }
92  else
93  {
94  // s+:
95  v = V_MAX;
96  w = 0;
97  }
98 
99  // Turn in the opposite direction??
100  if (alpha<0)
101  w*=-1;
102 
103  v*=K;
104  w*=K;
105 }
106 
107 bool CPTG_DiffDrive_CCS::PTG_IsIntoDomain( double x, double y ) const
108 {
109  // If signs of K and X are different, it is into the domain:
110  if ((K*x)<0)
111  return true;
112 
113  if (fabs(y)>=R)
114  {
115  // Segmento de arriba:
116  return (fabs(x)<=R);
117  }
118  else
119  {
120  // The circle at (0,R):
121  return (square(x)+square(fabs(y)-R))<=R;
122  }
123 }
124 
126 {
128  K = +1.0;
129 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
bool PTG_IsIntoDomain(double x, double y) const MRPT_OVERRIDE
Returns the same than inverseMap_WS2TP() but without any additional cost.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
STL namespace.
#define M_PI
Definition: bits.h:78
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
std::string getDescription() const MRPT_OVERRIDE
Gets a short textual description of the PTG and its parameters.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This is the base class for any user-defined PTG.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
#define MRPT_START
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
const float R
GLuint in
Definition: glext.h:6301
GLenum GLint GLint y
Definition: glext.h:3516
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS 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:191
virtual void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
A PTG for optimal paths of type "C|C,S" (as named in PTG papers).
GLenum GLint x
Definition: glext.h:3516
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
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...
void ptgDiffDriveSteeringFunction(float alpha, float t, float x, float y, float phi, float &v, float &w) const MRPT_OVERRIDE
The main method to be implemented in derived classes: it defines the differential-driven differential...



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