polygons.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <nav_2d_utils/polygons.h>
37 #include <mapbox/earcut.hpp>
38 #include <algorithm>
39 #include <limits>
40 #include <string>
41 #include <vector>
42 
43 namespace nav_2d_utils
44 {
45 
46 using nav_2d_msgs::Point2D;
47 using nav_2d_msgs::Polygon2D;
48 
49 std::vector<std::vector<double> > parseVVD(const std::string& input)
50 {
51  std::vector<std::vector<double> > result;
52 
53  std::stringstream input_ss(input);
54  int depth = 0;
55  std::vector<double> current_vector;
56  while (input_ss.good())
57  {
58  switch (input_ss.peek())
59  {
60  case EOF:
61  break;
62  case '[':
63  depth++;
64  if (depth > 2)
65  {
66  throw PolygonParseException("Array depth greater than 2");
67  }
68  input_ss.get();
69  current_vector.clear();
70  break;
71  case ']':
72  depth--;
73  if (depth < 0)
74  {
75  throw PolygonParseException("More close ] than open [");
76  }
77  input_ss.get();
78  if (depth == 1)
79  {
80  result.push_back(current_vector);
81  }
82  break;
83  case ',':
84  case ' ':
85  case '\t':
86  input_ss.get();
87  break;
88  default: // All other characters should be part of the numbers.
89  if (depth != 2)
90  {
91  std::stringstream err_ss;
92  err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
93  throw PolygonParseException(err_ss.str());
94  }
95  double value;
96  if (input_ss >> value)
97  {
98  current_vector.push_back(value);
99  }
100  break;
101  }
102  }
103 
104  if (depth != 0)
105  {
106  throw PolygonParseException("Unterminated vector string.");
107  }
108 
109  return result;
110 }
111 
112 Polygon2D polygonFromRadius(const double radius, const unsigned int num_points)
113 {
114  Polygon2D polygon;
115  Point2D pt;
116  for (unsigned int i = 0; i < num_points; ++i)
117  {
118  double angle = i * 2 * M_PI / num_points;
119  pt.x = cos(angle) * radius;
120  pt.y = sin(angle) * radius;
121  polygon.points.push_back(pt);
122  }
123 
124  return polygon;
125 }
126 
127 Polygon2D polygonFromString(const std::string& polygon_string)
128 {
129  Polygon2D polygon;
130  // Will throw PolygonParseException if problematic
131  std::vector<std::vector<double> > vvd = parseVVD(polygon_string);
132 
133  // convert vvd into points.
134  if (vvd.size() < 3)
135  {
136  throw PolygonParseException("You must specify at least three points for the polygon.");
137  }
138 
139  polygon.points.resize(vvd.size());
140  for (unsigned int i = 0; i < vvd.size(); i++)
141  {
142  if (vvd[ i ].size() != 2)
143  {
144  std::stringstream err_ss;
145  err_ss << "Points in the polygon specification must be pairs of numbers. Point index " << i << " had ";
146  err_ss << int(vvd[ i ].size()) << " numbers.";
147  throw PolygonParseException(err_ss.str());
148  }
149 
150  polygon.points[i].x = vvd[i][0];
151  polygon.points[i].y = vvd[i][1];
152  }
153 
154  return polygon;
155 }
156 
157 
162 {
163  if (value.getType() == XmlRpc::XmlRpcValue::TypeInt)
164  {
165  return static_cast<double>(static_cast<int>(value));
166  }
167  else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble)
168  {
169  return static_cast<double>(value);
170  }
171 
172  std::stringstream err_ss;
173  err_ss << "Values in the polygon specification must be numbers. Found value: " << value.toXml();
174  throw PolygonParseException(err_ss.str());
175 }
176 
181 {
183  {
184  throw PolygonParseException("Subarray must have type list.");
185  }
186  std::vector<double> array;
187  for (int i = 0; i < value.size(); i++)
188  {
189  array.push_back(getNumberFromXMLRPC(value[i]));
190  }
191  return array;
192 }
193 
194 Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
195 {
196  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
197  polygon_xmlrpc != "" && polygon_xmlrpc != "[]")
198  {
199  return polygonFromString(std::string(polygon_xmlrpc));
200  }
201 
202  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeStruct)
203  {
204  if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y"))
205  {
206  throw PolygonParseException("Dict-like Polygon must specify members x and y.");
207  }
208  std::vector<double> xs = getNumberVectorFromXMLRPC(polygon_xmlrpc["x"]);
209  std::vector<double> ys = getNumberVectorFromXMLRPC(polygon_xmlrpc["y"]);
210  return polygonFromParallelArrays(xs, ys);
211  }
212 
213  // Make sure we have an array of at least 3 elements.
214  if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray)
215  {
216  std::stringstream err_ss;
217  err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType()
218  << " instead.";
219  throw PolygonParseException(err_ss.str());
220  }
221  else if (polygon_xmlrpc.size() < 3)
222  {
223  throw PolygonParseException("You must specify at least three points for the polygon.");
224  }
225 
226  Polygon2D polygon;
227  Point2D pt;
228  for (int i = 0; i < polygon_xmlrpc.size(); ++i)
229  {
230  // Make sure each element of the list is an array of size 2. (x and y coordinates)
231  XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
232  if (point_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
233  {
234  std::stringstream err_ss;
235  err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead.";
236  throw PolygonParseException(err_ss.str());
237  }
238  else if (point_xml.size() != 2)
239  {
240  throw PolygonParseException("Each point must have two numbers (x and y).");
241  }
242 
243  pt.x = getNumberFromXMLRPC(point_xml[0]);
244  pt.y = getNumberFromXMLRPC(point_xml[1]);
245  polygon.points.push_back(pt);
246  }
247  return polygon;
248 }
249 
250 Polygon2D polygonFromParams(const ros::NodeHandle& nh, const std::string parameter_name, bool search)
251 {
252  std::string full_param_name;
253  if (search)
254  {
255  nh.searchParam(parameter_name, full_param_name);
256  }
257  else
258  {
259  full_param_name = parameter_name;
260  }
261 
262  if (!nh.hasParam(full_param_name))
263  {
264  std::stringstream err_ss;
265  err_ss << "Parameter " << parameter_name << "(" + nh.resolveName(parameter_name) << ") not found.";
266  throw PolygonParseException(err_ss.str());
267  }
268  XmlRpc::XmlRpcValue polygon_xmlrpc;
269  nh.getParam(full_param_name, polygon_xmlrpc);
270  return polygonFromXMLRPC(polygon_xmlrpc);
271 }
272 
276 XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
277 {
279  xml.setSize(array.size());
280  for (unsigned int i = 0; i < array.size(); ++i)
281  {
282  xml[i] = array[i];
283  }
284  return xml;
285 }
286 
287 XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
288 {
290  if (array_of_arrays)
291  {
292  xml.setSize(polygon.points.size());
293  for (unsigned int i = 0; i < polygon.points.size(); ++i)
294  {
295  xml[i].setSize(2);
296  const Point2D& p = polygon.points[i];
297  xml[i][0] = p.x;
298  xml[i][1] = p.y;
299  }
300  }
301  else
302  {
303  std::vector<double> xs, ys;
304  polygonToParallelArrays(polygon, xs, ys);
305  xml["x"] = vectorToXMLRPC(xs);
306  xml["y"] = vectorToXMLRPC(ys);
307  }
308  return xml;
309 }
310 
311 void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const ros::NodeHandle& nh, const std::string parameter_name,
312  bool array_of_arrays)
313 {
314  nh.setParam(parameter_name, polygonToXMLRPC(polygon, array_of_arrays));
315 }
316 
317 nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys)
318 {
319  if (xs.size() < 3)
320  {
321  throw PolygonParseException("You must specify at least three points for the polygon.");
322  }
323  else if (xs.size() != ys.size())
324  {
325  throw PolygonParseException("Length of x array does not match length of y array.");
326  }
327 
328  Polygon2D polygon;
329  polygon.points.resize(xs.size());
330  for (unsigned int i = 0; i < xs.size(); i++)
331  {
332  Point2D& pt = polygon.points[i];
333  pt.x = xs[i];
334  pt.y = ys[i];
335  }
336  return polygon;
337 }
338 
339 void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys)
340 {
341  xs.clear();
342  ys.clear();
343  for (const Point2D& pt : polygon.points)
344  {
345  xs.push_back(pt.x);
346  ys.push_back(pt.y);
347  }
348 }
349 
350 bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1)
351 {
352  if (polygon0.points.size() != polygon1.points.size())
353  {
354  return false;
355  }
356  for (unsigned int i=0; i < polygon0.points.size(); i++)
357  {
358  if (polygon0.points[i].x != polygon1.points[i].x ||polygon0.points[i].y != polygon1.points[i].y)
359  {
360  return false;
361  }
362  }
363  return true;
364 }
365 
366 nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
367  const geometry_msgs::Pose2D& pose)
368 {
369  nav_2d_msgs::Polygon2D new_polygon;
370  new_polygon.points.resize(polygon.points.size());
371  double cos_th = cos(pose.theta);
372  double sin_th = sin(pose.theta);
373  for (unsigned int i = 0; i < polygon.points.size(); ++i)
374  {
375  nav_2d_msgs::Point2D& new_pt = new_polygon.points[i];
376  new_pt.x = pose.x + polygon.points[i].x * cos_th - polygon.points[i].y * sin_th;
377  new_pt.y = pose.y + polygon.points[i].x * sin_th + polygon.points[i].y * cos_th;
378  }
379  return new_polygon;
380 }
381 
382 bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y)
383 {
384  // Determine if the given point is inside the polygon using the number of crossings method
385  // https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
386  int n = polygon.points.size();
387  int cross = 0;
388  // Loop from i = [0 ... n - 1] and j = [n - 1, 0 ... n - 2]
389  // Ensures first point connects to last point
390  for (int i = 0, j = n - 1; i < n; j = i++)
391  {
392  // Check if the line to x,y crosses this edge
393  if ( ((polygon.points[i].y > y) != (polygon.points[j].y > y))
394  && (x < (polygon.points[j].x - polygon.points[i].x) * (y - polygon.points[i].y) /
395  (polygon.points[j].y - polygon.points[i].y) + polygon.points[i].x) )
396  {
397  cross++;
398  }
399  }
400  // Return true if the number of crossings is odd
401  return cross % 2 > 0;
402 }
403 
404 void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist)
405 {
406  min_dist = std::numeric_limits<double>::max();
407  max_dist = 0.0;
408  auto& points = polygon.points;
409  if (points.size() == 0)
410  {
411  return;
412  }
413 
414  for (unsigned int i = 0; i < points.size() - 1; ++i)
415  {
416  // check the distance from the robot center point to the first vertex
417  double vertex_dist = hypot(points[i].x, points[i].y);
418  double edge_dist = distanceToLine(0.0, 0.0, points[i].x, points[i].y,
419  points[i + 1].x, points[i + 1].y);
420  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
421  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
422  }
423 
424  // we also need to do the last vertex and the first vertex
425  double vertex_dist = hypot(points.back().x, points.back().y);
426  double edge_dist = distanceToLine(0.0, 0.0, points.back().x, points.back().y,
427  points.front().x, points.front().y);
428  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
429  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
430 }
431 } // namespace nav_2d_utils
432 
433 // Adapt Point2D for use with the triangulation library
434 namespace mapbox
435 {
436 namespace util
437 {
438 template <>
439 struct nth<0, nav_2d_msgs::Point2D>
440 {
441  inline static double get(const nav_2d_msgs::Point2D& point)
442  {
443  return point.x;
444  };
445 };
446 
447 template <>
448 struct nth<1, nav_2d_msgs::Point2D>
449 {
450  inline static double get(const nav_2d_msgs::Point2D& point)
451  {
452  return point.y;
453  };
454 };
455 
456 } // namespace util
457 } // namespace mapbox
458 
459 
460 namespace nav_2d_utils
461 {
462 std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon)
463 {
464  // Compute the triangulation
465  std::vector<std::vector<nav_2d_msgs::Point2D>> rings;
466  rings.reserve(1 + polygon.inner.size());
467  rings.push_back(polygon.outer.points);
468  for (const nav_2d_msgs::Polygon2D& inner : polygon.inner)
469  {
470  rings.push_back(inner.points);
471  }
472  std::vector<unsigned int> indices = mapbox::earcut<unsigned int>(rings);
473 
474  // Create a sequential point index. The triangulation results are indices in this vector.
475  std::vector<nav_2d_msgs::Point2D> points;
476  for (const auto& ring : rings)
477  {
478  for (const nav_2d_msgs::Point2D& point : ring)
479  {
480  points.push_back(point);
481  }
482  }
483 
484  // Construct the output triangles
485  std::vector<nav_2d_msgs::Point2D> result;
486  result.reserve(indices.size());
487  for (unsigned int index : indices)
488  {
489  result.push_back(points[index]);
490  }
491  return result;
492 }
493 
494 std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::Polygon2D& polygon)
495 {
496  nav_2d_msgs::ComplexPolygon2D complex;
497  complex.outer = polygon;
498  return triangulate(complex);
499 }
500 
501 
502 } // namespace nav_2d_utils
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1)
Definition: geometry_help.h:52
nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue &polygon_xmlrpc)
Create a polygon from the given XmlRpcValue.
Definition: polygons.cpp:194
std::vector< std::vector< double > > parseVVD(const std::string &input)
Parse a vector of vectors of doubles from a string. Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...].
Definition: polygons.cpp:49
A set of utility functions for Bounds objects interacting with other messages/types.
Definition: bounds.h:45
nav_2d_msgs::Polygon2D polygonFromParams(const ros::NodeHandle &nh, const std::string parameter_name, bool search=true)
Load a polygon from a parameter, whether it is a string or array, or two arrays.
Definition: polygons.cpp:250
bool equals(const nav_2d_msgs::Polygon2D &polygon0, const nav_2d_msgs::Polygon2D &polygon1)
check if two polygons are equal. Used in testing
Definition: polygons.cpp:350
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
std::vector< nav_2d_msgs::Point2D > triangulate(const nav_2d_msgs::ComplexPolygon2D &polygon)
Decompose a complex polygon into a set of triangles.
Definition: polygons.cpp:462
XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector< double > &array)
Helper method to convert a vector of doubles.
Definition: polygons.cpp:276
int size() const
std::string resolveName(const std::string &name, bool remap=true) const
Exception to throw when Polygon doesn&#39;t load correctly.
Definition: polygons.h:52
Type const & getType() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue &value)
Helper function. Convert value to double.
Definition: polygons.cpp:161
void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector< double > &xs, std::vector< double > &ys)
Create two parallel arrays from a polygon.
Definition: polygons.cpp:339
std::string toXml() const
void setSize(int size)
nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points=16)
Create a "circular" polygon from a given radius.
Definition: polygons.cpp:112
std::vector< double > getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue &value)
Helper function. Convert value to double array.
Definition: polygons.cpp:180
bool searchParam(const std::string &key, std::string &result) const
void polygonToParams(const nav_2d_msgs::Polygon2D &polygon, const ros::NodeHandle &nh, const std::string parameter_name, bool array_of_arrays=true)
Save a polygon to a parameter.
Definition: polygons.cpp:311
TFSIMD_FORCE_INLINE const tfScalar & x() const
nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector< double > &xs, const std::vector< double > &ys)
Create a polygon from two parallel arrays.
Definition: polygons.cpp:317
bool hasMember(const std::string &name) const
bool getParam(const std::string &key, std::string &s) const
void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D &polygon, double &min_dist, double &max_dist)
Calculate the minimum and maximum distance from (0, 0) to any point on the polygon.
Definition: polygons.cpp:404
bool hasParam(const std::string &key) const
nav_2d_msgs::Polygon2D polygonFromString(const std::string &polygon_string)
Make a polygon from the given string. Format should be bracketed array of arrays of doubles...
Definition: polygons.cpp:127
nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D &polygon, const geometry_msgs::Pose2D &pose)
Translate and rotate a polygon to a new pose.
Definition: polygons.cpp:366
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
Definition: earcut.hpp:9
XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D &polygon, bool array_of_arrays=true)
Create XMLRPC Value for writing the polygon to the parameter server.
Definition: polygons.cpp:287
bool isInside(const nav_2d_msgs::Polygon2D &polygon, const double x, const double y)
Check if a given point is inside of a polygon.
Definition: polygons.cpp:382


nav_2d_utils
Author(s):
autogenerated on Sun Jan 10 2021 04:08:32