订阅sbg_driver::SbgEkfNav的话题消息转为 sensor_msgs::NavSatFix格式
//*****************************************************************************************************
// 订阅GPS并将其转为 NavSatFix格式
// source /home/nefu/catkin_gap/devel/setup.bash && rosrun point_imu gps_to_Nav
//
//*****************************************************************************************************
#include<ros/ros.h>
#include<string>
#include <stdio.h>
#include "std_msgs/String.h"
#include <sensor_msgs/NavSatFix.h>
#include <sbg_driver/SbgEkfNav.h>
ros::Publisher gps_pub;
using namespace std;
void myCallback(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg)
{
sensor_msgs::NavSatFix gps_data;
gps_data.header.stamp = gpsmsg->header.stamp;
gps_data.header.frame_id = "base_link";
gps_data.latitude=gpsmsg->position.x;
gps_data.longitude=gpsmsg->position.y;
gps_data.altitude=gpsmsg->position.z;
gps_pub.publish(gps_data);
cout<<gps_data<< setprecision(9) <<endl;
}
int main(int argc,char** argv)
{
ros::init (argc, argv, "GPS_txt");
ros::NodeHandle n;
ros::Subscriber sub= n.subscribe<sbg_driver::SbgEkfNav>( "/sbg/ekf_nav", 1,myCallback);
gps_pub= n.advertise<sensor_msgs::NavSatFix>("Nav_raw", 1);//发布gps
ros::spin ();
}