編寫第一個ROS程序-發布器(Publisher)

在上一篇文章中我們安裝好了ROS環境。本篇文章我們將熟悉ROS中的一些概念（Concept），並嘗試使用C++來實現一個發布器(Publisher)和一個訂閱器(Subscriber)。

該文章是個人學習ROS的過程記錄，參考的書是中文版《ROS機器人編程：原理與應用》，英文版為A Systematic Approach to Learning Robot Programming with ROS，該書代碼托管在作者wsnewman的github上，感謝作者的辛苦付出。

該篇文章分為以下幾個部分：

1. ROS概念
2. 在ROS中實現一個發布器
3. 運行你的發布器

1. ROS

我僅僅列舉了幾個本篇文章將會涉及到的概念，這些概念的定義來源於ROS Wiki，有興趣可以進一步深入了解。

1.1 ROS 文件系統

主要是介紹在ROS中的文件組織方式，類似於Python中包(Package)的文件組織形式。

• Packages: ROS包是ROS中程序的主要組織單元，在一個包中可能包含一系列相關的節點（nodes)，ROS依賴庫，配置文件等等。ROS包是你可以編譯及發行的最小單元了，大部分時候你執行編譯操作時便是在編譯包。
• Package.xml: 主要是用來描述包，提供關於包的一些信息，包括包名稱，版本，簡述，版權信息，依賴等等。
• 消息類型：對話題(topic)中的消息進行定義，以便發布器/訂閱器都能正確編碼/解碼字節流。

1.2 ROS概念

• 節點(Nodes): 節點就是可執行文件，該可執行文件可能是你使用roscpp或者rospy創建的。

• Master: 在上一篇文章中我們運行過命令roscore該命令的作用之一就是啟動ROS Master，Master主要是提供名稱注冊與解析，以便每個節點可以通過名稱來找到另外的節點。

• 消息(message)：節點通過消息來完成彼此之間的交流，消息是一個類似於struct的數據結構，其中可能包含幾個字段。舉個例子：在路徑/opt/share/melodic/share/std_msgs/msg(如果你是像我上篇文章中那樣安裝的)中，你可以看到許多的消息定義，打開一個比如說ColorRGBA.msg，內容如下:

  float32 r;
  float32 g;
  float32 b;
  float32 a;
  

• 話題(topic): 話題由話題名(topic name)表示，你可以把它想象為一個郵箱，發布器認為我只需要把消息投放到郵箱里就行了，而訂閱器認為我只需要去這個郵箱拿消息就行了，因為節點交流的本質是傳遞消息，話題只是指明一個雙方約定好的交流路徑。整個過程可以如下的圖來表示

2. 在ROS中實現一個發布器

2.1 創建工作區和包

在編寫ROS程序之前，首先要建議一個ROS工作區，之后的編寫工作都將在該工作區下進行，工作區有一定的格式，你可以選擇在你的home目錄下創建，依次輸入以下幾行命令

mkdir -p ~/catkin_ws/src # 創建工作區
cd ~/catkin_ws/src # 進入工作區
# catkin_create_pkg package_name(包名稱) dependencies（依賴)
catkin_create_pkg my_minimal_node roscpp std_msgs #新建一個ROS包

創建完成以后，我們可以看到在目錄my_minimal_node下多了兩個文件(package.xml ,CMakeLists.txt)和兩個文件夾(src, include)。

其中的package.xml便是之前提到的ROS包配置文件，描述關於包的信息。CMakeLists.txt是用來配置編譯過程，這是本篇文章所主要使用的兩個文件。

2.2 修改package.xml

首先我們來修改package.xml，在編輯器中打開該文件，可以看到文件中包含了大量的注釋，這些注釋都是來指導你該如何書寫該文件。

一般說來我們應該包括<name>，<version>，<description>，<maintanner>，<license>，<author>，<build_depend>，<build_export_depend>， <exec_depend>便可以了，這部分大家可以按照自己的信息進行修改。

修改后的package.xml類似於下面的內容，當然你也可以不修改，對於這一篇內容來說，這無關緊要。

<?xml version="1.0"?>
<package format="2">
  <name>my_minimal_node</name>
  <version>0.1.0</version>
  <description>The my_minimal_node package</description>
  <maintainer email="[e-mail]">gnc</maintainer>
  <license>MIT</license>
  <author email="sharku">Jane Doe</author>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
</package>

2.3 編寫第一個簡單的ROS程序-發布器

進入到目錄~/catkin_ws/src/my_minimal_node/下，在該目錄下的src文件夾創建minimal_publisher.cpp，內容如下，代碼內容也可以在文章最開始給出的github中找到。

#include <ros/ros.h>
#include <std_msgs/Float64.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "minimal_publisher"); // 初始化節點名
    ros::NodeHandle n; // 
    ros::Publisher my_publisher_object = n.advertise<std_msgs::Float64>("topic1", 1); // 創建一個發布器，調用advertise通知ROS Master話題名稱以及話題類型
    //"topic1" 是話題名
    // 參數 "1" 是queue_size，表示緩沖區大小
    
    std_msgs::Float64 input_float; // 創建一個發布器將要使用的消息變量
    // 該消息定義在： /opt/ros/indigo/share/std_msgs
    // 在ROS中發布的消息都應該提前定義，以便訂閱者接收到消息后該如何解讀
	// Float64消息的定義如下，其中包含一個數據字段data：
	// float64 data
	
    input_float.data = 0.0; // 設置數據字段
    
    
    // 程序所要做的工作將在下面的循環里完成
    while (ros::ok()) 
    {
        // 該循環沒有sleep，因此將一直處於運行狀態，不斷消耗CPU資源
        input_float.data = input_float.data + 0.001; //每循環一次+0.01
        my_publisher_object.publish(input_float); // 發布消息到對應的話題
    }
}

代碼中，#include <ros/ros.h>表示包含ROS頭文件，#include <std_msgs/Float64.h>表示包含標准消息類型中的Float64，它的具體類型是float64。

在上述代碼中，在main函數中，首先對ROS進行初始化，調用ros::init定義了節點名稱。然后定義了 ros::NodeHandle該變量用來創建一個發布者，通過調用函數.advertise，表示向Master聲明我要注冊一個話題名為topic1，然后我將會向該話題發布消息，然后返回一個發布器對象。隨后我們便可以調用該發布器的.publish來進行消息的發布。

其余部分的解釋可以看代碼中的注釋。

2.4 修改CMakeLists.txt文件

在編寫了發布器的程序以后，我們還必須在CMakeLists文件中進行聲明，以便讓編譯器知道應該編譯新增的文件。

在CMakeLists文件的最后一行加入下面兩行代碼：

add_executable(my_minimal_publisher src/minimal_publisher.cpp) # 第一個參數是生成后的可執行文件名 第二個參數
# 是源文件路徑名
target_link_libraries(my_minimal_publisher ${catkin_LIBRARIES}) # 鏈接庫

然后關閉文件，打開終端，導航到目錄~/catkin_ws/下，運行命令catkin_make即可編譯我們剛剛創建的文件。

有可能你的輸出結果和我這里顯示的不同，因為我已經編譯過了，所以編譯器沒有執行任何動作。

3. 運行你的發布器

在上面的編譯過程沒有出現問題的情況下，那么你的編譯就成功了，接下來我們來運行我們剛剛編寫的發布器。

3.1 將工作區路徑加入你的終端

類似於$PATH變量，ROS也有自己的路徑變量，用來搜索所有的ROS包，因為到這里我們還沒有將我們的ROS包加入到ROS路徑中去，因此在運行的時候會找不到我們的ROS包。

不過幸運的是，ROS幫我們生成了一個方便的編譯腳本，位於路徑~/catkin_ws/devel/setup.bash下。如果我們直接運行source ~/catkin_ws/devel/setup.bash那么只會在當前的終端下生效，當你重新打開一個終端，又會無效。因此，我們需要將上面的命令加入到啟動腳本中，運行下面的命令完成：

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

之后你的終端在啟動時都將首先運行該命令，然后ROS就可以找到你的程序了。

3.2 啟動發布器

打開終端，運行命令：

roscore # 啟動ROS Master

然后運行：

 rosrun my_minimal_node my_minimal_publisher # 啟動發布器

啟動以后，盡管看起來什么都沒有發生，但是事實上發布器已經在不斷發布消息了。下面我們用ros工具來查看該發布器。

運行命令：

rosnode list # rosnode list 用來列出當前ROS中運行的節點名 

我們可以得到當前ROS系統運行的節點名稱，可以看到我們初始化的節點名稱出現在了該列表中，證明我們的發布器的確在運行中。

然后運行命令：

rostopic list # rostopic list 用來列出當前ROS中所有的話題名

同樣可以看到我們注冊的話題名稱topic1。

然后運行命令：

rostopic  hz /topic1 # rostopic hz 用來檢查話題的發布頻率

可以看到發布平均頻率是3.3kHz，發布頻率比較高，因此造成的問題是長時間占用CPU。如下圖所示：

這一問題在之后我們將使用sleep方法來設置一個合適的發布頻率。

然后運行命令：

rostopic echo -n1 /topic1 # rostopic echo 用來顯示話題的消息 -n1代表只接收一次

可以看出輸出值為192239.12749，因此通過簡單的計算便可以得出程序已經循環了19223912次左右。

視頻

ROS編寫第一個發布器程序

以上所有過程我錄制了一個視頻，在瀏覽文章過程中如果遇到問題，您可以查看視頻來看看我是怎么做的。