MRPT  2.0.1
CPTG_RobotShape_Circular.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 
12 #include <mrpt/core/round.h>
16 
17 using namespace mrpt::nav;
18 
21 void CPTG_RobotShape_Circular::setRobotShapeRadius(const double robot_radius)
22 {
23  m_robotRadius = robot_radius;
25 }
26 
29  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
30 {
31  const double old_R = m_robotRadius;
33  robot_radius, double, m_robotRadius, cfg, sSection);
34 
36 }
38  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
39 {
40  const int WN = 25, WV = 30;
41 
42  cfg.write(
43  sSection, "robot_radius", m_robotRadius, WN, WV, "Robot radius [m].");
44 }
45 
47  mrpt::opengl::CSetOfLines& gl_shape,
48  const mrpt::poses::CPose2D& origin) const
49 {
50  const double R = m_robotRadius;
51  const int N = 17;
52  // Transform coordinates:
53  std::vector<double> shap_x(N), shap_y(N), shap_z(N);
54  for (int i = 0; i < N; i++)
55  {
56  origin.composePoint(
57  R * cos(i * 2 * M_PI / (N - 1)), R * sin(i * 2 * M_PI / (N - 1)), 0,
58  shap_x[i], shap_y[i], shap_z[i]);
59  }
60  // Draw a "radius" to identify the "forward" orientation (phi=0)
61  gl_shape.appendLine(
62  origin.x(), origin.y(), .0, shap_x[0], shap_y[0], shap_z[0]);
63  for (unsigned int i = 1; i <= shap_x.size(); i++)
64  {
65  const unsigned int idx = i % shap_x.size();
66  gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
67  }
68  // Draw a "cross" to identify the robot center
69  gl_shape.appendLine(
70  origin.x() - R * 0.02, origin.y(), .0, origin.x() + R * 0.02,
71  origin.y(), .0);
72  gl_shape.appendLine(
73  origin.x(), origin.y() - R * 0.02, .0, origin.x(),
74  origin.y() + R * 0.02, .0);
75 }
76 
79 {
80  uint8_t version;
81  in >> version;
82 
83  switch (version)
84  {
85  case 0:
86  in >> m_robotRadius;
87  break;
88  default:
90  }
91 }
92 
95 {
96  uint8_t version = 0;
97  out << version;
98 
99  out << m_robotRadius;
100 }
101 
103 {
104  return m_robotRadius;
105 }
106 
108  const double x, const double y) const
109 {
110  return mrpt::hypot_fast(x, y) < m_robotRadius;
111 }
112 
114  const double ox, const double oy) const
115 {
116  return mrpt::hypot_fast(ox, oy) - m_robotRadius;
117 }
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
void appendLineStrip(float x, float y, float z)
Appends a line whose starting point is the end point of the last line (similar to OpenGL&#39;s GL_LINE_ST...
Definition: CSetOfLines.h:91
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:199
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This class allows loading and storing values and vectors of different types from a configuration text...
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:71
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
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())
double evalClearanceToRobotShape(const double ox, const double oy) const override
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
mrpt::vision::TStereoCalibResults out
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:32
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
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. ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020