将sbg_driver::SbgEkfNav的话题消息转为 sensor_msgs::NavSatFix格式

订阅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 ();
}