enu.cpp
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   // Convert reference LLH-formatted datum to ECEF format
00073   double ecef_datum[3];
00074   fix_to_ecef(datum, ecef_datum);
00075 
00076   // Prepare the appropriate input vector to convert the input latlon
00077   // to an ECEF triplet.
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   // ECEF triplet is converted to north-east-down (NED), by combining it
00085   // with the ECEF-formatted datum point.
00086   double ned[3];
00087   wgsecef2ned_d(ecef, ecef_datum, ned);
00088 
00089   // Output data
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   // Convert reference LLH-formatted datum to ECEF format
00099   double ecef_datum[3];
00100   fix_to_ecef(datum, ecef_datum);
00101 
00102   // Prepare NED vector from ENU coordinates, perform conversion in libswiftnav
00103   // library calls.
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   // Output Fix message. Convert radian latlon output back to degrees.
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 }  // namespace enu


enu
Author(s): Mike Purvis , Ryan Gariepy
autogenerated on Fri Aug 28 2015 10:37:27