to_fix.cpp
Go to the documentation of this file.
00001 
00038 #include <boost/bind.hpp>
00039 
00040 #include "ros/ros.h"
00041 #include "sensor_msgs/NavSatFix.h"
00042 #include "nav_msgs/Odometry.h"
00043 
00044 #include "enu/enu.h"  // ROS wrapper for conversion functions
00045 
00046 static void handle_enu(const nav_msgs::OdometryConstPtr odom_ptr,
00047                        const sensor_msgs::NavSatFix& datum,
00048                        const ros::Publisher& pub_fix) {
00049   // Prepare LLH from ENU coordinates, perform conversion
00050   // using predefined datum
00051   // Use input message frame_id and timestamp in output fix message
00052   sensor_msgs::NavSatFix fix;
00053   enu::point_to_fix(odom_ptr->pose.pose.position, datum, &fix);
00054 
00055   fix.header.frame_id = odom_ptr->child_frame_id;
00056   fix.header.stamp = odom_ptr->header.stamp;
00057 
00058   // We only need to populate the diagonals of the covariance matrix; the
00059   // rest initialize to zero automatically, which is correct as the
00060   // dimensions of the state are independent.
00061   fix.position_covariance[0] = odom_ptr->pose.covariance[0];
00062   fix.position_covariance[4] = odom_ptr->pose.covariance[7];
00063   fix.position_covariance[8] = odom_ptr->pose.covariance[14];
00064 
00065   pub_fix.publish(fix);
00066 }
00067 
00068 static void handle_datum(const sensor_msgs::NavSatFixConstPtr datum_ptr,
00069                          ros::NodeHandle& n) {
00070   // Pass datum into the data subscriber. Subscriber, Publisher, and the datum
00071   // array are static, so that they stick around when this function has exited.
00072   static sensor_msgs::NavSatFix datum(*datum_ptr);
00073   static ros::Publisher pub_fix = n.advertise<sensor_msgs::NavSatFix>("fix", 5);
00074   static ros::Subscriber sub_enu = n.subscribe<nav_msgs::Odometry>("enu", 5,
00075       boost::bind(handle_enu, _1, datum, boost::ref(pub_fix)));
00076 }
00077 
00078 int main(int argc, char **argv) {
00079   ros::init(argc, argv, "to_fix");
00080   ros::NodeHandle n;
00081 
00082   // Initially just subscribe to the datum topic. Once we receive that, we'll
00083   // spin up the rest of the node's activities.
00084   ros::Subscriber sub_datum = n.subscribe<sensor_msgs::NavSatFix>("enu_datum", 5,
00085       boost::bind(handle_datum, _1, boost::ref(n)));
00086 
00087   ros::spin();
00088   return 0;
00089 }
00090 


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