00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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:
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
00130 std::vector<std::vector<double> > vvd = parseVVD(polygon_string);
00131
00132
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
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
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
00384
00385 int n = polygon.points.size();
00386 int cross = 0;
00387
00388
00389 for (int i = 0, j = n - 1; i < n; j = i++)
00390 {
00391
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
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
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
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 }