Main MRPT website > C++ reference for MRPT 1.9.9
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-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 std;
18 using namespace mrpt::system;
19 
22 
24  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
25 {
27 
28  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K, double, cfg, sSection);
29 
30  // The constant curvature turning radius used in this PTG:
31  R = V_MAX / W_MAX;
32 }
34  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
35 {
37  const int WN = 25, WV = 30;
39 
40  cfg.write(
41  sSection, "K", K, WN, WV,
42  "K=+1 forward paths; K=-1 for backwards paths.");
43 
44  MRPT_END
45 }
46 
49 {
51 
52  switch (version)
53  {
54  case 0:
55  in >> K;
56  break;
57  default:
59  };
60 }
61 
64 {
66  out << K;
67 }
69 {
70  char str[100];
71  os::sprintf(str, 100, "CPTG_DiffDrive_CS,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 {
80  MRPT_UNUSED_PARAM(phi);
81  const float T = 0.847f * std::sqrt(std::abs(alpha)) * R / V_MAX;
82 
83  if (t < T)
84  {
85  // l+
86  v = V_MAX;
87  w = W_MAX * min(1.0f, 1.0f - (float)exp(-square(alpha)));
88  }
89  else
90  {
91  // s+:
92  v = V_MAX;
93  w = 0;
94  }
95 
96  // Turn in the opposite direction??
97  if (alpha < 0) w *= -1;
98 
99  v *= K;
100  w *= K;
101 }
102 
103 bool CPTG_DiffDrive_CS::PTG_IsIntoDomain(double x, double y) const
104 {
105  // If signs of K and X are different, it is not into the domain:
106  if ((K * x) < 0) return false;
107 
108  if (fabs(y) >= R)
109  {
110  // Segmento de arriba:
111  return (fabs(x) > R - 0.10f);
112  }
113  else
114  {
115  // The circle at (0,R):
116  return (square(x) + square(fabs(y) - (R + 0.10f))) > square(R);
117  }
118 }
119 
121 {
123  K = +1.0;
124 }
os.h
nav-precomp.h
mrpt::config::CConfigFileBase::write
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())
Definition: config/CConfigFileBase.h:85
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::nav::CPTG_DiffDrive_CS::loadFromConfigFile
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally,...
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:23
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
R
const float R
Definition: CKinematicChain.cpp:138
alpha
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadDefaultParams
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_DiffDrive_CollisionGridBased.cpp:40
mrpt::nav::CPTG_DiffDrive_CS::PTG_IsIntoDomain
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
Definition: CPTG_DiffDrive_CS.cpp:103
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:377
v
const GLdouble * v
Definition: glext.h:3678
mrpt::system::os::sprintf
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
mrpt::nav::CPTG_DiffDrive_CS
A PTG for optimal paths of type "CS", as named in PTG papers.
Definition: CPTG_DiffDrive_CS.h:27
mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_writeToStream
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
Definition: CPTG_DiffDrive_CollisionGridBased.cpp:935
mrpt::nav::CPTG_DiffDrive_CS::saveToConfigFile
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.
Definition: CPTG_DiffDrive_CS.cpp:33
mrpt::nav::CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally,...
Definition: CPTG_DiffDrive_CollisionGridBased.cpp:50
mrpt::nav::CPTG_DiffDrive_CS::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPTG_DiffDrive_CS.cpp:47
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::nav::CPTG_DiffDrive_CS::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPTG_DiffDrive_CS.cpp:62
mrpt::nav::CPTG_DiffDrive_CS::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_DiffDrive_CS.cpp:120
mrpt::nav::CPTG_DiffDrive_CollisionGridBased::saveToConfigFile
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.
Definition: CPTG_DiffDrive_CollisionGridBased.cpp:64
mrpt::nav::CPTG_DiffDrive_CollisionGridBased::internal_readFromStream
void internal_readFromStream(mrpt::serialization::CArchive &in) override
Definition: CPTG_DiffDrive_CollisionGridBased.cpp:915
CPTG_DiffDrive_CS.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::nav::CPTG_DiffDrive_CS::ptgDiffDriveSteeringFunction
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...
Definition: CPTG_DiffDrive_CS.cpp:75
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
in
GLuint in
Definition: glext.h:7274
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::nav::CPTG_DiffDrive_CS::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPTG_DiffDrive_CS.cpp:63
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::nav::CPTG_DiffDrive_CS::getDescription
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
Definition: CPTG_DiffDrive_CS.cpp:68
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:76
x
GLenum GLint x
Definition: glext.h:3538
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
Definition: math_frwds.h:25
IMPLEMENTS_SERIALIZABLE
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_CS, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_CS
Definition: CPTG_DiffDrive_CS.cpp:20



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST