前言

球球队友们了，别忽悠我了，我写还不行吗TAT

一、准备工作

ubuntu系统，ros，C++。以提取geometry_msgs/Vector3Stamped类型话题中的时间和x,y为例

二、使用步骤

1.修改launch文件中bag的路径

建议使用绝对路径

<launch>

<!-- bag的地址与名称 -->
<arg name="bag_filename" default="your_bag_path"/>

<!-- 使用bag的时间戳 -->
<param name="use_sim_time" value="true" />

<node name="get_data" pkg="get_data" type="get_data_node" output = "screen"/>

<!-- play bagfile -->
<node name="playbag" pkg="rosbag" type="play" args="--clock $(arg bag_filename)" />

</launch>

2.代码正文

需要修改代码中txt文件的位置，建议绝对路径。为了将提取的数据方便后续处理，提取的时间从零开始，如果有其他想法可以自己改。

#include <geometry_msgs/Vector3Stamped.h>
#include "ros/ros.h"
#include <string>
#include <fstream>
#include <iostream>

using namespace std;

class Get_data
{

public:
    double first_time;
    bool get_first_time = true;
    ros::Subscriber rpy_sub;
    double rpy_time, rpy_x, rpy_y;
    ofstream outfile_time, outfile_x, outfile_y;

    Get_data(string str)
    {
        outfile_time.open("/home/chen/tools_ws/src/get_data/txt/rpy_time.txt");
        outfile_x.open("/home/chen/tools_ws/src/get_data/txt/rpy_x.txt");
        outfile_y.open("/home/chen/tools_ws/src/get_data/txt/rpy_y.txt");
        ros::NodeHandle nh;

        cout << "get data from rosbag !" << endl;
        rpy_sub = nh.subscribe<geometry_msgs::Vector3Stamped>("imu/rpy/filtered", 5, &Get_data::RpyCallback, this);
    }
    void RpyCallback(const geometry_msgs::Vector3StampedConstPtr &rpy_msg)
    {
        if (get_first_time)
        {
            first_time = rpy_msg->header.stamp.toSec();
            get_first_time = false;
        }

        // std::cout << "(roll, pitch, yaw)" << rpy_msg->vector << std::endl;
        rpy_time = rpy_msg->header.stamp.toSec() - first_time;
        rpy_x = rpy_msg->vector.x;
        rpy_y = rpy_msg->vector.y;
        outfile_time << rpy_time << endl;
        outfile_x << rpy_x << endl;
        outfile_y << rpy_y << endl;
    }
    ~Get_data()
    {
        outfile_time.close();
        outfile_x.close();
        outfile_y.close();
        cout << "get data from rosbag finished !" << endl;
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "get_data");

    Get_data get_data("get_data");
    ros::spin();
    return 0;
}

注意，每次执行完及时将提取到的数据复制走，本代码为了方便，下一次执行会先清空上一次的数据再向txt文件里写入。

总结

程序总共分三部，1 打开冰箱门，2 把大象放进去，3 关上冰箱门。

1. 读取rosbag，订阅者订阅该话题
2. 读取到的数据转化成想要的形式，写入txt文件
3. bag文件读取完毕，关闭程序。
   代码比较简单，有更好的方法欢迎指导。