Go to the documentation of this file.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 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
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;
00158 float range;
00159
00160
00161 Polar(void)
00162 {
00163 heading = range = 0.0;
00164 }
00165
00166
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
00177
00178 namespace Coordinates
00179 {
00180
00181
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
00214 inline Polar MapXY_to_Polar(MapXY point,
00215 const nav_msgs::Odometry &origin)
00216 {
00217
00218
00219
00220
00221
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
00230 inline MapXY Polar_to_MapXY(Polar polar, const MapPose &origin)
00231 {
00232
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