技術標籤：ros 寫node 儲存資料到txt

終於到了寫程式碼的階段了，哈哈。

上一篇文章我們通過實驗知道了雷達資料的各種性質，但是雷達資料在程式碼裡是如何體現的呢？

本篇文章將通過新建一個ros的包來學習一下如何遍歷雷達資料，以及如何對雷達資料進行處理。

首先說明一下我使用的程式碼環境：

Ubuntu版本: 16.04.01
ROS: kinetic 版本
程式語言: C++
IDE推薦: 目前我使用的是 VS code，其如何配置會在之後的文章中講解

1 ros中鐳射雷達資料的訊息格式

通過在終端中輸入如下命令可以打印出ros中鐳射雷達資料的訊息格式

rosmsg show sensor_msgs/LaserScan
 

結果如下所示

std_msgs/Header header  //　資料的訊息頭  uint32 seq      //　資料的序號  time stamp      //　資料的時間戳  string frame_id    //　資料的座標系float32 angle_min    //　雷達資料的起始角度(最小角度)float32 angle_max    //　雷達資料的終止角度(最大角度)float32 angle_increment  //　雷達資料的角度解析度(角度增量)float32 time_increment  //　雷達資料每個資料點的時間間隔float32 scan_time    //　當前幀資料與下一幀資料的時間間隔float32 range_min    //　雷達資料的最小值float32 range_max    //　雷達資料的最大值float32[] ranges    //　雷達資料每個點對應的在極座標系下的距離值float32[] intensities  //　雷達資料每個點對應的強度值
 

2 新建ros包

知道了雷達資料的資料結構，接下來我們將通過程式碼來更深入的認識下雷達資料。

首先，通過如下命令新建個工作空間，以及一個新的包

mkdir -p ~/catkin_ws/srccd ~/catkin_ws/src/catkin_create_pkg lesson1 roscpp sensor_msgs

由於我們使用c++進行編寫，所以第一個依賴為roscpp；由於雷達資料的資料型別為sensor_msgs/LaserScan，所以第二個依賴為sensor_msgs。

2.1 src/laser_scan_node.cc

首先，在src資料夾下新建個檔案，取名為laser_scan_node.cc 。將如下程式碼複製進去，程式碼的意思已經在註釋中解釋的很清楚了。

#include #include // 宣告一個類class LaserScan{private:    ros::NodeHandle node_handle_;           // ros中的控制代碼    ros::NodeHandle private_node_;          // ros中的私有控制代碼    ros::Subscriber laser_scan_subscriber_; // 宣告一個Subscriberpublic:    LaserScan();    ~LaserScan();    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);};// 建構函式LaserScan::LaserScan() : private_node_("~"){    ROS_INFO_STREAM("LaserScan initial.");    // 將雷達的回撥函式與訂閱的topic進行繫結    laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);}LaserScan::~LaserScan(){}// 回撥函式void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg){    ROS_INFO_STREAM(        "seqence: " << scan_msg->header.seq <<         ", time stamp: " << scan_msg->header.stamp <<         ", frame_id: " << scan_msg->header.frame_id <<         ", angle_min: " << scan_msg->angle_min <<         ", angle_max: " << scan_msg->angle_max <<         ", angle_increment: " << scan_msg->angle_increment <<         ", time_increment: " << scan_msg->time_increment <<         ", scan_time: " << scan_msg->scan_time <<         ", range_min: " << scan_msg->range_min <<         ", range_max: " << scan_msg->range_max <<         ", range size: " << scan_msg->ranges.size() <<         ", intensities size: " << scan_msg->intensities.size());    // 第5個點的歐式座標為    double range = scan_msg->ranges[4];    double angle = scan_msg->angle_min + scan_msg->angle_increment * 4;    double x = range * cos(angle);    double y = range * sin(angle);    ROS_INFO_STREAM(        // 第5個數據點對應的極座標為:         "range = " << range << ", angle = " << angle <<         // 第5個數據點對應的歐式座標為:         ", x = " << x << ", y = " << y    );    // 通過ranges中資料的個數進行雷達資料的遍歷    // for (int i = 0; i < scan_msg->ranges.size(); i++)    // {    // }}int main(int argc, char **argv){    ros::init(argc, argv, "lesson1_laser_scan_node");　// 節點的名字    LaserScan laser_scan;    ros::spin();    // 程式執行到此處時開始進行等待，每次訂閱的訊息到來都會執行一次ScanCallback()    return 0;}

2.2 CMakeLists.txt

由於我們新增了一個.cc檔案，所以我們要在CMakeLists.txt中將這個檔案新增編譯選項，在檔案的底部新增如下內容：

# 為指定的檔案生成可執行檔案add_executable(${PROJECT_NAME}_laser_scan_node src/laser_scan_node.cc)# 為生成的可執行檔案新增依賴add_dependencies(${PROJECT_NAME}_laser_scan_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})# 為生成的可執行檔案新增庫的連結target_link_libraries(${PROJECT_NAME}_laser_scan_node  ${catkin_LIBRARIES})

2.3 package.xml

由於我們是使用的catkin_create_pkg命令生成的包，在生成包的時候它已經將package.xml配置好了，所以我們這裡不需要再去改這個檔案，只將生成的檔案內容列出來。

<?xml version="1.0"?><package format="2">  <name>lesson1name>  <version>0.0.0version>  <description>The lesson1 packagedescription>  <maintainer email="[email protected]">lxmaintainer>  <license>TODOlicense>  <buildtool_depend>catkinbuildtool_depend>  <build_depend>roscppbuild_depend>  <build_depend>sensor_msgsbuild_depend>  <build_export_depend>roscppbuild_export_depend>  <build_export_depend>sensor_msgsbuild_export_depend>  <exec_depend>roscppexec_depend>  <exec_depend>sensor_msgsexec_depend>    <export>  export>package>

2.4 編譯

修改好CMakeLists.txt檔案後，就可以進行程式碼的編譯了，具體指令如下：

cd ~/catkin_wscatkin_make

2.5 執行

不出意外的話應該沒有產生編譯錯誤.

編譯完成後會在 ~/catkin_ws/devel/lib/lesson1 資料夾下 產生了一個叫做 lesson1_laser_scan_node 的可執行檔案。

我們可以通過如下命令進行執行

首先，開一個終端，在終端中輸入 roscore再新建一個終端，在終端中輸入 cd ~/catkin_ws/devel/lib/lesson1 ./lesson1_laser_scan_node

將打印出如下的log:

[ INFO] [1606545572.752075473]: LaserScan initial.

此時是沒有其他訊息打印出來的，這是因為我們的程式碼沒有接收到鐳射雷達的資料訊息，所以我們通過如下命令進行bag的播放，bag資料下載下來後，右鍵單擊進行解壓，並放在 ~/bagfiles 資料夾下。

(本文對應的bag資料可以去我的公眾號: 從零開始搭SLAM中回覆lesson1獲取下載連結，)

再次新開一個終端，輸入如下命令cd ~/bagfilesrosbag play lesson1.bag

這時，在執行 ./lesson1_laser_scan_node 的終端視窗中應該會不停地打印出如下訊息

[ INFO] [1606545575.110606737]: seqence: 4131, time stamp: 1606455444.278184417, frame_id: front_laser_link, angle_min: -3.14159, angle_max: 3.14159, angle_increment: 0.00436332, time_increment: 7.15627e-05, scan_time: 0.102979, range_min: 0.01, range_max: 25, range size: 1440, intensities size: 1440[ INFO] [1606545575.110772238]: range = 2.6, angle = -3.12414, x = -2.5996, y = -0.0453758

2.6 結果分析

從這些資訊中我們可以看出：

• 雷達的座標系為front_laser_link

• 雷達資料的最小角度與最大角度分別為 -3.14159 與 3.14159，可見，這是一個水平視角為360度的雷達

• 雷達資料的最近最遠距離分別為 0.01m 與 25m，可見，這個雷達的盲區為1cm

• 雷達掃描一週將返回1440個數據點。

2.7 說明

2.7.1雷達資料

從雷達資料中我們可以得到雷達資料的最基本訊息，但是這些訊息不一定是真實可靠的，因為這些資料是雷達的驅動包中寫的，有些雷達廠商的程式碼不規範，在發出雷達資料時填的資訊有可能是錯誤的。如，雷達的盲區很少能有1cm這麼小的，一般都10cm以上。

2.7.2 座標轉換

還有，雷達資料中的 ranges 欄位中儲存的只有極座標系下的距離值，如果我們想知道每個資料點對應歐幾里得座標，還需要將極座標進行轉換。

轉換的方法就是 通過索引來獲取 ranges 中的值，再通過索引算出這個值對應的角度

第i個數據點的距離值為 ranges[i]
第i個數據點的角度為 angle = angle_min + angle_increment * i
所以這個點對應的x座標為 ranges[i] * cos(angle)
所以這個點對應的y座標為 ranges[i] * sin(angle)

2.7.3 雷達資料的遍歷

二維鐳射雷達資料的遍歷只有通過 ranges 欄位的個數 進行for 迴圈，並通過索引進行距離值的獲取。

2.8 launch檔案

2.8.1 launch檔案

目前，我們想要啟動這個節點需要開3個終端才能夠啟動，那有沒有更方便的方式進行啟動呢。

當然是有的，ROS中使用launch檔案來實現這個功能。

首先，先將開啟的終端全部關掉，在ubuntu中終止正在執行的東西的命令為Ctrl+C。

# 新建一個launch資料夾mkdir-p~/catkin_ws/src/lesson1/launch

‍

在新建的launch資料夾中新建一個名為 demo.launch 的檔案，並將如下內容填進去。

<launch>        <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>        <param name="use_sim_time" value="true" />        <node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_laser_scan_node" output="screen" />        <node name="playbag" pkg="rosbag" type="play"        args="--clock $(arg bag_filename)" />launch>

接下來只要啟動這個launch檔案，它就會幫我們啟動roscore，想要的節點，以及bag包的播放。

2.8.2 設定環境

由於我們是新建的工作空間，我們首先要執行如下命令將我們新建的包的索引在終端中註冊一下，以便終端能夠找到我們的包。

rospack profilesource ~/catkin_ws/devel/setup.bash

其中source的這條命令，是每次新開一個終端都要重新執行一次的，也可以通過如下命令將其寫入到 .bashrc 中，這樣每次新開終端就不再需要執行source命令了。

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

環境弄好了，接下來我們通過roslaunch來啟動launch檔案

roslaunch lesson1 demo.launch

如果一切順利的話我們可以得到上邊同樣的結果。

3 總結

本篇文章中，我們通過獲取鐳射雷達資料的例子，從零新建了一個ROS中的package。

我們講解了如何通過命令新建包，以及在新建包的時候的依賴如何新增。

之後，我們新建了一個.cc檔案，檔案中的內容就是按照標準ROS的格式書寫的，以後我們的程式都會按照這個模式進行程式碼的編寫。在程式中，我們知道了雷達資料的訊息內容，如何進行極座標與歐式座標的轉換，以及如何對雷達資料進行遍歷。

之後，我們修改了CMakeLists.txt檔案，併成功編譯和執行。

最後，我們講解了launch檔案以及如何設定程式碼的環境。

4 Next

接下來的文章我們不會再這麼細緻的進行環境的講解，我們將更多的關注功能的實現以及程式碼的實現。

下一篇文章我們將要介紹如何對雷達資料進行處理與過濾，如何在雷達資料中識別出人的小腿，並將人的小腿過濾掉。

文章將在公眾號: 從零開始搭SLAM進行同步更新，歡迎大家關注，以在文章更新的第一時間通知您。

同時，也希望您將這個公眾號推薦給您身邊做鐳射SLAM的人們，大家共同進步。

在公眾號中回覆lesson1可以獲取本文中的bag資料的下載連結
在公眾號中回覆開源地址可以獲取本文中程式碼的github地址