ubuntu下imu数据txt格式转为rosbag

在imu标定中，imu数据必不可少，有时我们并不会直接得到imu的rosbag，比如txt、csv格式，本文详细介绍了txt文件格式转为rosbag。

代码是基于以下链接内容修改的：

https://gitee.com/houyiliang/data2bag.git

1、clone该代码

2、在data2bag-master文件夹下新建文件，用来输出bag包

mkdir output

3、将你的imu数据放入output文件夹下

 4、对代码进行修改，主要包括三部分

（1）data2bag-master/src/data2bag的CMakeLists.txt进行修改，第9行开始添加如下代码：

find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )

（2）data2bag-master/src/data2bag/src的imu_node.cpp进行修改，这里我只需要把imu数据转为bag，所以其他代码不考虑。

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Imu.h>
#include <stdio.h>
#include <time.h>

#include <cmath>
#include <iostream>
#include <string>
#include <vector>

using namespace std;

#define BASE_TIME 1609307878

struct imu_data {
  double sec;
  double gx;
  double gy;
  double gz;
  double ax;
  double ay;
  double az;
  // double mx;
  // double my;
  // double mz;
  // double ox;
  // double oy;
  // double oz;
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "imu_node");
  ros::NodeHandle n("~");

  rosbag::Bag bag;
  
  printf("read dataPath: %s\n", argv[1]);
  string dataPath = argv[1];
  bag.open(dataPath + "/imu_calib.bag", 1U);

  FILE* file;

  // Step 1 record  imu data into kalibr bag
  // Step 1.1 read imu data
  cout << dataPath + "/imu_calib.txt" << endl;
  file = std::fopen((dataPath + "/imu_calib.txt").c_str(), "r");
  if (file == NULL) {
    printf("cannot find file: %s \n", dataPath.c_str());
    ROS_BREAK();
    return 0;
  }
  vector<imu_data> imuList;
  char string_Temp[60];
  double acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z;
  double nanosec;
  // ROS_INFO("reading head ");
  // for (int i = 0; i < 13; i++) {
  //   fscanf(file, "%s", &string_Temp);
  // }
  // ROS_INFO("reading data");
  // for (int i = 0; i < 13; i++) {
  //   fscanf(file, "%s", &string_Temp);
  // }
  while (fscanf(file, "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &nanosec, &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z) != EOF) {
    double sec = nanosec;
    imu_data data;
    data.sec = sec;

    data.gx = gyro_z;
    data.gy = gyro_x;
    data.gz = gyro_y;

    data.ax = acc_z;
    data.ay = acc_x;
    data.az = acc_y;

    //data.mx = m_z;
    //data.my = m_x;
    //data.mz = m_y;
    imuList.push_back(data);
  }
  std::fclose(file);

  // Step 1.2 write imu data to bag
  // imu frequency is 1000Hz,down to 100Hz
  
  for (int i = 1; i < imuList.size(); i += 1) {
    if (ros::ok()) {
      printf("\nprocess imu %d   size:%d\n", (int)i, (int)imuList.size());
      double time = BASE_TIME + imuList[i].sec;
      sensor_msgs::Imu imu_msg;
      imu_msg.header.stamp = ros::Time(time);
      imu_msg.linear_acceleration.x = imuList[i].ax;
      imu_msg.linear_acceleration.y = imuList[i].ay;
      imu_msg.linear_acceleration.z = imuList[i].az;

      imu_msg.angular_velocity.x = imuList[i].gx;
      imu_msg.angular_velocity.y = imuList[i].gy;
      imu_msg.angular_velocity.z = imuList[i].gz;
      bag.write("imu", imu_msg.header.stamp, imu_msg);
    } else {
      break;
    }
  }

注意：我的imu数据没有磁力计，所以注释掉，我的第一列数据是时间，单位s，因此数据类型要改为double，还要注意文件名和bag名。

（3）对数据读取路径作修改，data2bag-master/src/data2bag/launch的startup.launch

<?xml version="1.0"?>
<launch>
    <!--node pkg="data2bag" type="image_node" name="image_node" output="screen" args="rosrun data2bag imu_node /home/heibaoslam/Data/tianhao_2021_02_04/2021_02_04_14_04_10"/-->
    <node pkg="data2bag" type="imu_node" name="imu_node" output="screen" args="/home/ly/data2bag-master/output/"/>
    
</launch>

主要对args进行路径修改。

5、在data2rosbag工作区下编译

catkin_make

source ./devel/setup.bash

roslaunch data2bag startup.launch

6、然后就可以在output文件夹下看到bag包，进行数据处理了。