MRPT  2.0.4
CPTG_DiffDrive_CS.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/system/os.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::nav;
18 using namespace std;
19 using namespace mrpt::system;
20 
23 
25  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
26 {
28 
29  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K, double, cfg, sSection);
30 
31  // The constant curvature turning radius used in this PTG:
32  R = V_MAX / W_MAX;
33 }
35  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
36 {
38  const int WN = 25, WV = 30;
40 
41  cfg.write(
42  sSection, "K", K, WN, WV,
43  "K=+1 forward paths; K=-1 for backwards paths.");
44 
45  MRPT_END
46 }
47 
49  mrpt::serialization::CArchive& in, uint8_t version)
50 {
52 
53  switch (version)
54  {
55  case 0:
56  in >> K;
57  break;
58  default:
60  };
61 }
62 
63 uint8_t CPTG_DiffDrive_CS::serializeGetVersion() const { return 0; }
65 {
67  out << K;
68 }
70 {
71  char str[100];
72  os::sprintf(str, 100, "CPTG_DiffDrive_CS,K=%i", (int)K);
73  return std::string(str);
74 }
75 
77  float alpha, float t, [[maybe_unused]] float x, [[maybe_unused]] float y,
78  [[maybe_unused]] float phi, float& v, float& w) const
79 {
80  const float T = 0.847f * std::sqrt(std::abs(alpha)) * R / V_MAX;
81 
82  if (t < T)
83  {
84  // l+
85  v = V_MAX;
86  w = W_MAX * min(1.0f, 1.0f - (float)exp(-square(alpha)));
87  }
88  else
89  {
90  // s+:
91  v = V_MAX;
92  w = 0;
93  }
94 
95  // Turn in the opposite direction??
96  if (alpha < 0) w *= -1;
97 
98  v *= K;
99  w *= K;
100 }
101 
102 bool CPTG_DiffDrive_CS::PTG_IsIntoDomain(double x, double y) const
103 {
104  // If signs of K and X are different, it is not into the domain:
105  if ((K * x) < 0) 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 }
#define MRPT_START
Definition: exceptions.h:241
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
void internal_readFromStream(mrpt::serialization::CArchive &in) override
STL namespace.
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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. ...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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())
return_t square(const num_t x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
const float R
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
A PTG for optimal paths of type "CS", as named in PTG papers.
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
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. ...
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...
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_CS, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_CS
void internal_writeToStream(mrpt::serialization::CArchive &out) const override



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7ea9b7e81 Mon May 25 11:43:10 2020 +0200 at lun may 25 11:45:15 CEST 2020