【學習筆記】按標準格式將/odom數據保存爲txt文件

上一篇的姊妹篇，還是實現通過節點訂閱/odom的話題消息，將該消息按照ROS官方的標準格式保存爲本地的txt文件。

1.標準文件格式

ROS官方定義的/odom的數據格式如下：

header: 
  seq: 172551
  stamp: 
    secs: 1555296552
    nsecs: 823127003
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 42.3783798218
      y: 25.7542228699
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.0346746705472
      w: 0.999398648739
  covariance: [0.08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [100.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0]

2.實現代碼

#include "ros/ros.h"
#include <iostream>
#include <string>
#include <fstream> 
#include <nav_msgs/Odometry.h>

using namespace std;
string txt_path="/home/kanghao/learning_something/learning_files/0621/odom_data/odom";
int j = 0;
string filetype=".txt";
string path;

void odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
    boost::array<double, 36ul> pconv=msg->pose.covariance;
    boost::array<double, 36ul> tconv=msg->twist.covariance;
    cout << msg->header.stamp << " ";
    cout << msg->header.frame_id << " ";
    cout << msg->child_frame_id << endl;
    cout << msg->pose.pose.position << endl;
    cout << msg->pose.pose.orientation << endl;
    cout << msg->twist.twist.linear << endl;
    cout << msg->twist.twist.angular << endl; 
    stringstream ss;
    ss << j;
    string num = ss.str();
    path = txt_path+num+filetype;
    cout << path << endl;
    j++;
    cout << pconv[1] << endl;


    ofstream OutFile(path.c_str());
    OutFile << "headr:" << endl;
    OutFile << "  stamp: " << msg->header.stamp << endl;
    OutFile << "  frame_id: " <<msg->header.frame_id << endl;
    OutFile << "child_frame_id: " << msg->child_frame_id << endl;
    OutFile << "pose:" << endl;
    OutFile << "  pose:" << endl;
    OutFile << "    position:" << endl;
    OutFile << "      x:" << setprecision(12) << msg->pose.pose.position.x << endl;
    OutFile << "      y:" << msg->pose.pose.position.y << endl;
    OutFile << "      z:" << msg->pose.pose.position.z << endl;
    OutFile << "    orientation:" << endl;
    OutFile << "      x:" << msg->pose.pose.orientation.x << endl;
    OutFile << "      y:" << msg->pose.pose.orientation.y << endl;
    OutFile << "      z:" << msg->pose.pose.orientation.z << endl;
    OutFile << "      w:" << msg->pose.pose.orientation.w << endl;
    OutFile << "  covariance:[" << "";
    for(int i=0;i<pconv.size();i++)
    {
        if(i < pconv.size()-1)
        {
            OutFile << pconv[i] << ",";
        }
        else
        {
            OutFile << pconv[i] << "]" << endl;
        }
    }
    OutFile << "twist:" << endl;
    OutFile << "  twist:" << endl;
    OutFile << "    linear:" << endl;
    OutFile << "      x:" << msg->twist.twist.linear.x << endl;
    OutFile << "      y:" << msg->twist.twist.linear.y << endl;
    OutFile << "      z:" << msg->twist.twist.linear.z << endl;
    OutFile << "    angular:" << endl;
    OutFile << "      x:" << msg->twist.twist.angular.x << endl;
    OutFile << "      y:" << msg->twist.twist.angular.y << endl;
    OutFile << "      z:" << msg->twist.twist.angular.z << endl;
    OutFile << "  covariance:[" << "";
    for(int i=0;i<tconv.size();i++)
    {
        if(i < tconv.size()-1)
        {
            OutFile << tconv[i] << ",";
        }
        else
        {
            OutFile << tconv[i] << "]" << endl;
        }
    }
    OutFile.close();

}

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

    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("/odom", 100, odomCallback);

    ros::spin();

    return 0;
}

通過上述代碼，我們得到的其中一個文件內容如下：

headr:
  stamp: 1555296552.642989295
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x:42.3783798218
      y:25.7542228699
      z:0
    orientation:
      x:0
      y:0
      z:-0.0346746705472
      w:0.999398648739
  covariance:[0.08,0,0,0,0,0,0,0.08,0,0,0,0,0,0,0.08,0,0,0,0,0,0,0.0001,0,0,0,0,0,0,0.0001,0,0,0,0,0,0,0.0001]
twist:
  twist:
    linear:
      x:0
      y:0
      z:0
    angular:
      x:0
      y:0
      z:0
  covariance:[100,0,0,0,0,0,0,100,0,0,0,0,0,0,100,0,0,0,0,0,0,100,0,0,0,0,0,0,100,0,0,0,0,0,0,100]

3.注意點

在寫這個節點時遇到了如下幾個問題：

（1）如何輸出協方差矩陣（covariance）？

照貓畫虎按照激光雷達矩陣解析方式，定義了vactor，但是編譯時被告知不匹配。

通過查閱定義，發現在odom中，協方差矩陣的定義是一個不定長的boost array，於是使用如下語句輸出：

boost::array<double, 36ul> pconv=msg->pose.covariance;

（2）爲什麼我的odom有719條消息，最終保存文件僅有450個？？

在通過rosbag info查詢我的bag文件時，我發現我的odom消息共計719條，然而保存的txt文件僅有450個，爲什麼呢？後來發現是緩存隊列設置的太小（初始設置爲1），這樣來不及處理的數據就被丟棄了。於是將緩存隊列設爲100，所有的數據均被保存：

ros::Subscriber sub = n.subscribe("/odom", 100, odomCallback);