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 #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
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
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
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
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
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
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
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
00243
00244
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
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
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
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
00322
00323
00324 }