Main MRPT website > C++ reference for MRPT 1.5.7
CPTG_RobotShape_Polygonal.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
11 
14 #include <mrpt/utils/CStream.h>
15 
16 using namespace mrpt::nav;
17 
19  m_robotShape(),
20  m_robotMaxRadius(.01)
21 {
22 }
24 {
25 }
26 
28 {
29  ASSERT_ABOVEEQ_(robotShape.size(),3u);
30  m_robotShape = robotShape;
31 
32  m_robotMaxRadius = .0; // Default minimum
33  for (const auto &v : m_robotShape)
35 
37 }
38 
40 {
41  m_robotShape.clear();
42  m_robotShape.AddVertex(-0.15, 0.15);
43  m_robotShape.AddVertex(0.2, 0.1);
44  m_robotShape.AddVertex(0.2, -0.1);
45  m_robotShape.AddVertex(-0.15, -0.15);
46 }
47 
49 {
50  bool any_pt = false;
51  const double BADNUM = std::numeric_limits<double>::max();
52 
53  for (unsigned int nPt = 0; ; ++nPt)
54  {
55  const std::string sPtx = mrpt::format("shape_x%u", nPt);
56  const std::string sPty = mrpt::format("shape_y%u", nPt);
57 
58  const double ptx = cfg.read_double(sSection, sPtx, BADNUM, false);
59  const double pty = cfg.read_double(sSection, sPty, BADNUM, false);
60  if (ptx == BADNUM && pty == BADNUM) break;
61  ASSERTMSG_((ptx != BADNUM && pty != BADNUM), "Error: mismatch between number of pts in {x,y} defining robot shape");
62 
63  if (!any_pt) {
64  m_robotShape.clear();
65  any_pt = true;
66  }
67 
68  m_robotShape.AddVertex(ptx, pty);
69  }
70 
71  if (any_pt)
73 }
74 
76 {
77  const int WN = 25, WV = 30;
78 
79  for (unsigned int i = 0; i<m_robotShape.size(); i++)
80  {
81  const std::string sPtx = mrpt::format("shape_x%u", i);
82  const std::string sPty = mrpt::format("shape_y%u", i);
83 
84  cfg.write(sSection, sPtx, m_robotShape[i].x, WN, WV, "Robot polygonal shape, `x` [m].");
85  cfg.write(sSection, sPty, m_robotShape[i].y, WN, WV, "Robot polygonal shape, `y` [m].");
86  }
87 }
88 
90  mrpt::opengl::CSetOfLines &gl_shape,
91  const mrpt::poses::CPose2D &origin) const
92 {
93  const int N = m_robotShape.size();
94  if (N >= 2)
95  {
96  // Transform coordinates:
97  mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
98  for (int i = 0; i<N; i++) {
99  origin.composePoint(
100  m_robotShape[i].x, m_robotShape[i].y, 0,
101  shap_x[i], shap_y[i], shap_z[i]);
102  }
103 
104  gl_shape.appendLine(shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
105  for (int i = 0; i <= shap_x.size(); i++) {
106  const int idx = i % shap_x.size();
107  gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
108  }
109  }
110 }
111 
113 {
115  in >> version;
116 
117  switch (version)
118  {
119  case 0:
120  in >> m_robotShape;
121  break;
122  default:
124  }
125 }
126 
128 {
129  uint8_t version = 0;
130  out << version;
131 
132  out << m_robotShape;
133 }
134 
136 {
137  return m_robotMaxRadius;
138 }
139 
140 bool CPTG_RobotShape_Polygonal::isPointInsideRobotShape(const double x, const double y) const
141 {
143 }
144 
145 double CPTG_RobotShape_Polygonal::evalClearanceToRobotShape(const double ox, const double oy) const
146 {
147  // Approximated computation, valid for relatively distant objects, which
148  // is where clearance is useful.
149 
150  if (isPointInsideRobotShape(ox, oy))
151  return .0;
152 
153  double d = mrpt::math::hypot_fast(ox, oy) - m_robotMaxRadius;
154 
155  // if d<=0, we know from the isPointInsideRobotShape() above that
156  // it's a false positive: enforce a minimum "fake" clearance:
158 
159  return d;
160 }
void internal_shape_saveToStream(mrpt::utils::CStream &out) const
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:84
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
virtual double evalClearanceToRobotShape(const double ox, const double oy) const MRPT_OVERRIDE
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center...
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
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:178
bool isPointInsideRobotShape(const double x, const double y) const MRPT_OVERRIDE
Returns true if the point lies within the robot shape.
void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:70
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
void internal_shape_loadFromStream(mrpt::utils::CStream &in)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const MRPT_OVERRIDE
Auxiliary function for rendering.
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
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. ...
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
const GLdouble * v
Definition: glext.h:3603
#define ASSERT_ABOVEEQ_(__A, __B)
double getMaxRobotRadius() const MRPT_OVERRIDE
Returns an approximation of the robot radius.
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())
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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...
GLuint in
Definition: glext.h:6301
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:35
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Lightweight 2D point.
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:36
#define ASSERTMSG_(f, __ERROR_MSG)
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...



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