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"
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
00050
00051
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
00059
00060
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
00071
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
00083
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