coordinates.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /*
00003  *  Description:  Map coordinate class definitions
00004  *
00005  *  Copyright (C) 2009 Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: coordinates.h 1665 2011-08-17 15:11:43Z ruifei0713 $
00010  */
00011 
00012 #ifndef _COORDINATES_HH
00013 #define _COORDINATES_HH
00014 
00022 #include <math.h>
00023 #include <vector>
00024 
00025 #include <angles/angles.h>
00026 #include <geometry_msgs/Point.h>
00027 #include <geometry_msgs/Point32.h>
00028 #include <geometry_msgs/Pose.h>
00029 #include <nav_msgs/Odometry.h>
00030 #include <tf/transform_datatypes.h>
00031 #include <art/conversions.h>
00032 
00034 class LatLong
00035 {
00036 public:
00037   double latitude;
00038   double longitude;
00039 
00040   // void constructor
00041   LatLong(void)
00042   {
00043     latitude = longitude = 0.0;
00044   }
00045 
00046   // initialization constructor
00047   LatLong(double lat, double lon)
00048   {
00049     latitude = lat;
00050     longitude = lon;
00051   }
00052   bool operator==(const LatLong &that){
00053     //    double latdiff = this->latitude - that.latitude;
00054     //    double longdiff = this->longitude - that.longitude;
00055     //    printf("1:Lat: %3.12f Long %3.12f \n", this->latitude, this->longitude);
00056     //    printf("2:Lat: %3.12f Long %3.12f \n", that.latitude, that.longitude);
00057     //    printf("Difference %3.12f, %3.12f\n", latdiff, longdiff);
00058         bool match = this->latitude == that.latitude &&
00059       this->longitude == that.longitude;
00060         //printf("%s\n", match?"Match":"No Match");
00061       return (match);
00062   }
00063 };
00064 
00074 class MapXY                             
00075 {
00076 public:
00077   float x;
00078   float y;
00079 
00080   // constructors
00081   MapXY(void): x(0.0), y(0.0) {};
00082   MapXY(float _x, float _y): x(_x), y(_y) {};
00083   MapXY(double _x, double _y): x(_x), y(_y) {};
00084   MapXY(const geometry_msgs::Point &pt): x(pt.x), y(pt.y) {};
00085   MapXY(const geometry_msgs::Point32 &pt): x(pt.x), y(pt.y) {};
00086   MapXY(const MapXY &pt): x(pt.x), y(pt.y) {};
00087   
00088   // TODO figure out how to define this without circular references
00089   //MapXY(const MapPose &pose): x(pose.map.x), y(pose.map.y) {};
00090 
00091   bool operator==(const MapXY &that) const
00092   {
00093     return (this->x == that.x && this->y == that.y);
00094   }
00095   bool operator!=(const MapXY &that) const
00096   {
00097     return (this->x != that.x || this->y != that.y);
00098   }
00099   MapXY operator+(const MapXY &that) const
00100   {
00101     return MapXY(this->x + that.x, this->y + that.y);
00102   }
00103   MapXY operator-(const MapXY &that) const
00104   {
00105     return MapXY(this->x - that.x, this->y - that.y);
00106   }
00107 
00108   void toMsg(geometry_msgs::Point &pt) {
00109     pt.x = x;
00110     pt.y = y;
00111     pt.z = 0;
00112   }
00113 
00114   void toMsg(geometry_msgs::Point32 &pt) {
00115     pt.x = x;
00116     pt.y = y;
00117     pt.z = 0;
00118   }
00119  
00120 };
00121 
00122 typedef std::vector<MapXY> mapxy_list_t;
00123 
00124 
00131 class MapPose                           
00132 {
00133 public:
00134   MapXY map;
00135   float yaw;
00136 
00137   // constructors
00138   MapPose(void): map(0.0, 0.0), yaw(0.0) {};
00139   MapPose(MapXY _map, float _yaw): map(_map), yaw(_yaw) {};
00140   MapPose(float _x, float _y, float _yaw): map(_x, _y), yaw(_yaw) {};
00141   MapPose(const geometry_msgs::Pose &pose)
00142   {
00143     map = MapXY(pose.position);
00144     yaw = tf::getYaw(pose.orientation);
00145   };
00146 };
00147 
00154 class Polar
00155 {
00156 public:
00157   float heading;                        // radians
00158   float range;                          // meters
00159 
00160   // void constructor
00161   Polar(void)
00162   {
00163     heading = range = 0.0;
00164   }
00165 
00166   // initialization constructor
00167   Polar(float _heading, float _range)
00168   {
00169     heading = _heading;
00170     range = _range;
00171   }
00172 };
00173 
00174 typedef std::vector<Polar> polar_list_t;
00175 
00176 // coordinate transformation functions
00177 
00178 namespace Coordinates
00179 {
00180   // normalize a heading to the range (-M_PI, M_PI]
00181   // (disallows -M_PI, allowing normalized headings to compare equal)
00182   inline float normalize(float heading)
00183   {
00184     while (heading > M_PI)
00185       heading -= TWOPI;
00186     while (heading <= -M_PI)
00187       heading += TWOPI;
00188     return heading;
00189   }
00190 
00191 
00192   inline float mod2pi(float angle)
00193   {
00194     while (angle < 0.0) 
00195       angle = angle + TWOPI;
00196     while (angle >= TWOPI) 
00197       angle = angle - TWOPI;
00198     return angle;
00199   }
00200 
00201 
00202   inline float bearing(MapXY from_point, MapXY to_point)
00203   {
00204     MapXY offset = to_point - from_point;
00205     return atan2f(offset.y, offset.x);
00206   }
00207 
00208   inline float bearing(MapPose from_pose, MapXY to_point)
00209   {
00210     return normalize(bearing(from_pose.map, to_point) - from_pose.yaw);
00211   }
00212 
00213   // transform MapXY coordinate to egocentric Polar
00214   inline Polar MapXY_to_Polar(MapXY point,
00215                               const nav_msgs::Odometry &origin)
00216   {
00217     // TODO: figure out how to use Euclidean::DistanceTo() function,
00218     // (it is not working here for some reason) 
00219 
00220     //PFB: It's because there are circular header dependencies.
00221     //euclidean_distance.h needs this header file.
00222     
00223     MapPose orgpose = MapPose(origin.pose.pose);
00224     MapXY diff = point - orgpose.map;
00225     return Polar(bearing(orgpose, point),
00226                  sqrtf(diff.x*diff.x + diff.y*diff.y));
00227   }
00228 
00229   // transform egocentric Polar coordinate to MapXY
00230   inline MapXY Polar_to_MapXY(Polar polar, const MapPose &origin)
00231   {
00232     // Don't normalize map_heading, we just need sin() and cos().
00233     float map_heading = origin.yaw + polar.heading;
00234     MapXY retval;
00235     retval.x = (origin.map.x + cosf(map_heading) * polar.range);
00236     retval.y = (origin.map.y + sinf(map_heading) * polar.range);
00237     return retval;
00238   }
00239 
00240   inline float sign(float val)
00241   {
00242     return (val >= 0? 1.0f: -1.0f);
00243   }
00244 };
00245 
00246 #endif // _COORDINATES_HH


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34