ros GPS信息存储节点

writer：lIcht                      email：lIcht.gzl@gmail.com                 date:  2020.2.9

        为了在上位机上现实小车在谷歌地图上的轨迹，所以需要对GPS的轨迹做记录。写此节点的目的就是接收ros无人小车上的GPS信息，存储成文件以方便上位机读取。因为在家没有条件实际对小车进行测试，所以编写测试节点发送变化的gps信息。

一、存储gps信息节点的编写：

xxxxxxxxxx
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/NavSatFix.h"
#include <fstream>
#include <string>
#include <iostream>
#include <streambuf>
​
using namespace std;
​
double last_lat = 0;
double last_lon = 0;
​
//定义文件输出流
ofstream points;
​
​
​
void callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
  if (msg->latitude != last_lat || msg->longitude != last_lon)
  {
    ROS_INFO("write GPS points lat: %f, long: %f", msg->latitude, msg->longitude);
    //ROS_INFO("write GPS points lat: %f, long: %f", last_lat, last_lon);
    points.open("gps_points.csv", ios::out | ios::app);  // 打开输出的文件，位置在该工作空间下
    // 写入数据，第一列为lat, 第二列为long
    points << msg->latitude << "," << msg->longitude << endl;
    points.close();
​
​
  }
  last_lat = msg->latitude;
  last_lon = msg->longitude;
}
​
​
int main(int argc, char **argv)
{
  //清空上一次的数据
  points.open("gps_points.csv", ios::out | ios::trunc);
  points.close();
​
  ros::init(argc, argv, "gps_line");  //初始化ROS，节点名必须唯一。
  ros::NodeHandle n;  //节点句柄实例化
  ros::Subscriber sub = n.subscribe("fix", 1000, callback); //向话题订阅，一旦发布节点在该话题上发布消息，本节点就会调用callbck函数。
 
  ros::spin();  //程序进入循环，直到ros::ok()返回false，进程结束。
​
  return 0;
}

一、测试节点的编写：

xxxxxxxxxx
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
​
​
double lat = 40.0350682;
double lon = 116.3421781;
​
int main(int argc, char** argv)
{
    // Step 2: Initialization:
    ros::init(argc, argv, "imu_gps");
    ros::NodeHandle n;  
    //定义GPS数据的消息发布
    ros::Publisher GPS_pub = n.advertise<sensor_msgs::NavSatFix>("fix", 20);
    ros::Rate loop_rate(10);  
    while(ros::ok())
    {
​
            //gps消息的赋值和发布
            sensor_msgs::NavSatFix gps_data;
            gps_data.header.stamp = ros::Time::now();
            gps_data.header.frame_id = "base_link";
​
            //gps_data.status="working";
            gps_data.latitude=lat;
            gps_data.longitude=lon;
            gps_data.altitude=40.5;
​
            GPS_pub.publish(gps_data);
    ROS_INFO("GPS points lat: %f, long: %f", gps_data.latitude, gps_data.longitude=lon);
    ++lat;
    ++lon;
​
        ros::spinOnce();  
        loop_rate.sleep();  
    }
​
    return 0;
}
​
​

三、测试结果：

1、正常发送gps主题：

2、gps信息写入节点作用：

3、成功创建csv文件：

4、记录的gps数据：