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="gnc@todo.todo">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編寫第一個發佈器程序

以上全部過程我錄製了一個視頻，在瀏覽文章過程當中若是遇到問題，您能夠查看視頻來看看我是怎麼作的。