00001
00002
00003
00004
00005
00006
00007
00008
00009
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
00041 LatLong(void)
00042 {
00043 latitude = longitude = 0.0;
00044 }
00045
00046
00047 LatLong(double lat, double lon)
00048 {
00049 latitude = lat;
00050 longitude = lon;
00051 }
00052 bool operator==(const LatLong &that){
00053
00054
00055
00056
00057
00058 bool match = this->latitude == that.latitude &&
00059 this->longitude == that.longitude;
00060
00061 return (match);
00062 }
00063 };
00064
00074 class MapXY
00075 {
00076 public:
00077 float x;
00078 float y;
00079
00080
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
00089
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
00109 typedef std::vector<MapXY> mapxy_list_t;
00110
00111
00118 class MapPose
00119 {
00120 public:
00121 MapXY map;
00122 float yaw;
00123
00124
00125 MapPose(void): map(0.0, 0.0), yaw(0.0) {};
00126 MapPose(MapXY _map, float _yaw): map(_map), yaw(_yaw) {};
00127 MapPose(float _x, float _y, float _yaw): map(_x, _y), yaw(_yaw) {};
00128 MapPose(const geometry_msgs::Pose &pose)
00129 {
00130 map = MapXY(pose.position);
00131 yaw = tf::getYaw(pose.orientation);
00132 };
00133 };
00134
00141 class Polar
00142 {
00143 public:
00144 float heading;
00145 float range;
00146
00147
00148 Polar(void)
00149 {
00150 heading = range = 0.0;
00151 }
00152
00153
00154 Polar(float _heading, float _range)
00155 {
00156 heading = _heading;
00157 range = _range;
00158 }
00159 };
00160
00161 typedef std::vector<Polar> polar_list_t;
00162
00163
00164
00165 namespace Coordinates
00166 {
00167
00168
00169 inline float normalize(float heading)
00170 {
00171 while (heading > M_PI)
00172 heading -= TWOPI;
00173 while (heading <= -M_PI)
00174 heading += TWOPI;
00175 return heading;
00176 }
00177
00178
00179 inline float mod2pi(float angle)
00180 {
00181 while (angle < 0.0)
00182 angle = angle + TWOPI;
00183 while (angle >= TWOPI)
00184 angle = angle - TWOPI;
00185 return angle;
00186 }
00187
00188
00189 inline float bearing(MapXY from_point, MapXY to_point)
00190 {
00191 MapXY offset = to_point - from_point;
00192 return atan2f(offset.y, offset.x);
00193 }
00194
00195 inline float bearing(MapPose from_pose, MapXY to_point)
00196 {
00197 return normalize(bearing(from_pose.map, to_point) - from_pose.yaw);
00198 }
00199
00200
00201 inline Polar MapXY_to_Polar(MapXY point,
00202 const nav_msgs::Odometry &origin)
00203 {
00204
00205
00206
00207
00208
00209
00210 MapPose orgpose = MapPose(origin.pose.pose);
00211 MapXY diff = point - orgpose.map;
00212 return Polar(bearing(orgpose, point),
00213 sqrtf(diff.x*diff.x + diff.y*diff.y));
00214 }
00215
00216
00217 inline MapXY Polar_to_MapXY(Polar polar, const MapPose &origin)
00218 {
00219
00220 float map_heading = origin.yaw + polar.heading;
00221 MapXY retval;
00222 retval.x = (origin.map.x + cosf(map_heading) * polar.range);
00223 retval.y = (origin.map.y + sinf(map_heading) * polar.range);
00224 return retval;
00225 }
00226
00227 inline float sign(float val)
00228 {
00229 return (val >= 0? 1.0f: -1.0f);
00230 }
00231 };
00232
00233 #endif // _COORDINATES_HH