撰寫自己的節點

Charly Huang ROS自學筆記、、

在上一篇如何使用節點中，我們討論如何啟動節點以及幾個跟節點相關的有用工具。這一篇，我們便來探討如何撰寫自己的節點。在 ROS Node 節點的概念與意義中，我們可以在圖四中大概知道節點可以約略區分成兩種類型，這一篇我們也要詳細探討。

基本概念

節點也可以單純就是一個獨立且悶不吭聲的傢伙，也可以分身為發佈者與訂閱者的角色。其實一個節點可以同時擔當這兩個角色，不過為了學習方便，我們將其拆解開來。此外，節點或者程序內部，本身會設有變數、參數，且都可以藉由其他介面調整其數值。由於節點可以用好幾種語言撰寫，因此我們會以最常見的 C++ 和 Python 作為主要撰寫範例語言。

前置作業

創建工作空間

$ mkdir -p ~/catkin_ws/src

$ cd ~/catkin_ws/src

$ catkin_init_workspace

$ cd ..

$ catkin_make

我們創建了一個名叫 catkin_ws 的工作空間，以及它下面的 src路徑。接著我們移到 src 資料夾內。初始化這個工作空間，系統會將 ROS 相關的函式庫與工具連上來。接著，儘管空作空間中空空如也，我們依舊回到工作空間的最上端路徑編譯，這樣，就會讓工作空間中產生 devel 和 build 資料夾。

創建程式包

$ cd ~/catkin_ws/src

$ catkin_create_pkg my_tutorial std_msgs roscpp rospy

$ roscd && cd ..

$ catkin_make

我們再度進入工作空間的 src 資料夾裡，創建我們的程式包。這個程式包叫做 my_tutorial，所需包含的相依函式庫有 std_msgs, roscpp 和 rospy。接著我們再回到 ~/catkin_ws 路徑下，編譯，這樣就會讓程式包內多了 CMakelists.txt、 package.xml和 src/ 路徑，最後這個路徑，就是放置所有原始碼的地方。

為了創建原始碼的檔案，我們再度切回程式包 src 的路徑上：

$ roscd my_tutorial/src

用自己最喜歡的文字編輯器，為 C++和 Python 各創立一個文件，就稱之為 helloworld.cpp 和 helloworld.py 吧！把文件打開了嗎？那我們開始寫吧！

最基本的原始碼樣板

// 這是 ROS 最基本的 Hello World 程式

// 這個標頭的 include 指定了基本 ROS  類別

# include <ros/ros.h> 

// 主程式進入點

int main (int argc, char **argv)
{
// 初始化 ROS 系統
    ros::init(argc, argv, “hello_ros") ;
// 將這個程式創建成一個 ROS 節點
    ros::NodeHandle nh ;
// 輸出訊息

     ROS_INFO_STREAM(“Hello ROS!") ;
return 0 ;

} // 主程式結束

讓我們拆開來看。

  • ros/ros.h 標頭檔包含了標準 ROS 類別，所以會把這個標頭檔 include 到每個 ROS 程式來。
  • ros::init 函式初始化 ROS client library，在每個程式開頭呼叫一次。最後一個字串是預設的節點名稱。譬如，我們這邊的節點叫 hello_ros。記得，這個名稱可以在指令列或者 launch 檔內更改，詳細用法請參考如何使用節點。
  • ros::NodeHandle 物件是這個程式用來與 ROS 系統互動的主要機制。透過這個執行續指引碼（handle）讓 ROS Master 註冊這個節點。最簡單的方式，是整個程式碼使用單一 NodeHandle。

在系統內部，NodeHandle 維護著一個引用計數，且只有當第一個物件被宣告時才會通報 ROS Master 註冊一個新的節點。同樣地，當所有的 NodeHandles 都被關閉後，節點才會被取消註冊。這個細節會產生兩個影響，第一，您可以一次創建多個 NodeHandles，全部指向同一個節點。在某些狀況下，這樣的架構有其存在的意義。第二，你無法只用一個標準的 roscpp 介面在一個程式中執行多個不同的節點。
  • ROS_INFO_STREAM 是產生 log 訊息的 ROS 系統 API，訊息會同時傳送到幾個地方，包括在終端機上顯示。

編譯我們的第一個節點！

在上述前置作業的編譯後，我們的 CMakelissts.txt 應該已經宣告了所需的相依函式庫，如下：

find_package(catkin REQUIRED COMPONENTS [相依程式庫名稱])

我們再去看看 package.xml 裡，系統也自動幫我們產生了以下相依函式庫的連結：

相依函式庫

相依函式庫

例如

roscpp

roscpp

都具備以上元素後，我們回到 CMakelists.txt 來加上我們要編譯的檔案，目前可以先將以下加到原本文件最後一行之後：

add_executable ([節點名稱] [原始碼檔案] )

target_link_libraries([節點名稱]  ${catkin_LIBRARIES})

其實就跟一般在使用 CMake 指令一樣。第二行指定編譯該節點所需的還是庫，根據 find_package () 中載明的相依函式庫相呼應。

例如，我們的這份範例檔就是：

add_executable (hello_ros helloworld.cpp)

target_link_libraries(hello_ros ${catkin_LIBRARIES})

其實編譯時使用的節點名稱以及程式碼中的預設節點名稱沒有關連性，這邊我們只是為了理解方便，所以名稱統一。

一旦完成以上步驟，到終端機上，編譯！

$ roscd && cd ..

$ catkin_make  

如果沒有任何編譯上的錯誤，我們便已經編譯完成我們的第一個節點！恭喜！可以用 rosrun 試著執行看看。

寫一個發佈者節點（Publisher）

%e6%93%b7%e5%8f%96

#include “ros/ros.h"

#include “std_msgs/String.h"

#include

//程式進入點

int main(int argc, char **argv)

{
ros::init(argc, argv, “talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise(“chatter", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
std_msgs::String msg
std::stringstream ss;
ss << “Hello ROS! I am talking to you!! " << count;
msg.data = ss.str() ;
ROS_INFO(“%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;

} //主程式結束

這是一個基礎的發佈者原始碼（原出處在這裡），這個節點會隨這技術的遞增，一次赴一次的發佈話題，其中訊息的字串內容就是 “Hello ROS! I am talking to you !"。跟前一個節點程式碼不同的地方，在於它多了以下：

  •  多宣告了發佈者物件：  ros::Publisher chatter_pub = n.advertise(“chatter", 1000); 請注意後面如何用 advertise 發佈該資料型態的話題，我們有空再詳加探討。
  • 在 while(ros.ok()) 迴圈內，宣告 ss 物件承接了文字訊息後，再轉換成ROS能接受的資料格式，存入 msg 中，並用 publish 函式發佈出來，也就是： chatter_pub.publish(msg); 至於發佈出來的 topic 叫甚麼？其實就是 n.advertise 引數中的名稱"chatter”。
  • 這邊的 ros.spinOnce () 以及許多其他節點的 ros.spin() 可以從名稱看得出跟重複運轉主程式內的程式碼有關係。前者除了有額外指令，否則從啟動該節點開始，只執行所有程式碼一次，便結束。後者，則是一次又一次的執行主程式，直到系統令它停止，或者使用者按下 Ctrl+ C 令其停止。

寫一個訂閱者節點 (Subscriber )

在了解並時作了發佈者節點後，我們要如何寫一個節點，會訂閱發佈者節點輸出的資訊呢？首先，我們要先創建一個 cpp 檔，我們就直接命名為 subscriber.cpp 吧！然後我們再到 CMakeLists.txt
加個兩行，讓程式能夠被編譯。改好後的 CMakeLists 應該看起來會像是這樣：

cmake_minimum_required(VERSION 2.8.3)

project(subscriber_and_publisher_node)

## Find catkin and any catkin packages

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

## Generate added messages and services

generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package

catkin_package()

## Build talker and listener

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)

target_link_libraries(talker ${catkin_LIBRARIES})

add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)

target_link_libraries(listener ${catkin_LIBRARIES})

add_dependencies(listener beginner_tutorials_generate_messages_cpp)

基本上，當你完成之前的「寫一個發佈者節點（Publisher）」後，應該會有紅字之外的其他指令，而後面這三行紅字，則是我們為了這個小傑的前置動作，請確認已經將這兩行加上了。

一旦準備好了，我們便可以創建一個 subscriber.cpp 檔案。其中的原始碼就像以下：

#include “ros/ros.h"

#include “std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)

{
ROS_INFO(“I heard: [%s]",
msg->data.c_str());

}

int main(int argc, char **argv)

{
ros::init(argc, argv,"listener");
ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe(“chatter", 1000, chatterCallback);
ros::spin();
return 0;

}

是的，就這麼簡單！其實主程式大部分的架構跟發佈者沒有差多少，都是先宣告

 ros::init(argc, argv, “listener");
ros::NodeHandle n;

前者初始了結這個節點，並給定名稱。後者啟動了這個節點的主要物件，這個把柄將負責存取節點的輸出與輸入資訊。接著，再宣告：

ros::spin()

是用來運轉這個節點的所有執行序，直到這個節點接收到 ros::shutdown() 或者使用者按下 Ctrl + C 後，才會關閉。有時候也會看到

ros::spinOnce()

將會於單一時段內，運行所有執行序。注意！這兩種 spinning
都只適用於酖執行序程式當中。如果要寫成平行運算，則必須使用其他方式，也許之後有空我們可以再回來討論，但要更加聊解這方面的實作的話，請參考這個網頁。

編譯並執行

再確認了 CMakeLists.txt、package.xml 和兩個節點的 cpp
檔案都沒問題後，我們便可以打開終端機，移動到工作空間的路徑上，然後編譯：

$ catkin_make

或者，如果想單純編譯我們這個節點包的話，可以使用：

$ catkin_make –pkg
subscriber_and_publisher_test

如果想要查詢如何使用節點，可以參考如何使用節點。

如果還是不清楚我們這篇在幹什麼，可以在 ROS Node 節點的概念與意義這篇中，釐清其中的概念。

假如你已經順利的完成上述步驟，接下來會想著手寫更進階的程式，在撰寫節點的小撇步一篇中，會針對一些可能碰到的狀況，提出解決方案。

 

相關主題文章：

更多精采文章，請回目錄瀏覽！

發表留言