Main MRPT website > C++ reference for MRPT 1.5.7
CPTG_DiffDrive_CS.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 std;
17 using namespace mrpt::system;
18 using namespace mrpt::utils;
19 
21 
22 void CPTG_DiffDrive_CS::loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection)
23 {
25 
26  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K,double, cfg,sSection);
27 
28  // The constant curvature turning radius used in this PTG:
29  R = V_MAX / W_MAX;
30 }
32 {
34  const int WN = 25, WV = 30;
36 
37  cfg.write(sSection,"K",K, WN,WV, "K=+1 forward paths; K=-1 for backwards paths.");
38 
39  MRPT_END
40 }
41 
43 {
45 
46  switch (version)
47  {
48  case 0:
49  in >> K;
50  break;
51  default:
53  };
54 }
55 
57 {
58  if (version)
59  {
60  *version = 0;
61  return;
62  }
63 
65  out << K;
66 }
68 {
69  char str[100];
70  os::sprintf(str,100,"CPTG_DiffDrive_CS,K=%i",(int)K);
71  return std::string(str);
72 }
73 
74 void CPTG_DiffDrive_CS::ptgDiffDriveSteeringFunction( float alpha, float t,float x, float y, float phi, float &v, float &w ) const
75 {
77  const float T = 0.847f*std::sqrt(std::abs(alpha))*R/V_MAX;
78 
79  if (t< T)
80  {
81  // l+
82  v = V_MAX;
83  w = W_MAX * min( 1.0f , 1.0f - (float)exp( -square(alpha) ));
84  }
85  else
86  {
87  // s+:
88  v = V_MAX;
89  w = 0;
90  }
91 
92  // Turn in the opposite direction??
93  if (alpha<0)
94  w*=-1;
95 
96  v*=K;
97  w*=K;
98 
99 }
100 
101 bool CPTG_DiffDrive_CS::PTG_IsIntoDomain( double x, double y ) const
102 {
103  // If signs of K and X are different, it is not into the domain:
104  if ((K*x)<0)
105  return false;
106 
107  if (fabs(y)>=R)
108  {
109  // Segmento de arriba:
110  return (fabs(x)>R-0.10f);
111  }
112  else
113  {
114  // The circle at (0,R):
115  return (square(x)+square(fabs(y)-(R+0.10f)))>square(R);
116  }
117 }
118 
120 {
122  K = +1.0;
123 }
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
#define min(a, b)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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 MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
STL namespace.
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.
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
#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
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...
GLuint in
Definition: glext.h:6301
A PTG for optimal paths of type "CS", as named in PTG papers.
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...
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
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
GLenum GLint x
Definition: glext.h:3516
std::string getDescription() const MRPT_OVERRIDE
Gets a short textual description of the PTG and its parameters.
bool PTG_IsIntoDomain(double x, double y) const MRPT_OVERRIDE
Returns the same than inverseMap_WS2TP() but without any additional cost.



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