Main MRPT website > C++ reference for MRPT 1.5.9
CPTG_DiffDrive_CC.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_CC::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 
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 
67 }
68 
70 {
71  char str[100];
72  os::sprintf(str,100,"CPTG_DiffDrive_CC,K=%i",(int)K);
73  return std::string(str);
74 }
75 
76 void CPTG_DiffDrive_CC::ptgDiffDriveSteeringFunction( float alpha, float t,float x, float y, float phi, float &v, float &w ) const
77 {
79  float u = fabs(alpha) * 0.5f; /// 6.0f;
80 
81  if (t< u*R/V_MAX)
82  {
83  // l-
84  v = -V_MAX;
85  w = W_MAX;
86  }
87  else
88  if ( t< (u + M_PI*0.5f) * R/V_MAX )
89  {
90  // l+
91  v = V_MAX;
92  w = W_MAX;
93  }
94  else
95  {
96  // END:
97  v = w = 0;
98  }
99 
100  // Turn in the opposite direction??
101  if (alpha<0)
102  w*=-1;
103 
104  v*=K;
105  w*=K;
106 
107 }
108 
109 bool CPTG_DiffDrive_CC::PTG_IsIntoDomain( double x, double y ) const
110 {
111  // Aprox domain: The circle at (0,R):
112  return (square(x)+square(fabs(y)-R))<=R;
113 }
114 
116 {
118  K = +1.0;
119 }
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
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
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...
STL namespace.
#define M_PI
Definition: bits.h:78
IMPLEMENTS_SERIALIZABLE(CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav) IMPLEMENTS_SERIALIZABLE(CHolonomicFullEval
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
bool PTG_IsIntoDomain(double x, double y) const MRPT_OVERRIDE
Returns the same than inverseMap_WS2TP() but without any additional cost.
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.
A PTG for optimal paths of type "C|C" , as named in PTG papers.
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
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
#define MRPT_START
std::string getDescription() const MRPT_OVERRIDE
Gets a short textual description of the PTG and its parameters.
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.
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. ...
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...
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLenum GLint x
Definition: glext.h:3516



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020