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

（初學C++及ROS，如有錯誤請指出）

學習C++，實現編寫一個ROS的節點，用於訂閱激光雷達發佈的消息，將該消息按照sensor_msg/Laser_scan的標準格式保存爲本地txt文件。

1.標準文件格式示例：

我所說的標準格式如下，是ROS官方定義的laser的數據格式，其中ranges數組保存每個激光點的距離：

header: 
  seq: 7209
  stamp: 
    secs: 1550560992
    nsecs: 702586014
  frame_id: "laser"
angle_min: -2.35619449615
angle_max: 2.35572338104
angle_increment: 0.00581718236208
time_increment: 6.17283949396e-05
scan_time: 0.0666666701436
range_min: 0.0500000007451
range_max: 100.0
ranges: [1.100000023841858, 1.0920000076293945, 2.7850000858306885…………]

2.實現思路：

我的思路是建立一個subscriber，用於訂閱激光雷達的topic，讀取到之後在回調函數中把數據保存的到txt文件中，你說什麼？格式？手擼就完事了……

3.實現代碼：

#include "ros/ros.h"
#include <iostream>
#include <string>
#include <fstream> 
#include "sensor_msgs/LaserScan.h"

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

void laserCallback(const sensor_msgs::LaserScanConstPtr& msg)
{
    vector<float> ranges=msg->ranges;
    cout << msg->header.stamp << " ";
    cout << msg->header.frame_id << " ";
    cout << msg->angle_min << " ";
    cout << msg->angle_max << " ";
    cout << msg->angle_increment << " ";
    cout << msg->time_increment << " ";
    cout << msg->range_min << " ";
    cout << msg->range_max << endl;
    stringstream ss;
    ss << j;
    string num = ss.str();
    path = txt_path+num+filetype;
    cout << path << endl;
    j++;

    ofstream OutFile(path.c_str());
    OutFile << "headr:" << endl;
    OutFile << "  stamp: " << msg->header.stamp << endl;
    OutFile << "  frame_id: " <<msg->header.frame_id << endl;
    OutFile << "angle_min: " << setprecision(12) << msg->angle_min << endl;
    OutFile << "angle_max: " << msg->angle_max << endl;
    OutFile << "angle_increment: " << msg->angle_increment << endl;
    OutFile << "time_increment: " << msg->time_increment << endl;
    OutFile << "range_min: " << msg->range_min << endl;
    OutFile << "range_max: " << msg->range_max << endl;
    OutFile << "ranges: [" << "";
    for(int i=0;i<ranges.size();i++)
    {
        if(i < ranges.size()-1)
        {
            OutFile << ranges[i] << ",";
        }
        else
        {
            OutFile << ranges[i] << "]";
        }
    }
    OutFile.close();

}

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

    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("/sick_tim/laser/scan", 1, laserCallback);

    ros::spin();

    return 0;
}

上述代碼最終會將訂閱到的激光話題（我的是/sick_tim/laser/scan)保存爲txt文件，其中一個文件內容：

headr:
  stamp: 1555296552.624446932
  frame_id: laser_tim
angle_min: -2.35619449615
angle_max: 2.35572338104
angle_increment: 0.00581718236208
time_increment: 6.17283949396e-05
range_min: 0.0500000007451
range_max: 30
ranges: [0,0,0,0,0.0769999995828太長了……]

雖然還是有一點差別，其實重點不是生成一模一樣的文件，而是是可以讀取到需要的數據，比如每個點的距離，又可以輸出。以後可以直接把輸出的數據進行使用。

4.注意點：

這裏記錄幾個自己產生過疑惑的地方，可以跳過了：

(1）如何把文件保存到不同的txt文件中？

爲了實現這個功能因該是使用地址拼接，我最初的想法是按照讀取到的時間戳（stamp）來命名文件，但是不會轉換成string的形式……然後曲線救國，定義了int j，利用int轉string實現地址拼接……這裏記錄一下折騰半天的int轉string，關鍵代碼爲：

int j=0；
stringstream ss;
ss << j;
string num = ss.str();

（2）輸出的小數點精度如何設定？

代碼剛寫好的時候，我的數據包括小數只有六位，精度對比原來的數據損失很多，怎麼解決這個問題？其實只需要在輸出時加一句紅色的語句：

OutFile << "angle_min: " << setprecision(12) << msg->angle_min << endl;

這樣輸出數據爲12位，看到博文說這樣對於完全爲0的數據輸出還是0，可以把上述語句變爲：

OutFile << "angle_min: " << fixed << setprecision(12) << msg->angle_min << endl;

也就是固定12位，然後0會變成0.00000000000，emmm，太誇張了吧，0就0吧，和0.沒差……

另外，經過測試setprecision(12)不需要每一句都添加，只要有一句添加了精度都會跟隨改變。

 

最後大寫加粗的吐槽：C++相比於Python太難受了！！用b站小翔哥的語氣來說：C++，普～通～