to_fix.cpp
Go to the documentation of this file.
00001 
00047 #include "ros/ros.h"
00048 #include "sensor_msgs/NavSatFix.h"
00049 #include "nav_msgs/Odometry.h"
00050 
00051 #include <boost/bind.hpp>
00052 
00053 extern "C" {
00054   // The backend of this node is an included C library called libswiftnav.
00055   #include "coord_system.h"
00056 }
00057 
00058 #define TO_RADIANS (M_PI/180)
00059 #define TO_DEGREES (180/M_PI)
00060 
00061 
00062 static void handle_enu(const nav_msgs::OdometryConstPtr odom_ptr,
00063                        const double ecef_datum[3],
00064                        const ros::Publisher& pub_fix)
00065 {
00066   // Prepare NED vector from ENU coordinates, perform conversion in libswiftnav
00067   // library calls.
00068   double ned[3] = { odom_ptr->pose.pose.position.y,
00069                     odom_ptr->pose.pose.position.x
00070                     -odom_ptr->pose.pose.position.z };
00071 
00072   double ecef[3];
00073   wgsned2ecef_d(ned, ecef_datum, ecef);
00074   
00075   double llh[3];
00076   wgsecef2llh(ecef, llh);
00077 
00078   // Prepare output Fix message. Copy over timestamp from source message,
00079   // convert radian latlon output back to degrees.
00080   sensor_msgs::NavSatFix fix_msg;
00081   fix_msg.header.stamp = odom_ptr->header.stamp;
00082   fix_msg.latitude = llh[0] * TO_DEGREES;
00083   fix_msg.longitude = llh[1] * TO_DEGREES;
00084   fix_msg.altitude = llh[2];
00085 
00086   // We only need to populate the diagonals of the covariance matrix; the
00087   // rest initialize to zero automatically, which is correct as the
00088   // dimensions of the state are independent.
00089   fix_msg.position_covariance[0] = odom_ptr->pose.covariance[0];
00090   fix_msg.position_covariance[4] = odom_ptr->pose.covariance[7];
00091   fix_msg.position_covariance[8] = odom_ptr->pose.covariance[14];
00092 
00093   pub_fix.publish(fix_msg); 
00094 }
00095 
00096 
00097 static void handle_datum(const sensor_msgs::NavSatFixConstPtr datum_ptr,
00098                          ros::NodeHandle& n)
00099 {
00100   // Convert received datum to latlon using function from libswiftnav.
00101   double llh[3] = { datum_ptr->latitude * TO_RADIANS,
00102                     datum_ptr->longitude * TO_RADIANS,
00103                     datum_ptr->altitude };
00104   static double ecef_datum[3];
00105   wgsllh2ecef(llh, ecef_datum);
00106 
00107   // Pass datum into the data subscriber. Subscriber, Publisher, and the datum
00108   // array are static, so that they stick around when this function has exited.
00109   static ros::Publisher pub_fix = n.advertise<sensor_msgs::NavSatFix>("fix", 5);
00110   static ros::Subscriber sub_enu = n.subscribe<nav_msgs::Odometry>("enu", 5, 
00111       boost::bind(handle_enu, _1, ecef_datum, boost::ref(pub_fix)));
00112 }
00113 
00114 
00115 int main(int argc, char **argv)
00116 {
00117   ros::init(argc, argv, "to_fix");
00118   ros::NodeHandle n;
00119 
00120   // Initially just subscribe to the datum topic. Once we receive that, we'll
00121   // spin up the rest of the node's activities.
00122   ros::Subscriber sub_datum = n.subscribe<sensor_msgs::NavSatFix>("enu_datum", 5,
00123       boost::bind(handle_datum, _1, boost::ref(n)));
00124 
00125   ros::spin();
00126   return 0;
00127 }
00128 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07