footprint.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include<costmap_2d/costmap_math.h>
00031 #include <boost/tokenizer.hpp>
00032 #include <boost/foreach.hpp>
00033 #include <boost/algorithm/string.hpp>
00034 #include <costmap_2d/footprint.h>
00035 #include <costmap_2d/array_parser.h>
00036 #include<geometry_msgs/Point32.h>
00037 
00038 namespace costmap_2d
00039 {
00040 
00041 void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
00042 {
00043   min_dist = std::numeric_limits<double>::max();
00044   max_dist = 0.0;
00045 
00046   if (footprint.size() <= 2)
00047   {
00048     return;
00049   }
00050 
00051   for (unsigned int i = 0; i < footprint.size() - 1; ++i)
00052   {
00053     // check the distance from the robot center point to the first vertex
00054     double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
00055     double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
00056                                       footprint[i + 1].x, footprint[i + 1].y);
00057     min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
00058     max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
00059   }
00060 
00061   // we also need to do the last vertex and the first vertex
00062   double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
00063   double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
00064                                       footprint.front().x, footprint.front().y);
00065   min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
00066   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
00067 }
00068 
00069 geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
00070 {
00071   geometry_msgs::Point32 point32;
00072   point32.x = pt.x;
00073   point32.y = pt.y;
00074   point32.z = pt.z;
00075   return point32;
00076 }
00077 
00078 geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
00079 {
00080   geometry_msgs::Point point;
00081   point.x = pt.x;
00082   point.y = pt.y;
00083   point.z = pt.z;
00084   return point;
00085 }
00086 
00087 geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
00088 {
00089   geometry_msgs::Polygon polygon;
00090   for (int i = 0; i < pts.size(); i++){
00091     polygon.points.push_back(toPoint32(pts[i]));
00092   }
00093   return polygon;
00094 }
00095 
00096 std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
00097 {
00098   std::vector<geometry_msgs::Point> pts;
00099   for (int i = 0; i < polygon.points.size(); i++)
00100   {
00101     pts.push_back(toPoint(polygon.points[i]));
00102   }
00103   return pts;
00104 }
00105 
00106 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
00107                         std::vector<geometry_msgs::Point>& oriented_footprint)
00108 {
00109   // build the oriented footprint at a given location
00110   oriented_footprint.clear();
00111   double cos_th = cos(theta);
00112   double sin_th = sin(theta);
00113   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
00114   {
00115     geometry_msgs::Point new_pt;
00116     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
00117     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
00118     oriented_footprint.push_back(new_pt);
00119   }
00120 }
00121 
00122 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
00123                         geometry_msgs::PolygonStamped& oriented_footprint)
00124 {
00125   // build the oriented footprint at a given location
00126   oriented_footprint.polygon.points.clear();
00127   double cos_th = cos(theta);
00128   double sin_th = sin(theta);
00129   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
00130   {
00131     geometry_msgs::Point32 new_pt;
00132     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
00133     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
00134     oriented_footprint.polygon.points.push_back(new_pt);
00135   }
00136 }
00137 
00138 void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
00139 {
00140   // pad footprint in place
00141   for (unsigned int i = 0; i < footprint.size(); i++)
00142   {
00143     geometry_msgs::Point& pt = footprint[ i ];
00144     pt.x += sign0(pt.x) * padding;
00145     pt.y += sign0(pt.y) * padding;
00146   }
00147 }
00148 
00149 
00150 std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
00151 {
00152   std::vector<geometry_msgs::Point> points;
00153 
00154   // Loop over 16 angles around a circle making a point each time
00155   int N = 16;
00156   geometry_msgs::Point pt;
00157   for (int i = 0; i < N; ++i)
00158   {
00159     double angle = i * 2 * M_PI / N;
00160     pt.x = cos(angle) * radius;
00161     pt.y = sin(angle) * radius;
00162 
00163     points.push_back(pt);
00164   }
00165 
00166   return points;
00167 }
00168 
00169 
00170 bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
00171 {
00172   std::string error;
00173   std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
00174 
00175   if (error != "")
00176   {
00177     ROS_ERROR("Error parsing footprint parameter: '%s'", error.c_str());
00178     ROS_ERROR("  Footprint string was '%s'.", footprint_string.c_str());
00179     return false;
00180   }
00181 
00182   // convert vvf into points.
00183   if (vvf.size() < 3)
00184   {
00185     ROS_ERROR("You must specify at least three points for the robot footprint, reverting to previous footprint.");
00186     return false;
00187   }
00188   footprint.reserve(vvf.size());
00189   for (unsigned int i = 0; i < vvf.size(); i++)
00190   {
00191     if (vvf[ i ].size() == 2)
00192     {
00193       geometry_msgs::Point point;
00194       point.x = vvf[ i ][ 0 ];
00195       point.y = vvf[ i ][ 1 ];
00196       point.z = 0;
00197       footprint.push_back(point);
00198     }
00199     else
00200     {
00201       ROS_ERROR("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.",
00202                  int(vvf[ i ].size()));
00203       return false;
00204     }
00205   }
00206 
00207   return true;
00208 }
00209 
00210 
00211 
00212 std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
00213 {
00214   std::string full_param_name;
00215   std::string full_radius_param_name;
00216   std::vector<geometry_msgs::Point> points;
00217 
00218   if (nh.searchParam("footprint", full_param_name))
00219   {
00220     XmlRpc::XmlRpcValue footprint_xmlrpc;
00221     nh.getParam(full_param_name, footprint_xmlrpc);
00222     if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
00223     {
00224       if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
00225       {
00226         writeFootprintToParam(nh, points);
00227       }
00228     }
00229     else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
00230     {
00231       points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
00232       writeFootprintToParam(nh, points);
00233     }
00234   }
00235   else if (nh.searchParam("robot_radius", full_radius_param_name))
00236   {
00237     double robot_radius;
00238     nh.param(full_radius_param_name, robot_radius, 1.234);
00239     points = makeFootprintFromRadius(robot_radius);
00240     nh.setParam("robot_radius", robot_radius);
00241   }
00242   // Else neither param was found anywhere this knows about, so
00243   // defaults will come from dynamic_reconfigure stuff, set in
00244   // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
00245   return points;
00246 }
00247 
00248 void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
00249 {
00250   std::ostringstream oss;
00251   bool first = true;
00252   for (unsigned int i = 0; i < footprint.size(); i++)
00253   {
00254     geometry_msgs::Point p = footprint[ i ];
00255     if (first)
00256     {
00257       oss << "[[" << p.x << "," << p.y << "]";
00258       first = false;
00259     }
00260     else
00261     {
00262       oss << ",[" << p.x << "," << p.y << "]";
00263     }
00264   }
00265   oss << "]";
00266   nh.setParam("footprint", oss.str().c_str());
00267 }
00268 
00269 double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
00270 {
00271   // Make sure that the value we're looking at is either a double or an int.
00272   if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
00273       value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
00274   {
00275     std::string& value_string = value;
00276     ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.",
00277                full_param_name.c_str(), value_string.c_str());
00278     throw std::runtime_error("Values in the footprint specification must be numbers");
00279   }
00280   return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
00281 }
00282 
00283 std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
00284                                 const std::string& full_param_name)
00285 {
00286   // Make sure we have an array of at least 3 elements.
00287   if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
00288       footprint_xmlrpc.size() < 3)
00289   {
00290     ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
00291                full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
00292     throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
00293                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00294   }
00295 
00296   std::vector<geometry_msgs::Point> footprint;
00297   geometry_msgs::Point pt;
00298 
00299   for (int i = 0; i < footprint_xmlrpc.size(); ++i)
00300   {
00301     // Make sure each element of the list is an array of size 2. (x and y coordinates)
00302     XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
00303     if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
00304         point.size() != 2)
00305     {
00306       ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
00307                 "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
00308                  full_param_name.c_str());
00309       throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
00310                                "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00311     }
00312 
00313     pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
00314     pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
00315 
00316     footprint.push_back(pt);
00317   }
00318   return footprint;
00319 }
00320 
00321 }  // end namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21