utm_odometry_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Translates sensor_msgs/NavSat{Fix,Status} into nav_msgs/Odometry using UTM
00003  */
00004 
00005 #include <ros/ros.h>
00006 #include <message_filters/subscriber.h>
00007 #include <message_filters/time_synchronizer.h>
00008 #include <sensor_msgs/NavSatStatus.h>
00009 #include <sensor_msgs/NavSatFix.h>
00010 #include <gps_common/conversions.h>
00011 #include <nav_msgs/Odometry.h>
00012 
00013 using namespace gps_common;
00014 
00015 static ros::Publisher odom_pub;
00016 std::string frame_id, child_frame_id;
00017 double rot_cov;
00018 
00019 void callback(const sensor_msgs::NavSatFixConstPtr& fix) {
00020   if (fix->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
00021     ROS_INFO("No fix.");
00022     return;
00023   }
00024 
00025   if (fix->header.stamp == ros::Time(0)) {
00026     return;
00027   }
00028 
00029   double northing, easting;
00030   std::string zone;
00031 
00032   LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);
00033 
00034   if (odom_pub) {
00035     nav_msgs::Odometry odom;
00036     odom.header.stamp = fix->header.stamp;
00037 
00038     if (frame_id.empty())
00039       odom.header.frame_id = fix->header.frame_id;
00040     else
00041       odom.header.frame_id = frame_id;
00042 
00043     odom.child_frame_id = child_frame_id;
00044 
00045     odom.pose.pose.position.x = easting;
00046     odom.pose.pose.position.y = northing;
00047     odom.pose.pose.position.z = fix->altitude;
00048     
00049     odom.pose.pose.orientation.x = 0;
00050     odom.pose.pose.orientation.y = 0;
00051     odom.pose.pose.orientation.z = 0;
00052     odom.pose.pose.orientation.w = 1;
00053     
00054     // Use ENU covariance to build XYZRPY covariance
00055     boost::array<double, 36> covariance = {{
00056       fix->position_covariance[0],
00057       fix->position_covariance[1],
00058       fix->position_covariance[2],
00059       0, 0, 0,
00060       fix->position_covariance[3],
00061       fix->position_covariance[4],
00062       fix->position_covariance[5],
00063       0, 0, 0,
00064       fix->position_covariance[6],
00065       fix->position_covariance[7],
00066       fix->position_covariance[8],
00067       0, 0, 0,
00068       0, 0, 0, rot_cov, 0, 0,
00069       0, 0, 0, 0, rot_cov, 0,
00070       0, 0, 0, 0, 0, rot_cov
00071     }};
00072 
00073     odom.pose.covariance = covariance;
00074 
00075     odom_pub.publish(odom);
00076   }
00077 }
00078 
00079 int main (int argc, char **argv) {
00080   ros::init(argc, argv, "utm_odometry_node");
00081   ros::NodeHandle node;
00082   ros::NodeHandle priv_node("~");
00083 
00084   priv_node.param<std::string>("frame_id", frame_id, "");
00085   priv_node.param<std::string>("child_frame_id", child_frame_id, "");
00086   priv_node.param<double>("rot_covariance", rot_cov, 99999.0);
00087 
00088   odom_pub = node.advertise<nav_msgs::Odometry>("odom", 10);
00089 
00090   ros::Subscriber fix_sub = node.subscribe("fix", 10, callback);
00091 
00092   ros::spin();
00093 }
00094 


gps_common
Author(s):
autogenerated on Mon Oct 6 2014 00:16:08