MRPT  1.9.9
CPTG_DiffDrive_alpha.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 mrpt::system;
19 
22 
24  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
25 {
27 
29  cte_a0v_deg, double, cte_a0v, cfg, sSection);
31  cte_a0w_deg, double, cte_a0w, cfg, sSection);
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, "cte_a0v_deg", mrpt::RAD2DEG(cte_a0v), WN, WV,
42  "Contant for vel profile [deg].");
43  cfg.write(
44  sSection, "cte_a0w_deg", mrpt::RAD2DEG(cte_a0v), WN, WV,
45  "Contant for omega profile [deg].");
46 
47  MRPT_END
48 }
49 
51 {
52  char str[100];
54  str, 100, "CPTG_DiffDrive_alpha,av=%udeg,aw=%udeg",
55  (int)RAD2DEG(cte_a0v), (int)RAD2DEG(cte_a0w));
56  return std::string(str);
57 }
58 
60  mrpt::serialization::CArchive& in, uint8_t version)
61 {
63 
64  switch (version)
65  {
66  case 0:
67  in >> cte_a0v >> cte_a0w;
68  break;
69  default:
71  };
72 }
73 
74 uint8_t CPTG_DiffDrive_alpha::serializeGetVersion() const { return 0; }
76 {
78  out << cte_a0v << cte_a0w;
79 }
80 /*---------------------------------------------------------------
81  ptgDiffDriveSteeringFunction
82  ---------------------------------------------------------------*/
84  float alpha, float t, float x, float y, float phi, float& v, float& w) const
85 {
89  float At_a = alpha - phi;
90 
91  while (At_a > M_PI) At_a -= (float)M_2PI;
92  while (At_a < -M_PI) At_a += (float)M_2PI;
93 
94  v = V_MAX * exp(-square(At_a / cte_a0v));
95  w = W_MAX * (-0.5f + (1 / (1 + exp(-At_a / cte_a0w))));
96 }
97 
99 {
101 
102  cte_a0v = mrpt::DEG2RAD(45.0);
103  cte_a0w = mrpt::DEG2RAD(45.0);
104 }
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define MRPT_START
Definition: exceptions.h:241
#define M_2PI
Definition: common.h:58
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void internal_readFromStream(mrpt::serialization::CArchive &in) override
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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...
constexpr double DEG2RAD(const double x)
Degrees to radians.
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_alpha, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_alpha
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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
The "a(symptotic)-alpha PTG", as named in PTG papers.
mrpt::vision::TStereoCalibResults out
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...
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
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 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. ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
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:191
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at miƩ mar 25 23:05:41 CET 2020