Main MRPT website > C++ reference for MRPT 1.5.7
CPTG_DiffDrive_C.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/math/wrap2pi.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::nav;
16 using namespace mrpt::utils;
17 using namespace mrpt::system;
18 
20 
21 
22 void CPTG_DiffDrive_C::loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection)
23 {
25 
26  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K,double, cfg,sSection);
27 }
29 {
31  const int WN = 25, WV = 30;
33 
34  cfg.write(sSection,"K",K, WN,WV, "K=+1 forward paths; K=-1 for backwards paths.");
35 
36  MRPT_END
37 }
38 
40 {
42 
43  switch (version)
44  {
45  case 0:
46  in >> K;
47  break;
48  default:
50  };
51 }
52 
54 {
55  if (version)
56  {
57  *version = 0;
58  return;
59  }
60 
62  out << K;
63 }
64 
65 
66 
68 {
69  return mrpt::format("CPTG_DiffDrive_C,K=%i",(int)K);
70 }
71 
72 void CPTG_DiffDrive_C::ptgDiffDriveSteeringFunction( float alpha, float t,float x, float y, float phi, float &v, float &w ) const
73 {
75  // (v,w)
76  v = V_MAX * sign(K);
77  // Use a linear mapping: (Old was: w = tan( alpha/2 ) * W_MAX * sign(K))
78  w = (alpha/M_PI) * W_MAX * sign(K);
79 }
80 
81 bool CPTG_DiffDrive_C::PTG_IsIntoDomain( double x, double y ) const
82 {
84  return true;
85 }
86 
87 bool CPTG_DiffDrive_C::inverseMap_WS2TP(double x, double y, int &k_out, double &d_out, double tolerance_dist) const
88 {
89  MRPT_UNUSED_PARAM(tolerance_dist);
90  bool is_exact = true;
91  if (y!=0)
92  {
93  double R = (x*x+y*y)/(2*y);
94  const double Rmin = std::abs(V_MAX/W_MAX);
95 
96  double theta;
97 
98  if (K>0)
99  {
100  if (y>0)
101  theta = atan2( (double)x,fabs(R)-y );
102  else theta = atan2( (double)x,y+fabs(R) );
103  }
104  else
105  {
106  if (y>0)
107  theta = atan2( -(double)x,fabs(R)-y );
108  else theta = atan2( -(double)x,y+fabs(R) );
109  }
110 
111  // Arc length must be possitive [0,2*pi]
113 
114  // Distance thru arc:
115  d_out = (float)(theta * (fabs(R)+turningRadiusReference));
116 
117  if (std::abs(R)<Rmin)
118  {
119  is_exact=false;
120  R=Rmin*mrpt::utils::sign(R);
121  }
122 
123  //Was: a = 2*atan( V_MAX / (W_MAX*R) );
124  const double a = M_PI* V_MAX / (W_MAX*R);
125  k_out = alpha2index( (float)a );
126 
127  }
128  else
129  {
130  if (sign(x)==sign(K))
131  {
132  k_out = alpha2index(0);
133  d_out = x;
134  is_exact=true;
135  }
136  else
137  {
138  k_out = m_alphaValuesCount-1;
139  d_out = 1e+3;
140  is_exact=false;
141  }
142  }
143 
144  // Normalize:
145  d_out = d_out / refDistance;
146 
147  ASSERT_ABOVEEQ_(k_out,0)
148  ASSERT_BELOW_(k_out,m_alphaValuesCount)
149 
150  return is_exact;
151 }
152 
154 {
156  K = +1.0;
157 }
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
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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.
bool PTG_IsIntoDomain(double x, double y) const MRPT_OVERRIDE
Returns the same than inverseMap_WS2TP() but without any additional cost.
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
#define ASSERT_BELOW_(__A, __B)
int sign(T x)
Returns the sign of X as "1" or "-1".
STL namespace.
#define M_PI
Definition: bits.h:78
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...
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.
bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const MRPT_OVERRIDE
The default implementation in this class relies on a look-up-table.
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
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: bits.h:104
void wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:28
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
#define MRPT_START
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERT_ABOVEEQ_(__A, __B)
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
std::string getDescription() const MRPT_OVERRIDE
Gets a short textual description of the PTG and its parameters.
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. ...
GLuint in
Definition: glext.h:6301
GLenum GLint GLint y
Definition: glext.h:3516
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLenum GLint x
Definition: glext.h:3516
A PTG for circular paths ("C" type PTG in papers).
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
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