polygons.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <nav_2d_utils/polygons.h>
00036 #include <nav_2d_utils/geometry_help.h>
00037 #include <algorithm>
00038 #include <limits>
00039 #include <string>
00040 #include <vector>
00041 
00042 namespace nav_2d_utils
00043 {
00044 
00045 using nav_2d_msgs::Point2D;
00046 using nav_2d_msgs::Polygon2D;
00047 
00048 std::vector<std::vector<double> > parseVVD(const std::string& input)
00049 {
00050   std::vector<std::vector<double> > result;
00051 
00052   std::stringstream input_ss(input);
00053   int depth = 0;
00054   std::vector<double> current_vector;
00055   while (input_ss.good())
00056   {
00057     switch (input_ss.peek())
00058     {
00059     case EOF:
00060       break;
00061     case '[':
00062       depth++;
00063       if (depth > 2)
00064       {
00065         throw PolygonParseException("Array depth greater than 2");
00066       }
00067       input_ss.get();
00068       current_vector.clear();
00069       break;
00070     case ']':
00071       depth--;
00072       if (depth < 0)
00073       {
00074         throw PolygonParseException("More close ] than open [");
00075       }
00076       input_ss.get();
00077       if (depth == 1)
00078       {
00079         result.push_back(current_vector);
00080       }
00081       break;
00082     case ',':
00083     case ' ':
00084     case '\t':
00085       input_ss.get();
00086       break;
00087     default:  // All other characters should be part of the numbers.
00088       if (depth != 2)
00089       {
00090         std::stringstream err_ss;
00091         err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
00092         throw PolygonParseException(err_ss.str());
00093       }
00094       double value;
00095       if (input_ss >> value)
00096       {
00097         current_vector.push_back(value);
00098       }
00099       break;
00100     }
00101   }
00102 
00103   if (depth != 0)
00104   {
00105     throw PolygonParseException("Unterminated vector string.");
00106   }
00107 
00108   return result;
00109 }
00110 
00111 Polygon2D polygonFromRadius(const double radius, const unsigned int num_points)
00112 {
00113   Polygon2D polygon;
00114   Point2D pt;
00115   for (unsigned int i = 0; i < num_points; ++i)
00116   {
00117     double angle = i * 2 * M_PI / num_points;
00118     pt.x = cos(angle) * radius;
00119     pt.y = sin(angle) * radius;
00120     polygon.points.push_back(pt);
00121   }
00122 
00123   return polygon;
00124 }
00125 
00126 Polygon2D polygonFromString(const std::string& polygon_string)
00127 {
00128   Polygon2D polygon;
00129   // Will throw PolygonParseException if problematic
00130   std::vector<std::vector<double> > vvd = parseVVD(polygon_string);
00131 
00132   // convert vvd into points.
00133   if (vvd.size() < 3)
00134   {
00135     throw PolygonParseException("You must specify at least three points for the polygon.");
00136   }
00137 
00138   polygon.points.resize(vvd.size());
00139   for (unsigned int i = 0; i < vvd.size(); i++)
00140   {
00141     if (vvd[ i ].size() != 2)
00142     {
00143       std::stringstream err_ss;
00144       err_ss << "Points in the polygon specification must be pairs of numbers. Point index " << i << " had ";
00145       err_ss << int(vvd[ i ].size()) << " numbers.";
00146       throw PolygonParseException(err_ss.str());
00147     }
00148 
00149     polygon.points[i].x = vvd[i][0];
00150     polygon.points[i].y = vvd[i][1];
00151   }
00152 
00153   return polygon;
00154 }
00155 
00156 
00160 double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value)
00161 {
00162   if (value.getType() == XmlRpc::XmlRpcValue::TypeInt)
00163   {
00164     return static_cast<double>(static_cast<int>(value));
00165   }
00166   else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble)
00167   {
00168     return static_cast<double>(value);
00169   }
00170 
00171   std::stringstream err_ss;
00172   err_ss << "Values in the polygon specification must be numbers. Found value: " << value.toXml();
00173   throw PolygonParseException(err_ss.str());
00174 }
00175 
00179 std::vector<double> getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue& value)
00180 {
00181   if (value.getType() != XmlRpc::XmlRpcValue::TypeArray)
00182   {
00183     throw PolygonParseException("Subarray must have type list.");
00184   }
00185   std::vector<double> array;
00186   for (int i = 0; i < value.size(); i++)
00187   {
00188     array.push_back(getNumberFromXMLRPC(value[i]));
00189   }
00190   return array;
00191 }
00192 
00193 Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
00194 {
00195   if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
00196       polygon_xmlrpc != "" && polygon_xmlrpc != "[]")
00197   {
00198     return polygonFromString(std::string(polygon_xmlrpc));
00199   }
00200 
00201   if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeStruct)
00202   {
00203     if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y"))
00204     {
00205       throw PolygonParseException("Dict-like Polygon must specify members x and y.");
00206     }
00207     std::vector<double> xs = getNumberVectorFromXMLRPC(polygon_xmlrpc["x"]);
00208     std::vector<double> ys = getNumberVectorFromXMLRPC(polygon_xmlrpc["y"]);
00209     return polygonFromParallelArrays(xs, ys);
00210   }
00211 
00212   // Make sure we have an array of at least 3 elements.
00213   if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray)
00214   {
00215     std::stringstream err_ss;
00216     err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType()
00217            << " instead.";
00218     throw PolygonParseException(err_ss.str());
00219   }
00220   else if (polygon_xmlrpc.size() < 3)
00221   {
00222     throw PolygonParseException("You must specify at least three points for the polygon.");
00223   }
00224 
00225   Polygon2D polygon;
00226   Point2D pt;
00227   for (int i = 0; i < polygon_xmlrpc.size(); ++i)
00228   {
00229     // Make sure each element of the list is an array of size 2. (x and y coordinates)
00230     XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
00231     if (point_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
00232     {
00233       std::stringstream err_ss;
00234       err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead.";
00235       throw PolygonParseException(err_ss.str());
00236     }
00237     else if (point_xml.size() != 2)
00238     {
00239       throw PolygonParseException("Each point must have two numbers (x and y).");
00240     }
00241 
00242     pt.x = getNumberFromXMLRPC(point_xml[0]);
00243     pt.y = getNumberFromXMLRPC(point_xml[1]);
00244     polygon.points.push_back(pt);
00245   }
00246   return polygon;
00247 }
00248 
00249 Polygon2D polygonFromParams(const ros::NodeHandle& nh, const std::string parameter_name, bool search)
00250 {
00251   std::string full_param_name;
00252   if (search)
00253   {
00254     nh.searchParam(parameter_name, full_param_name);
00255   }
00256   else
00257   {
00258     full_param_name = parameter_name;
00259   }
00260 
00261   if (!nh.hasParam(full_param_name))
00262   {
00263     std::stringstream err_ss;
00264     err_ss << "Parameter " << parameter_name << "(" + nh.resolveName(parameter_name) << ") not found.";
00265     throw PolygonParseException(err_ss.str());
00266   }
00267   XmlRpc::XmlRpcValue polygon_xmlrpc;
00268   nh.getParam(full_param_name, polygon_xmlrpc);
00269   return polygonFromXMLRPC(polygon_xmlrpc);
00270 }
00271 
00275 XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
00276 {
00277   XmlRpc::XmlRpcValue xml;
00278   xml.setSize(array.size());
00279   for (unsigned int i = 0; i < array.size(); ++i)
00280   {
00281     xml[i] = array[i];
00282   }
00283   return xml;
00284 }
00285 
00286 XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
00287 {
00288   XmlRpc::XmlRpcValue xml;
00289   if (array_of_arrays)
00290   {
00291     xml.setSize(polygon.points.size());
00292     for (unsigned int i = 0; i < polygon.points.size(); ++i)
00293     {
00294       xml[i].setSize(2);
00295       const Point2D& p = polygon.points[i];
00296       xml[i][0] = p.x;
00297       xml[i][1] = p.y;
00298     }
00299   }
00300   else
00301   {
00302     std::vector<double> xs, ys;
00303     polygonToParallelArrays(polygon, xs, ys);
00304     xml["x"] = vectorToXMLRPC(xs);
00305     xml["y"] = vectorToXMLRPC(ys);
00306   }
00307   return xml;
00308 }
00309 
00310 void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const ros::NodeHandle& nh, const std::string parameter_name,
00311                      bool array_of_arrays)
00312 {
00313   nh.setParam(parameter_name, polygonToXMLRPC(polygon, array_of_arrays));
00314 }
00315 
00316 nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys)
00317 {
00318   if (xs.size() < 3)
00319   {
00320     throw PolygonParseException("You must specify at least three points for the polygon.");
00321   }
00322   else if (xs.size() != ys.size())
00323   {
00324     throw PolygonParseException("Length of x array does not match length of y array.");
00325   }
00326 
00327   Polygon2D polygon;
00328   polygon.points.resize(xs.size());
00329   for (unsigned int i = 0; i < xs.size(); i++)
00330   {
00331     Point2D& pt = polygon.points[i];
00332     pt.x = xs[i];
00333     pt.y = ys[i];
00334   }
00335   return polygon;
00336 }
00337 
00338 void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys)
00339 {
00340   xs.clear();
00341   ys.clear();
00342   for (const Point2D& pt : polygon.points)
00343   {
00344     xs.push_back(pt.x);
00345     ys.push_back(pt.y);
00346   }
00347 }
00348 
00349 bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1)
00350 {
00351   if (polygon0.points.size() != polygon1.points.size())
00352   {
00353     return false;
00354   }
00355   for (unsigned int i=0; i < polygon0.points.size(); i++)
00356   {
00357     if (polygon0.points[i].x != polygon1.points[i].x ||polygon0.points[i].y != polygon1.points[i].y)
00358     {
00359       return false;
00360     }
00361   }
00362   return true;
00363 }
00364 
00365 nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
00366                                          const geometry_msgs::Pose2D& pose)
00367 {
00368   nav_2d_msgs::Polygon2D new_polygon;
00369   new_polygon.points.resize(polygon.points.size());
00370   double cos_th = cos(pose.theta);
00371   double sin_th = sin(pose.theta);
00372   for (unsigned int i = 0; i < polygon.points.size(); ++i)
00373   {
00374     nav_2d_msgs::Point2D& new_pt = new_polygon.points[i];
00375     new_pt.x = pose.x + polygon.points[i].x * cos_th - polygon.points[i].y * sin_th;
00376     new_pt.y = pose.y + polygon.points[i].x * sin_th + polygon.points[i].y * cos_th;
00377   }
00378   return new_polygon;
00379 }
00380 
00381 bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y)
00382 {
00383   // Determine if the given point is inside the polygon using the number of crossings method
00384   // https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
00385   int n = polygon.points.size();
00386   int cross = 0;
00387   // Loop from i = [0 ... n - 1] and j = [n - 1, 0 ... n - 2]
00388   // Ensures first point connects to last point
00389   for (int i = 0, j = n - 1; i < n; j = i++)
00390   {
00391     // Check if the line to x,y crosses this edge
00392     if ( ((polygon.points[i].y > y) != (polygon.points[j].y > y))
00393            && (x < (polygon.points[j].x - polygon.points[i].x) * (y - polygon.points[i].y) /
00394             (polygon.points[j].y - polygon.points[i].y) + polygon.points[i].x) )
00395     {
00396       cross++;
00397     }
00398   }
00399   // Return true if the number of crossings is odd
00400   return cross % 2 > 0;
00401 }
00402 
00403 void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist)
00404 {
00405   min_dist = std::numeric_limits<double>::max();
00406   max_dist = 0.0;
00407   auto& points = polygon.points;
00408   if (points.size() == 0)
00409   {
00410     return;
00411   }
00412 
00413   for (unsigned int i = 0; i < points.size() - 1; ++i)
00414   {
00415     // check the distance from the robot center point to the first vertex
00416     double vertex_dist = hypot(points[i].x, points[i].y);
00417     double edge_dist = distanceToLine(0.0, 0.0, points[i].x, points[i].y,
00418                                       points[i + 1].x, points[i + 1].y);
00419     min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
00420     max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
00421   }
00422 
00423   // we also need to do the last vertex and the first vertex
00424   double vertex_dist = hypot(points.back().x, points.back().y);
00425   double edge_dist = distanceToLine(0.0, 0.0, points.back().x, points.back().y,
00426                                       points.front().x, points.front().y);
00427   min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
00428   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
00429 }
00430 
00431 }  // namespace nav_2d_utils


nav_2d_utils
Author(s):
autogenerated on Wed Jun 26 2019 20:09:36