MRPT  1.9.9
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-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
11 
16 
17 using namespace mrpt::nav;
18 
20  : m_robotShape(), m_robotMaxRadius(.01)
21 {
22 }
25  const mrpt::math::CPolygon& robotShape)
26 {
27  ASSERT_ABOVEEQ_(robotShape.size(), 3u);
28  m_robotShape = robotShape;
29 
30  m_robotMaxRadius = .0; // Default minimum
31  for (const auto& v : m_robotShape)
33 
35 }
36 
38 {
39  m_robotShape.clear();
40  m_robotShape.AddVertex(-0.15, 0.15);
41  m_robotShape.AddVertex(0.2, 0.1);
42  m_robotShape.AddVertex(0.2, -0.1);
43  m_robotShape.AddVertex(-0.15, -0.15);
44 }
45 
47  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
48 {
49  bool any_pt = false;
50  const double BADNUM = std::numeric_limits<double>::max();
51 
52  for (unsigned int nPt = 0;; ++nPt)
53  {
54  const std::string sPtx = mrpt::format("shape_x%u", nPt);
55  const std::string sPty = mrpt::format("shape_y%u", nPt);
56 
57  const double ptx = cfg.read_double(sSection, sPtx, BADNUM, false);
58  const double pty = cfg.read_double(sSection, sPty, BADNUM, false);
59  if (ptx == BADNUM && pty == BADNUM) break;
60  ASSERTMSG_(
61  (ptx != BADNUM && pty != BADNUM),
62  "Error: mismatch between number of pts in {x,y} defining robot "
63  "shape");
64 
65  if (!any_pt)
66  {
67  m_robotShape.clear();
68  any_pt = true;
69  }
70 
71  m_robotShape.AddVertex(ptx, pty);
72  }
73 
74  if (any_pt) internal_processNewRobotShape();
75 }
76 
78  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
79 {
80  const int WN = 25, WV = 30;
81 
82  for (unsigned int i = 0; i < m_robotShape.size(); i++)
83  {
84  const std::string sPtx = mrpt::format("shape_x%u", i);
85  const std::string sPty = mrpt::format("shape_y%u", i);
86 
87  cfg.write(
88  sSection, sPtx, m_robotShape[i].x, WN, WV,
89  "Robot polygonal shape, `x` [m].");
90  cfg.write(
91  sSection, sPty, m_robotShape[i].y, WN, WV,
92  "Robot polygonal shape, `y` [m].");
93  }
94 }
95 
97  mrpt::opengl::CSetOfLines& gl_shape,
98  const mrpt::poses::CPose2D& origin) const
99 {
100  const int N = m_robotShape.size();
101  if (N >= 2)
102  {
103  // Transform coordinates:
104  mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
105  for (int i = 0; i < N; i++)
106  {
107  origin.composePoint(
108  m_robotShape[i].x, m_robotShape[i].y, 0, shap_x[i], shap_y[i],
109  shap_z[i]);
110  }
111 
112  gl_shape.appendLine(
113  shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
114  for (int i = 0; i <= shap_x.size(); i++)
115  {
116  const int idx = i % shap_x.size();
117  gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
118  }
119  }
120 }
121 
124 {
125  uint8_t version;
126  in >> version;
127 
128  switch (version)
129  {
130  case 0:
131  in >> m_robotShape;
132  break;
133  default:
135  }
136 }
137 
140 {
141  uint8_t version = 0;
142  out << version;
143 
144  out << m_robotShape;
145 }
146 
148 {
149  return m_robotMaxRadius;
150 }
151 
153  const double x, const double y) const
154 {
156 }
157 
159  const double ox, const double oy) const
160 {
161  // Approximated computation, valid for relatively distant objects, which
162  // is where clearance is useful.
163 
164  if (isPointInsideRobotShape(ox, oy)) return .0;
165 
166  double d = mrpt::hypot_fast(ox, oy) - m_robotMaxRadius;
167 
168  // if d<=0, we know from the isPointInsideRobotShape() above that
169  // it's a false positive: enforce a minimum "fake" clearance:
171 
172  return d;
173 }
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:88
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 loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
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:175
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This class allows loading and storing values and vectors of different types from a configuration text...
virtual 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...
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:68
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...
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
GLsizei const GLchar ** string
Definition: glext.h:4101
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())
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
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:3678
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLuint in
Definition: glext.h:7274
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. ...
GLenum GLint GLint y
Definition: glext.h:3538
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
GLenum GLint x
Definition: glext.h:3538
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:31
Lightweight 2D point.
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:28
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020