first commit

src/footprint.cpp

@@ -27,18 +27,18 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include<costmap_2d/costmap_math.h>
+#include <costmap_2d/costmap_math.h>
 #include <boost/tokenizer.hpp>
 #include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
 #include <costmap_2d/footprint.h>
 #include <costmap_2d/array_parser.h>
-#include<costmap_2d/msg.h>
+// #include<costmap_2d/msg.h>
 
 namespace costmap_2d
 {
 
-void calculateMinAndMaxDistances(const std::vector<Point>& footprint, double& min_dist, double& max_dist)
+void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
 {
   min_dist = std::numeric_limits<double>::max();
   max_dist = 0.0;
@@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector<Point>& footprint, double& mi
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
 }
 
-Point32 toPoint32(Point pt)
+geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
 {
-  Point32 point32;
+  geometry_msgs::Point32 point32;
   point32.x = pt.x;
   point32.y = pt.y;
   point32.z = pt.z;
   return point32;
 }
 
-Point toPoint(Point32 pt)
+geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
 {
-  Point point;
+  geometry_msgs::Point point;
   point.x = pt.x;
   point.y = pt.y;
   point.z = pt.z;
   return point;
 }
 
-Polygon toPolygon(std::vector<Point> pts)
+geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
 {
-  Polygon polygon;
+  geometry_msgs::Polygon polygon;
   for (int i = 0; i < pts.size(); i++){
     polygon.points.push_back(toPoint32(pts[i]));
   }
   return polygon;
 }
 
-std::vector<Point> toPointVector(Polygon polygon)
+std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
 {
-  std::vector<Point> pts;
+  std::vector<geometry_msgs::Point> pts;
   for (int i = 0; i < polygon.points.size(); i++)
   {
     pts.push_back(toPoint(polygon.points[i]));
@@ -103,8 +103,8 @@ std::vector<Point> toPointVector(Polygon polygon)
   return pts;
 }
 
-void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
-                        std::vector<Point>& oriented_footprint)
+void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+                        std::vector<geometry_msgs::Point>& oriented_footprint)
 {
   // build the oriented footprint at a given location
   oriented_footprint.clear();
@@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector<Poin
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
   {
-    Point new_pt;
+    geometry_msgs::Point new_pt;
     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
     oriented_footprint.push_back(new_pt);
   }
 }
 
-void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
-                        PolygonStamped& oriented_footprint)
+void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+                        geometry_msgs::PolygonStamped& oriented_footprint)
 {
   // build the oriented footprint at a given location
   oriented_footprint.polygon.points.clear();
@@ -128,32 +128,32 @@ void transformFootprint(double x, double y, double theta, const std::vector<Poin
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
   {
-    Point32 new_pt;
+    geometry_msgs::Point32 new_pt;
     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
     oriented_footprint.polygon.points.push_back(new_pt);
   }
 }
 
-void padFootprint(std::vector<Point>& footprint, double padding)
+void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
 {
   // pad footprint in place
   for (unsigned int i = 0; i < footprint.size(); i++)
   {
-    Point& pt = footprint[ i ];
+    geometry_msgs::Point& pt = footprint[ i ];
     pt.x += sign0(pt.x) * padding;
     pt.y += sign0(pt.y) * padding;
   }
 }
 
 
-std::vector<Point> makeFootprintFromRadius(double radius)
+std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
 {
-  std::vector<Point> points;
+  std::vector<geometry_msgs::Point> points;
 
   // Loop over 16 angles around a circle making a point each time
   int N = 16;
-  Point pt;
+  geometry_msgs::Point pt;
   for (int i = 0; i < N; ++i)
   {
     double angle = i * 2 * M_PI / N;
@@ -167,7 +167,7 @@ std::vector<Point> makeFootprintFromRadius(double radius)
 }
 
 
-bool makeFootprintFromString(const std::string& footprint_string, std::vector<Point>& footprint)
+bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
 {
   std::string error;
   std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
@@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
   {
     if (vvf[ i ].size() == 2)
     {
-      Point point;
+      geometry_msgs::Point point;
       point.x = vvf[ i ][ 0 ];
       point.y = vvf[ i ][ 1 ];
       point.z = 0;
@@ -209,11 +209,11 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
 
 
 
-// std::vector<Point> makeFootprintFromParams(ros::NodeHandle& nh)
+// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
 // {
 //   std::string full_param_name;
 //   std::string full_radius_param_name;
-//   std::vector<Point> points;
+//   std::vector<geometry_msgs::Point> points;
 
 //   if (nh.searchParam("footprint", full_param_name))
 //   {
@@ -249,13 +249,13 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
 //   return points;
 // }
 
-// void writeFootprintToParam(std::string& nh, const std::vector<Point>& footprint)
+// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
 // {
 //   std::ostringstream oss;
 //   bool first = true;
 //   for (unsigned int i = 0; i < footprint.size(); i++)
 //   {
-//     Point p = footprint[ i ];
+//     geometry_msgs::Point p = footprint[ i ];
 //     if (first)
 //     {
 //       oss << "[[" << p.x << "," << p.y << "]";
@@ -284,7 +284,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
 //   return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
 // }
 
-// std::vector<Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
+// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
 //                                 const std::string& full_param_name)
 // {
 //   // Make sure we have an array of at least 3 elements.
@@ -297,8 +297,8 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
 //                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
 //   }
 
-//   std::vector<Point> footprint;
-//   Point pt;
+//   std::vector<geometry_msgs::Point> footprint;
+//   geometry_msgs::Point pt;
 
 //   for (int i = 0; i < footprint_xmlrpc.size(); ++i)
 //   {