在上一篇文章中我們安裝好了ROS環(huán)境。本篇文章我們將熟悉ROS中的一些概念（Concept），并嘗試使用C++來(lái)實(shí)現(xiàn)一個(gè)發(fā)布器(Publisher)和一個(gè)訂閱器(Subscriber)。

該文章是個(gè)人學(xué)習(xí)ROS的過(guò)程記錄蜻牢，參考的書是中文版《ROS機(jī)器人編程：原理與應(yīng)用》，英文版為A Systematic Approach to Learning Robot Programming with ROS，該書代碼托管在作者wsnewman的github上载弄，感謝作者的辛苦付出。

該篇文章分為以下幾個(gè)部分：

1. ROS概念
2. 在ROS中實(shí)現(xiàn)一個(gè)發(fā)布器
3. 運(yùn)行你的發(fā)布器

1. ROS

我僅僅列舉了幾個(gè)本篇文章將會(huì)涉及到的概念撵颊，這些概念的定義來(lái)源于ROS Wiki侦锯，有興趣可以進(jìn)一步深入了解。

1.1 ROS 文件系統(tǒng)

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

• Packages: ROS包是ROS中程序的主要組織單元，在一個(gè)包中可能包含一系列相關(guān)的節(jié)點(diǎn)（nodes)，ROS依賴庫(kù)亲桥，配置文件等等洛心。ROS包是你可以編譯及發(fā)行的最小單元了，大部分時(shí)候你執(zhí)行編譯操作時(shí)便是在編譯包题篷。
• Package.xml: 主要是用來(lái)描述包词身，提供關(guān)于包的一些信息，包括包名稱番枚，版本法严，簡(jiǎn)述，版權(quán)信息葫笼，依賴等等深啤。
• 消息類型：對(duì)話題(topic)中的消息進(jìn)行定義，以便發(fā)布器/訂閱器都能正確編碼/解碼字節(jié)流路星。

1.2 ROS概念

• 節(jié)點(diǎn)(Nodes): 節(jié)點(diǎn)就是可執(zhí)行文件溯街，該可執(zhí)行文件可能是你使用roscpp或者rospy創(chuàng)建的。

• Master: 在上一篇文章中我們運(yùn)行過(guò)命令roscore該命令的作用之一就是啟動(dòng)ROS Master洋丐，Master主要是提供名稱注冊(cè)與解析呈昔，以便每個(gè)節(jié)點(diǎn)可以通過(guò)名稱來(lái)找到另外的節(jié)點(diǎn)。

• 消息(message)：節(jié)點(diǎn)通過(guò)消息來(lái)完成彼此之間的交流友绝，消息是一個(gè)類似于struct的數(shù)據(jù)結(jié)構(gòu)堤尾，其中可能包含幾個(gè)字段。舉個(gè)例子：在路徑/opt/share/melodic/share/std_msgs/msg(如果你是像我上篇文章中那樣安裝的)中迁客，你可以看到許多的消息定義哀峻，打開一個(gè)比如說(shuō)ColorRGBA.msg，內(nèi)容如下:

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

• 話題(topic): 話題由話題名(topic name)表示哲泊，你可以把它想象為一個(gè)郵箱剩蟀，發(fā)布器認(rèn)為我只需要把消息投放到郵箱里就行了，而訂閱器認(rèn)為我只需要去這個(gè)郵箱拿消息就行了切威，因?yàn)楣?jié)點(diǎn)交流的本質(zhì)是傳遞消息育特，話題只是指明一個(gè)雙方約定好的交流路徑。整個(gè)過(guò)程可以如下的圖來(lái)表示

  ROS.png

2. 在ROS中實(shí)現(xiàn)一個(gè)發(fā)布器

2.1 創(chuàng)建工作區(qū)和包

在編寫ROS程序之前先朦，首先要建議一個(gè)ROS工作區(qū)缰冤，之后的編寫工作都將在該工作區(qū)下進(jìn)行，工作區(qū)有一定的格式喳魏，你可以選擇在你的home目錄下創(chuàng)建棉浸，依次輸入以下幾行命令

mkdir -p ~/catkin_ws/src # 創(chuàng)建工作區(qū)
cd ~/catkin_ws/src # 進(jìn)入工作區(qū)
# catkin_create_pkg package_name(包名稱) dependencies（依賴)
catkin_create_pkg my_minimal_node roscpp std_msgs #新建一個(gè)ROS包

image.png

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

其中的package.xml便是之前提到的ROS包配置文件迷郑，描述關(guān)于包的信息枝恋。CMakeLists.txt是用來(lái)配置編譯過(guò)程，這是本篇文章所主要使用的兩個(gè)文件嗡害。

2.2 修改package.xml

首先我們來(lái)修改package.xml焚碌，在編輯器中打開該文件，可以看到文件中包含了大量的注釋霸妹，這些注釋都是來(lái)指導(dǎo)你該如何書寫該文件十电。

一般說(shuō)來(lái)我們應(yīng)該包括<name>，<version>叹螟，<description>鹃骂，<maintanner>，<license>罢绽，<author>畏线，<build_depend>有缆，<build_export_depend>棚壁， <exec_depend>便可以了袖外，這部分大家可以按照自己的信息進(jìn)行修改。

修改后的package.xml類似于下面的內(nèi)容鬓照，當(dāng)然你也可以不修改豺裆，對(duì)于這一篇內(nèi)容來(lái)說(shuō)臭猜，這無(wú)關(guān)緊要蔑歌。

<?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 編寫第一個(gè)簡(jiǎn)單的ROS程序-發(fā)布器

進(jìn)入到目錄~/catkin_ws/src/my_minimal_node/下园匹，在該目錄下的src文件夾創(chuàng)建minimal_publisher.cpp，內(nèi)容如下累颂，代碼內(nèi)容也可以在文章最開始給出的github中找到紊馏。

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

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

代碼中此衅，#include <ros/ros.h>表示包含ROS頭文件炕柔，#include <std_msgs/Float64.h>表示包含標(biāo)準(zhǔn)消息類型中的Float64，它的具體類型是float64欢嘿。

在上述代碼中炼蹦，在main函數(shù)中狗热，首先對(duì)ROS進(jìn)行初始化匿刮，調(diào)用ros::init定義了節(jié)點(diǎn)名稱。然后定義了ros::NodeHandle該變量用來(lái)創(chuàng)建一個(gè)發(fā)布者光羞，通過(guò)調(diào)用函數(shù).advertise，表示向Master聲明我要注冊(cè)一個(gè)話題名為topic1萍启，然后我將會(huì)向該話題發(fā)布消息，然后返回一個(gè)發(fā)布器對(duì)象。隨后我們便可以調(diào)用該發(fā)布器的.publish來(lái)進(jìn)行消息的發(fā)布山涡。

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

2.4 修改CMakeLists.txt文件

在編寫了發(fā)布器的程序以后鳞溉，我們還必須在CMakeLists文件中進(jìn)行聲明看政，以便讓編譯器知道應(yīng)該編譯新增的文件。

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

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

然后關(guān)閉文件嚷兔，打開終端，導(dǎo)航到目錄~/catkin_ws/下翩剪，運(yùn)行命令catkin_make即可編譯我們剛剛創(chuàng)建的文件。

image.png

有可能你的輸出結(jié)果和我這里顯示的不同违帆，因?yàn)槲乙呀?jīng)編譯過(guò)了刷后，所以編譯器沒(méi)有執(zhí)行任何動(dòng)作丧裁。

3. 運(yùn)行你的發(fā)布器

在上面的編譯過(guò)程沒(méi)有出現(xiàn)問(wèn)題的情況下，那么你的編譯就成功了，接下來(lái)我們來(lái)運(yùn)行我們剛剛編寫的發(fā)布器哟绊。

3.1 將工作區(qū)路徑加入你的終端

類似于$PATH變量兰迫，ROS也有自己的路徑變量，用來(lái)搜索所有的ROS包，因?yàn)榈竭@里我們還沒(méi)有將我們的ROS包加入到ROS路徑中去棘利，因此在運(yùn)行的時(shí)候會(huì)找不到我們的ROS包。

不過(guò)幸運(yùn)的是，ROS幫我們生成了一個(gè)方便的編譯腳本，位于路徑~/catkin_ws/devel/setup.bash下掌敬。如果我們直接運(yùn)行source ~/catkin_ws/devel/setup.bash那么只會(huì)在當(dāng)前的終端下生效框仔，當(dāng)你重新打開一個(gè)終端，又會(huì)無(wú)效。因此核偿，我們需要將上面的命令加入到啟動(dòng)腳本中，運(yùn)行下面的命令完成：

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

之后你的終端在啟動(dòng)時(shí)都將首先運(yùn)行該命令，然后ROS就可以找到你的程序了。

3.2 啟動(dòng)發(fā)布器

打開終端，運(yùn)行命令：

roscore # 啟動(dòng)ROS Master

roscore.png

然后運(yùn)行：

 rosrun my_minimal_node my_minimal_publisher # 啟動(dòng)發(fā)布器

rosrun.png

啟動(dòng)以后丹拯，盡管看起來(lái)什么都沒(méi)有發(fā)生融求，但是事實(shí)上發(fā)布器已經(jīng)在不斷發(fā)布消息了县昂。下面我們用ros工具來(lái)查看該發(fā)布器莱睁。

運(yùn)行命令：

rosnode list # rosnode list 用來(lái)列出當(dāng)前ROS中運(yùn)行的節(jié)點(diǎn)名 

rosnode.png

我們可以得到當(dāng)前ROS系統(tǒng)運(yùn)行的節(jié)點(diǎn)名稱南吮，可以看到我們初始化的節(jié)點(diǎn)名稱出現(xiàn)在了該列表中碧浊，證明我們的發(fā)布器的確在運(yùn)行中猾骡。

然后運(yùn)行命令：

rostopic list # rostopic list 用來(lái)列出當(dāng)前ROS中所有的話題名

rostopic.png

同樣可以看到我們注冊(cè)的話題名稱topic1捞镰。

然后運(yùn)行命令：

rostopic  hz /topic1 # rostopic hz 用來(lái)檢查話題的發(fā)布頻率

ros-hz.png

可以看到發(fā)布平均頻率是3.3kHz，發(fā)布頻率比較高忽洛，因此造成的問(wèn)題是長(zhǎng)時(shí)間占用CPU欲虚。如下圖所示：

CPU.png

這一問(wèn)題在之后我們將使用sleep方法來(lái)設(shè)置一個(gè)合適的發(fā)布頻率集灌。

然后運(yùn)行命令：

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

rosdata.png

可以看出輸出值為192239.12749，因此通過(guò)簡(jiǎn)單的計(jì)算便可以得出程序已經(jīng)循環(huán)了19223912次左右复哆。

視頻

ROS編寫第一個(gè)發(fā)布器程序

以上所有過(guò)程我錄制了一個(gè)視頻欣喧，在瀏覽文章過(guò)程中如果遇到問(wèn)題，您可以查看視頻來(lái)看看我是怎么做的寂恬。