Go to the documentation of this file.00001
00038 #include "enu/enu.h"
00039
00040 #include <boost/bind.hpp>
00041 #include <string>
00042
00043 extern "C" {
00044 #include "libswiftnav/coord_system.h"
00045 }
00046
00047 #include "ros/ros.h"
00048 #include "sensor_msgs/NavSatFix.h"
00049 #include "geometry_msgs/Point.h"
00050
00051 #define TO_RADIANS (M_PI/180)
00052 #define TO_DEGREES (180/M_PI)
00053
00054
00055 extern "C" int xerbla_(char *srname, int *info)
00056 {
00057 ROS_ERROR("On entry to %6s, parameter number %2i had an illegal value", srname, *info);
00058 }
00059
00060 namespace enu {
00061
00062 static void fix_to_ecef(const sensor_msgs::NavSatFix& fix, double ecef[3]) {
00063 double llh[3] = { fix.latitude * TO_RADIANS,
00064 fix.longitude * TO_RADIANS,
00065 fix.altitude };
00066 wgsllh2ecef(llh, ecef);
00067 }
00068
00069 void fix_to_point(const sensor_msgs::NavSatFix& fix,
00070 const sensor_msgs::NavSatFix& datum,
00071 geometry_msgs::Point* point_ptr) {
00072
00073 double ecef_datum[3];
00074 fix_to_ecef(datum, ecef_datum);
00075
00076
00077
00078 double llh[3] = { fix.latitude * TO_RADIANS,
00079 fix.longitude * TO_RADIANS,
00080 fix.altitude };
00081 double ecef[3];
00082 wgsllh2ecef(llh, ecef);
00083
00084
00085
00086 double ned[3];
00087 wgsecef2ned_d(ecef, ecef_datum, ned);
00088
00089
00090 point_ptr->x = ned[1];
00091 point_ptr->y = ned[0];
00092 point_ptr->z = -ned[2];
00093 }
00094
00095 void point_to_fix(const geometry_msgs::Point& point,
00096 const sensor_msgs::NavSatFix& datum,
00097 sensor_msgs::NavSatFix* fix_ptr) {
00098
00099 double ecef_datum[3];
00100 fix_to_ecef(datum, ecef_datum);
00101
00102
00103
00104 double ned[3] = { point.y, point.x, -point.z };
00105
00106 double ecef[3];
00107 wgsned2ecef_d(ned, ecef_datum, ecef);
00108
00109 double llh_raw[3];
00110 wgsecef2llh(ecef, llh_raw);
00111
00112
00113 fix_ptr->latitude = llh_raw[0] * TO_DEGREES;
00114 fix_ptr->longitude = llh_raw[1] * TO_DEGREES;
00115 fix_ptr->altitude = llh_raw[2];
00116 }
00117
00118 }