撰寫自己的節點

在上一篇如何使用節點中,我們討論如何啟動節點以及幾個跟節點相關的有用工具。這一篇,我們便來探討如何撰寫自己的節點。在 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 節點的概念與意義這篇中,釐清其中的概念。

如何使用節點

繼前一篇 ROS Node 節點概念與意義 之後,我們聊解了結點的概念,那要如何使用呢?

如何啟動節點?

在啟動節點之前,請先啟動管理器,也就是 master,在終端機打:

$ roscore

啟動 Master 後,再來才能啟動所有節點。

$ rosrun [程式包名稱] [節點名稱]

例如;

$ rosrun turtlebotsim turtlebotsim_node

附帶一提,當使用 roslaunch 批次啟動節點時,由於 roslaunch 會自動啟動 master,所以便不必在啟動之前輸入 roscore。

rosrun 本身的設計是可以讓使用者直接修改節點內的參數,就只需用引數的方式加在節點後面。例如:

$ rospy_tutorials talker _param:=1.0

就像結尾那個 _para:=1.0 一樣,注意,給值是用":=",而不是"="或"=="。

參數伺服器(Parameter Server)

為了瞭解節點後面的參數是怎麼一回事,我們可以先簡介一下參數伺服器(更深入的探討可以參考這裡)。參數伺服器是一個儲存所有參數的字典大倉庫,其中的參數值可以在執行時間以 ROS 的 API 直接存取。當我們在節點內定義參數時,其中的參數以及包含的資料,就會被記錄在參數伺服器內。

其他特殊字元

像剛才的 _param,其實這些參數是我們在寫節點程序的時候,自己定義的,所以只要知道節點裡面有涉及哪些參數,便可以直接在諸端機後面直接當場指定,只是這只是暫時的,下次啟動節點,一樣會使用預設值。還有,以下的名稱為系統內定,我們在指定參數時,不可以使用。

__name

這個特殊保留字元讓使用者可以重新位節點取名。必須要啟動至少一個節點的情況下,才能使用。

__log

這個特殊字元讓使用者可以指定錯誤訊息(log)的謝入位置,一般不建議使用,因為錯誤訊息通常是給其他工具參照的。

__ip, __hostname

這兩者代表著系統參數 ROS_IP 和 ROS_HOSTNAME一般不建議使用,除非在系統無法設定系統參數的特殊狀況下。

__master

這個特殊字元代表 ROS_MASTER_URI一般不建議使用,除非無法設定系統參數。

__ns

這個特殊字元代表  ROS_NAMESPACE一般不建議使用,除非無法設定系統參數。

將引數 Remap

結尾的引述,也可以用來 Remap。其實這個功能是真的很實用。語法就是 name:= target_name 。例如今天我們要改變雷射測距儀發佈出來的話題名稱,我們可以像這樣做:

$ rosrun hokuyo_node hokuyo_node /scan:=/new_scan

這樣,就可以把原先的話題 /scan 改成 /new_scan 了。

其他好用的工具

rosnode

rosnode 是一個指令工具,用來顯示所有節點的資料,例如顯示目前執行中的所有節點。其支援的指令包括:

rosnode info [節點]

顯示節點的詳細資料。

rosnode kill [節點]

將一個節點強制關閉。

rosnode list

條列所有執行中的節點。

rosnode machine [hostname]

將某機台上的所有執行中的節點條列出來。

rosnode ping [節點]

測試節點連線。有點像終端機測試電腦連線的 ping 指令。

rosnode cleanup

將殭屍節點的註冊資訊清除。

注意:這只是暫時性的解法,一般操作時不建議使用。這個功能可能會誤砍正在執行中的節點。

rqt

rqt 是一個方便開發上除錯、執行時監控、動態調整參數等等功能的,基於 Qt寫成的 GUI 工具,有點像是電機工程師相當仰賴的示波器和三用電表。詳細的參考資料可以參考 rqt – ROS Wiki

ros_gui.png

rqt_graph

rqt_graph 是 rqt 中的 GUI 插件,用來直接觀看各節點的運作狀態。更詳細的資料可以參考 rqt_graph – ROS Wiki

snap_rqt_graph_moveit_demo

rqt_reconfigure

rqt_reconfigure 接替了 dynamic_reconfigure (reconfigure_gui),提供可以及時動態監控與調整dynamic_reconfigure 所能偵測到的參數的功能。詳細資料可以參考 rqt_reconfigure – ROS Wiki

使用的方法如下:

$ rosrun rqt_reconfigure rqt_reconfigure

reconfigure_gui3

Launch 檔中啟動節點的方法

啟動節點相當簡單,在 launch 檔中,使用 <node>標籤即可。roslaunch 無法確保節點是否會按照使用者寫的順序依依開啟,不過就筆者經驗,launch 會逐步將各節點啟動,所以有些需要時間啟動的節點,個人的經驗,是將其寫在 launch 檔的後面。在某些情況下,由於某類節點會需要接收到輸入,或需要花較久時間啟動,為了因應這種狀況,也會將這類節點放在後面啟動。啟動節點的語法如下

<node pkg="foo" type="foo" name="foo" [屬性1] [屬性2] … [屬性n] />

上面<node>標籤裡面可以加上其他屬性。屬性有以下:

pkg=””

 

程式包名稱

type=””

 

節點類型,必須對應到一個存在,有相同名稱的節點

name=””

 

節點名稱,跟 __name相似,但只要這個launch檔啟動,這個節點就會被這個使用者自訂的名稱稱之。注意,不要包含命名空間,若要加上命名空間,請使用
ns 屬性。

args=”arg1 arg2 arg3”

非必要

輸入節點所需的引數。通常會常使用在
nodelet
的啟動上。

machine=””

非必要

指定啟動某一機台上的節點,請參考
<machine>
,使用方法可以參考這篇

respawn=”true”

非必要

當節點關閉時,自動再啟動它。

respawn_delay=”30” (預設為0)

非必要

如果 respawn=true,系統會等待指定秒數,才會讓關閉的節點再度啟動。

required=”true”

非必要

如果節點關閉,則關閉整個  launch 檔。

ns=””

非必要

指定節點的命名空間。

clear_params=”true|false”

非必要

在啟動之前,刪除節點私有空間中的所有參數

output=”log|screen”

非必要

如果選擇 screen,來自節點的stdout/stderr(輸出資訊/錯誤訊息)會顯示於終端機上,若為 log,則除了會被寫入 $ROS_HOME/log 內,也會顯示於終端機上。預設是 log

cwd=”ROS_HOME|node”

非必要

若引數值為 node,節點的工作路徑會被設為節點實際執行檔所在的路徑上。在
C-Turtle版本之後,預設值皆為ROS_HOME

Launch-prefix=”prefix arguments”

非必要

這個指令會將結點在 gdbvalgrind
xterm
nice等工具來在執行時間內除錯。更詳細的使用方法,請參考
Roslaunch Nodes in Valgrind or GDB

在<node>標籤中,你可以加上元素搭配使用。實例中會將命令指令與 launch 檔裡的語法做對應。元素的語法如下:

<node pkg=”” type=”” name=””>
    <element1/>

    <element2/>

     ….

    <element n />

</node>

可以用的元素如以下:

<env>

為節點設定環境變數。只有在這個元素後宣告的節點會起作用。

<remap>

為節點 remap 其引數。

<rosparam>

rosparam  檔載入該節點的 ~/local  命名框間中。簡言之,就是載入系統參數進節點中。

<param>

將參數載入節點的 ~/local 命名框間中。簡言之,就是將參數載入到節點內。

所以,如果終端機指令為:

$ rosrun my_pkg my_node __ns:=myns __name:=/new_node __param:=1.0 /old_topic:=/new_topic

對應到 launch 檔,則可以寫成:

 <node pkg="my_pkg" type="my_node" name="new_node" ns="myms" output="screen">

      <param name="param" value="1.0″ />

      <remap from="old_topic" to="new_topic" />

</node>

由於 launch 檔內直接載明了所有參數的設定值,所以在常使用某節點做測試,或執行的情況下,比每次都要打指令來得較為實際。

以上就是 ROS 中節點的使用方式。接下來,我們要探討如何撰寫節點。

ROS Node 節點的概念與意義

如果要理解節點的意義,需要從 ROS 的網狀架構說起。為什麼 ROS 是網狀架構呢?因為最初需要一整個軟體架構,來解決「抓取東西」的問題。這裡面就涉及如何擷取雷射掃描的資訊、馬達的控制、手臂每個環節的定位等等,如果每一個功能都由一個程序負擔,則會構成一個類似圖一的網狀行結構。

%e6%93%b7%e5%8f%96%e9%81%b8%e5%8f%96%e5%8d%80%e5%9f%9f_023
圖一  抓取東西的整個軟體架構

其實這樣的架構並非 ROS 首創,如果您學過網路架構相關知識,可以看得出來這樣的網路其實就是 Real-Time Publisher-Subscriber Protocol (RTPS),這個網狀圖或 Graph中,每個程序就是一個節點(Node),它們之間傳遞的訊息,就由邊(Edge)連起來。其中的節點為 POSIX 程序,邊則以 TCP連接。這樣的聯繫架構實作了平行運算,將功能與原始碼獨立開來,讓系統便得更簡潔,且有更大的容錯度。一個節點出問題掛了,只會自己關閉而不會連累到其他程序,錯誤的訊息會被寫進 logger 當中,供之後除錯時參考(圖二)。但其實更重要的特點,是可以極少依賴或根本不需要黏膠代碼(Software Glue)來組建一個複雜的系統,換個角度想,可以將不同語言寫成的節點串連起來構建出一個完整的系統。

b
圖二  節點之間除了溝通,也與 logger 連接,一單發生錯誤,錯誤訊息便會寫進 logger 裡。

所以,我們可以利用這樣彈性的網路,在執行時,將一個子網路接上另外一個子網路,或者即時將一個子網路替換成另外一個。這就是為什麼我們可以將真實機器人上使用的程式全套搬到模擬器上模擬,或者當場替換一個手臂抓取的功能等等。

那我們又要怎麼確定將不同節點,甚至不同機台上的節點接起來的時候,彼此可以找得到彼此呢?這就必須要藉由 roscore 來紀錄大家的存在,讓所有節點都可以看得見彼此。

a
圖三  節點管理器負責註冊所有節點,讓彼此能溝通。

想看一下實際的狀況嗎?我曾經開發一個用 RGB-D 相機辨識人體,並將辨識到的人過濾到的專案,發揮的功能,有點像是一個隱形斗篷的概念,就稱這個專案為「影行斗篷專案」吧!所有的節點串起來,就像圖五那樣。

figure001
圖四  節點分成兩種:發布者(Publisher)和訂閱者(Subscriber),連接節點的邊則為訊息,訊息會張貼在話題這個容器中,讓不同的訂閱者取用。

架構講完了,概括一下。ROS中有幾個基本的元素:

  • 節點(Node):一個節點極為一個執行序,收進來訊息,也傳出訊息,用這種方式,節點跟節點之間便可以溝通。節點也可以提供或使用某種服務。
  • 消息(Message):消息是一種 ROS 數據類型,用於訂閱或發布到一個話題。ROS 有預設的樣板,使用者也可以自訂樣板。
  • 話題(Topic):節點可以發布消息到話題,也可以訂閱話題以接收消息。
  • 節點管理器(Master):ROS 名稱服務
  • rosout:ROS中相當於 stdout/stderr。
  • roscore:主機+rosout+參數服務器 (parameter server)

 

目前 ROS 支援的語言,或者筆者看過的有以下幾種:

  • roscpp,也就是用 C++撰寫。
  • rospy,使用 Python 撰寫。
  • rosjava,使用 JAVA 撰寫,對應連接 Android 等智慧裝置開發的需求。
  • ROS support from Robotics Sytem Toolbox ,將 MATLAB 結合 ROS 的方案。安裝方式請參考這裡
  • RobotOS.jl,對於使用日漸廣泛的 Julia 語言,也提供了介面

 

 

10708648_10153865053736842_1265985979016866474_o
圖五  啟動「隱形斗篷專案」中所有節點後的網狀圖,其中一行一行毛毛蟲,就是整個系統的中一個一個節點。

我們會在之後的篇章中,討論如何使用節點,以及更重要的,如何撰寫節點。

 

 

 

 

 

讓機器人平滑移動以及增加應用彈性的方法

有沒有一個經驗,為什麼我做的機器人移動上永遠無法像 PR2 或者吸塵器機器人一樣平滑?為什麼我的機器人每次移動都好像一瞬間花了洪荒之力,而機身就是被不情願的往前脫去,這樣久了不只對馬達是種損害,對電力系統也宣判了慢性癌症,總有一天會讓電力系統中某個元件壞掉(如果你的電力系統跟我設計的一樣永遠都不完美)。

 

因此,我們需要為自己的導航系統加點控制系統,讓機器人移動上不會這麼魯莽。我們使用 Yujin robot (他們的 GitHub) 開發的 kobuki_safety_controllerkobuki_velocity_smoother 。其實這兩個 packages 是包在yujin_yocs 這個 stack 中。詳細的官方資訊可以參考這裡。有一點稍微提下,就是這些 Packages 裡面裝的,並不是節點,而是 nodelets (不知道怎麼翻XDD)。但這之後我們會再聊到,本篇先從略。我們來看一下整個驅動控制架構:

si_Ih7qGws9qsRJwX5xTb_g
圖一:Kobuki 整體控制架構

Source: kobukiTutorialsKobuki’s Control System

 

 

 

由圖一中可以觀察到,Kobuki 這台吸塵器機器人(加上上面的架子後就進化成 Turtlebot了,看倌可以花三萬塊台幣去收服它!)所採用的,就是這一套馬達驅動控制架構。yocs_safety_controller 其實由三個程式組成:kobuki_node 、cmd_vel_mux、kobuki:_safety_controller。其中,kobuki_node 其實就是 kobuki 吸塵器機器人的控制驅動節點。cmd_vel_mux 是一種多進單出的速度切換器。kobuki_safety_controller 則是檢查機器人是否碰撞到障礙物、輪子脫落、有沒有跑到懸崖上的檢查機制。

 

啟動速度切換器!

 

所以,我們可以利用這三個節點中的 cmd_vel_mux 來作切換器,這個節點確認就算有不同的速度指令同時要操控這台機器人,只有一種速度指令會傳送給馬達。它有10個 /cmd_vel 的接口,可以接上十種速度指令,並指定每個速度指令的優先性先後順序,由最優先的10到最後一位的第0位。預設是讓 ROS Nav stack 的優先性設在最後一位,第零位,而讓 safety controller 輸出的速度指令設在第十位,使用者遙控輸出的指令排在前述兩者之間。這樣子,多進單出的 cmd_vel_mux 切換器就完善了。另外,這個切換器還會輸出一個總輸出速度 /cmd_vre_mux/output 作為閉路回饋控制迴圈的輸入依據。

這樣講有點抽象,讓我舉個例子:

優先性:

10 . (highest priority) safety controller

  1.   keyboard teleop
  2.   joystick teleop
  3.   android teleop
  4.  (Lowest priority) nav cmd vel

 

以這個方式,我們一次接上了5個速度指令,而一次只有一個總輸出給馬達控制器。但在輸出到馬達之前,我們要在加上一個控制器。

 

那要如何實做呢?既然我們都有節點了,那麼現在要擔心的,是怎麼用 Launch file 來開啟每個節點以及輸出的 topics。其實不只是要學會用 launch,具體的速度指令優先順序,必須另外寫個社檔宣告,然後匯入到 launch 檔。範例如下:

 

<launch>
 <arg name="nodelet_manager_name" default="nodelet_manager" />
 <!–Velocity Smoother’s settings–>  <arg name="smoother_node_name"    default="velocity_smoother" />

 <arg name="config_file"           default="$(find mybot_bringup)/launch/velocity_smoother_param.yaml"/>

 <arg name="raw_cmd_vel_topic"     default="cmd_vel_mux/output"/>                <!–subscribed topic –>

 <arg name="smooth_cmd_vel_topic"  default="/smooth_vel"/>                          <!–publish topic–>

 <arg name="robot_cmd_vel_topic"   default="cmd_vel_mux/output"/>               <!–subscribed topic–>

 <arg name="odom_topic"            default="odom"/>                                             <!–subscribed topic–>
 <!– ****** Nodelet manager ******** –>

 <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" />
 <!– ***** cmd_vel_mux ************* –>

 <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg nodelet_manager_name)">

    <param name="yaml_cfg_file" value="$(find mybot_bringup)/launch/cmd_vel_mux_minimal_with_safety.yaml" />

 </node>

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>
   <!– velocity commands I/O –>

   <remap from="$(arg smoother_node_name)/raw_cmd_vel"        to="$(arg raw_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/smooth_cmd_vel"  to="$(arg smooth_cmd_vel_topic)"/>
   <!– Robot velocity feedbacks –>

   <remap from="$(arg smoother_node_name)/robot_cmd_vel"  to="$(arg robot_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/odometry"          to="$(arg odom_topic)"/>

 </node>

</launch>

 

接著我們來看一下設定檔,範例如下,:

 

cmd_vel_mux_with_safety_priority.yaml

subscribers:

 – name:        “Default input"

   topic:       “def_cmd_vel"

   timeout:     0.1

   priority:    0

   short_desc:  “Default velocity topic; controllers unaware that we are multiplexing cmd_vel will come here"
 – name:        “Navigation stack"

   topic:       “nav_cmd_vel"

   timeout:     0.5

   priority:    1

   short_desc:  “ROS navigation stack controller"
 – name:        “Safety Controller"

   topic:       “cmd_vel_safety"

   timeout:     0.2

   priority:    10

   short_desc:  “Kobuki’s safety controller"
 – name:        “Keyboard operation"

   topic:       “key_cmd_vel"

   timeout:     0.1

   priority:    9
 – name:        “Remote control"

   topic:       “rem_cmd_vel"

   timeout:     0.1

   priority:    8
 – name:        “Onboard joystick"

   topic:       “joy_cmd_vel"

   timeout:     0.1

   priority:    7
 – name:        “Web application"

   topic:       “web_cmd_vel"

   timeout:     0.3

   priority:    6

 

平滑輸出速率!

 

儘管 ros_control 架構下的控制程式本身就有速度平滑化的特點。但萬一我們使用的平台並非遵守這個架構,或者只想雙倍確認速度控制不會出錯,那麼我們可以使用 Yujin Robot 「出品」的 yocs_velocity_smoother 。方法很簡單,把 cmd_vel_mux 輸出的速度指令直接接到 yocs_velocity_smoother 的入口,這樣子,以目前 Yujin 給的 PID 參數,應該就可以應付您的機器人控制了。

 

從官網的ROS Wiki網頁上可以看得出來,Velocity Smoother Nodelet 的輸入與輸出:

輸入:

~raw_cmd_vel (geometry_msgs/Twist)

~odometry (nav_msgs/Odometry)

~robot_cmd_vel (geometry_msgs/Twist)

 

其中,如果沒有額外付上其他閉路控制設計,那麼 raw_cmd_vel 的輸入 topic 可以和輸入 robot_cmd_vel 的一樣。

 

輸出:

~smooth_cmd_vel (geometry_msgs/Twist):即平滑化後的速度指令,直接給 base_controller 節點。

此外,對於網頁上提及的參數,也可以另外寫成一個設定檔,由 launch 載入,讓其自動存進 parameter server 裡面。範例如下:

 

# Example configuration:

# – velocity limits are around a 10% above the physical limits

# – acceleration limits are just low enough to avoid jerking
# Mandatory parameters

speed_lim_v: 0.8

speed_lim_w: 5.4
accel_lim_v: 0.3

accel_lim_w: 3.5
# Optional parameters

frequency: 20.0

decel_factor: 1.0
# Robot velocity feedback type:

#  0 – none

#  1 – odometry

#  2 – end robot commands

robot_feedback: 2

 

在 launch 檔載入的方式其實已經出現在上述的範例裡面,我們節錄出來:

 

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>

 </node>

 

好了,如果把所有的節點搞定,輸出與輸入或對接的 Topics 都有接對(我還真的自己拿筆出來畫圖!),那麼,現在就可以打開你的機器人,遠端遙控一下!光是只給一個 Twist 都可以發現機器人的移動行為變了!這就表示,你成功了!

 

如何讓一台機台中的 Launch 檔也能啟動其他機台中的節點?

distributed-computing
之前看了林信男的這篇如何用一個 launch 檔啟動期胎台機器人上的節點(程式),看得我也心癢癢的,想說這樣的功能,真是 ROS 界的福音啊!趁著研究告一段落,比較有空的時候,也來試試。以下就是我的使用心得!
在開始之前,請先確定你已經完成了 DSA 無密碼遠端連線的設定,請參考這裡。如果還沒設定或還沒閱讀過,請連過去看。對!就是現在!我在這邊等著(啜飲焦糖瑪奇朵中)。
OK了嗎?好,確定一下你的 ~/.bashrc 裡面已經設定好 ROS Master 地址、你自己的 workspace 位置。都設定好了嗎?那我們繼續。
Launch 檔中有幾款標籤可以自由使用,請參考 這篇官方Launch/XML machine 標籤的用法教學 。雖然裡面有提供 password 選項,但既然我們能不須密碼遠端登入,那洩漏密碼這件事遍布成問題了。
ROS 的 Launch 檔裡,允許使用者針對各機台上的節點做啟動,但不可以遠端啟動另外一個 Launch 檔。此外,當初的設計應該是讓 Master 端用這樣的「至尊 Launch 檔」呼風喚雨,所以如果你是在非 master 的機台上呼叫這樣的 Launch 檔,你只會開啟這台基台上的節點而已。這是使用尚要注意的地方。在撰寫這類至尊 Launch 檔的語法上,我們需要用 <machine>標籤先宣告各機台,然後啟動各節點時,用 <node>裡面的 machine="" 來指定機台。
以下是一個範例檔。有很多標籤,既然是 Optional ,我就不使用了:
 ———————————————————————-
<launch>
<!–
This launch file activates both the robot and client side launches
going across all machines. Before using, please verify that all machines
are connected under the same network.
Make sure to set up DSA or password-less login to avoid compromising
your own password. Better if you can set DSA on all computers.
CAUTION: ONLY LAUNCH THIS FILE AT THE MASTER side, otherwise it won’t work.
–>
<arg name="client_ip" default="charly-MSI.local" />
<arg name="client_name" default="charly" />
  <!–The robot–>
  <machine name="myRobot"
  address="myRobot.local"
  user="bot" />
  <!–The client–>
  <machine name="client_pc"
  address="$(arg client_ip)"
  user="$(arg client_name)" />
  <!–機器人端開啟雷射測距儀–>
  <node machine="myRobot" pkg="hokuyo_node" type="hokuyo_node" name="hokuyo_node">
      <param name="frame_id" type="string" value="hokuyo_link" />
  </node>
  <!–客戶端開啟 Rviz–>
  <node machine="client_pc" name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

——————————————————————

自己使用的心得

雖然期盼已久的遠端單 launch file 開啟節點的夢終於實現了,但是我也碰到另外兩個問題,

問題一

所有的文件都解釋了如何啟動遠端節點的實作方法,但是竟然沒有超出四篇在談論如何遠端啟動對方的 launch 檔,亦言之,無法用 machine 標籤將別台機器的 launch 檔包進來。對我而言,目前我手上的專案根本無法一個節點一個節點置入 machine 標籤,因為一個專案就包括了數十個節點,改完手會斷。因次,我個人的使用經驗是挫折的,目前好像尚未能找到這樣的實作方法,看起來又必須重新回覆到之前遠端到各機台並個別啟動該機的 launch file 模式了,否則根本無法順利運作。

問題二

若你在非 master 節點所在的機台上啟動這個可控遠端的 launch 檔,你會遭遇到一則錯誤的訊息,就是 ROS 找不到 master, 所以無法啟動節點,laumch 就此關閉。這就表示,你只能在 master 的機台端啟動該 laumch 檔。
如果你的 master 剛好就在你現在所使用的電腦上,那就沒問題了,但如果你跟我一樣,master 放在機器人端,那麼你依舊每次還要遠端過去,然後才執行該 launch 檔,用久了自己都會嫌麻煩。那你可能會問,那為什麼不乾脆直接把 master 設在自己的電腦上就好?Well, 這世界上永遠都有特例,例如,當你所使用的機器人是公用的,隨時都會有好幾個人一起連到機器人的電腦上。除非今天你是老闆,否則應該沒有人敢一直請你開電腦,然後才遠端到你的電腦上,只為了啟動他的程式吧?
所以,主要的問題是 master 節點,這也可能是為什麼 ROS 2.0 可以改進的,就是去除必須啟動 master 節點才能做事的奇怪要求。

每次登入都要輸入密碼?用 DSA 實作無密碼遠端登入!

由於最近必須把所有的研究成果整合到一台全新的機器人平台上,所以有機會來嘗試一些之前沒時間嘗試的工具。譬如說只用一個 launch 檔同時開啟多台機器上的各個節點,這樣勢必會讓現在啟動機器人的方法發生革命性的變化,至少應該會更簡單。但在這之前,我想分享一下怎麼設定不需要密碼登入的遠端連線這回事,有這個前置動作,才會有更好的連線控制方法。好,那我們來實作看看!

Step 1  至少先確認能遠端

首先,我們需要確認兩台電腦能彼此遠端。請參考我的另一篇文章
在這邊只提醒各位,機器人端和客戶端個別都需要安裝 openssh-client 和 openssh-robot
$ sudo apt-get install openssh-client
$ sudo apt-get install opensh- server
直白來說,前者是遠端到別人那端時使用的,後者則是讓自己當伺服器或接受別人連到我們這邊。
如果不確定自己有什麼裝了什麼沒裝,可以安裝並使用 aptitude 這個小工具幫忙搜尋(如果你的系統中沒有。直接 apt- get install 它),用 Aptitude 搜尋方法如下:
$ aptitude search openssh*
在這一步快進入尾聲前,請確認自己的 .bashrc 內已經有以下:
export ROS-HOSTNAME=你的 Zeroconf 名稱
export ROS_MASTER_URI=http://[Master 的 Zeroconf 名稱]:11311/
例如我的設定如下:
export ROS_HOSTNAME=charly-MSI.local
export ROS_MASTER_URI=http://beebot.local:11311/
只要輸出的所有程式那行的前面出現 “i", 那麼就代表該程式已經安裝。

Step 2 打造金鑰

在機器人的終端機上打:
$ ssh -keygen -t dsa
這時終端機應該會跳出類似以下的訊息:
Enter file in which to save the key (/home/[機器人 host]/.ssh/id_dsa)
它已經有預設的儲存位置了, 就給把公鑰直接儲存在該路徑上吧!我們之後會需要它, 所以這一步就給它大方 Enter 鍵揍下去(噹噹噹噹!)
終端機會在輸出一行訊息:Enter passphrase:
就輸入密碼!
Enter same passphrase again: 再輸入密碼 Orz
所以目前公鑰存在以下路徑:/home/[我的機器人]/.ssh/id_rsa.pub
還有自己的私鑰, 這個永遠別給別人, 記得喔! /home/[我的機器人]/.ssh/id_rsa

Step 3 給對方公用金鑰

是的。你的家要讓朋友隨時進出,那你就要給朋友你的鑰匙。但這份鑰匙為了門禁驗證用,你只把半個鑰匙給人家,自己保留另外一半,或者用另外一種方式看,有點像你自己這邊設置門鎖。若其他朋友冒充你的朋友拿不知哪來的一半鑰匙開門,那對不起,依舊無法打開你家的門。
所以,再電腦遠端驗證中,你自己持有一把私鑰,給想要連近來的電腦一把公鑰。那要怎麼給呢?首先,先改公鑰(.ssh)執行權限:
$ cd
$ chmod 755 .ssh
複製公鑰到要遠端連進來的電腦上:
$ scp ~/.ssh/id_rsa.pub [客戶端電腦]@[客戶端電腦的 IP 或 Zeroconf]: .ssh/authorized_key
例如:
$ scp ~/.ssh/id_rsa.pub charly@charly-MSI:.ssh/authorized_key
現在,遠端到你的客戶端電腦上,打開終端機後,設定公鑰權限:
$ chmod 600 ~/ssh/authorized_keys
好了!設定完成!

Step 4 最後一次需要密碼的登入,此後一勞永逸

假設你從你的客戶端電腦用 ssh 遠端連線到機器人主機,例如:
$ ssh bot@myRobot.local
系統會問你最後一次輸入對方的密碼,一旦輸入好並登入後,之後便在也不會要求你輸入密碼。你已經可以順利以不輸入密碼的方式,登入對方的電腦。同樣的,若對方要遠端連線登入到你這台電腦,也必須照上述設定步驟,只是這次轉換角色而已。

Step 5 (Optional) 寫一個批次檔讓你每次都能輕鬆登入

科技始於人性。我不想每次都一直打 ssh 指令,有沒有更快的方式,讓登入便得更便捷自動?有的!批次檔現在可以更安全的幫你再不洩漏密碼的強況下,自動遠端登入。先在你中意的路徑下,建立一個文件檔,隨便取什麼名字,副檔名為 .sh,裡面打下遠端登入的指令即可。以下範例是我的遠端批次檔:
#! /bin/sh
ssh bot@myRobot.local
就這麼簡單!

對於 Navigation Stack 的基本了解

呼!從何談起?

Navigation Stack 在 ROS 整個架構中佔相當重要的份量,但是截至目前為止,我個人認為網路上能找到的資料雜亂不堪,官方教學有些部分並沒有寫得太清楚,對於剛要入門的新手而言,反而會起更大的心理壓力,不知從何學起。那我就以我自己的理解盡量寫,希望能幫助各位了解 Navigation Stack,並附上學理部分的講解,希望可以讓大家更加熟悉其徵的道理。

楔子

關於Navigation Stack 的介紹,我建議先看過我的良師益友賴柏任的部落格,他整理得很清楚。此外,如果要以更實際的方向來看,可以參考這位網友的文章。而對我而言,Navigation Stack 是一整組可以讓機器人或自動化載具可以在空間中全穩定的移動的相關程式。這些程式可以大致上分為:

  • 感測器、馬達編碼器、各種感測器的輸入輸出
  • 世界、機器人各關節之座標轉換
  • 即時建圖與定位
  • 分層導航(路徑規劃)
  • 上層傳動機制

別緊張,我們下面會娓娓道來。

overview_tf_small

相信大家對上面這張圖並不陌生。這一大組程式大致上就符合上面五個種類。

感測器、馬達編碼器、各種感測器的輸入輸出

這包括:odometry source, sensor sources

每個感測器都有一個驅動程式負責擷取底層的資料,並轉換成 ROS 格式的 messages,例如在途中可以看到的 senosr_msgs/LaserScan, sensor_msgs/PointCloud, nav_msgs/Odometry。ROS 的特有應用就是,其他節點可以收到這些資料,並做後續的處理。

世界、機器人各關節之座標的轉換

這包括:sensors transform

這跟機器人的定位有著密切的關係。可以參考這邊。機器人內部個關節的定義通常寫在 URDF 內,由最中心的關節 base_link 連接到其他元件,有其相對的位置與幾何關係。此外,馬達的傳動也會關係到 odom 相對於虛擬位於機器人正中心的座標 base_footprint的關係,最後,odometry 座標到 map 座標之間的關係,則會由 SLAM 的節點提供。

目前可能看得一頭霧水,沒關係,我們只要知道,必須有程式隨時關心各元件相對於世界的位置關係。

即時建圖與定位

這包括:amcl, map_server, gmapping

讓機器人能自動定位自己在空間中的位置,對於對於環境的感知,以及規劃到達目的地的路徑都相當重要。大家耳熟能詳的 Simultaneous Localization and Mapping (SLAM)演算法實際上可以直接使用 ROS 中的 gmapping 包寡達成,而裡面的定位,仍然是使用 AMCL實作的 Adaptive Monte Carlo Localization 定位演算法,依照編碼器推斷出 odometry 給出自己相對於周遭空間的關係。

分層導航(路徑規劃)

這包括:global planner, local planner, global costmap, local costmap, recovery_behaviour

ROS 架構採取分層導航的方式,使得機器人的導航更安全。系統一方面會用較低更新率的全域路徑規劃演算法計算出從現在位置到終點位置的路徑,另一方面,會使用更高頻的節點做路徑規劃。move_base 建立在 nav_core 基礎上,把這幾個程式,或節點,包起來,讓使用者只需呼叫一個節點,就可以包辦全部。其中運作的機制,其實就是上圖的架構。此外,move_base 把所得到的地圖轉換成象棋盤一樣,一格一格的 grid map,上面用數值標定障礙物與可通行空間的數值(如果障礙物的數值為1、可通行的空間為0,則整份地圖就成為二元地圖 binary map)(更多學理可以參考這裡)這樣的地圖被稱之為 Occupancy Map,這種格式尚無法拿來做導航,所以會再轉成 cost map 的格式,方能實行路經規劃。

當地圖有了,Global Planer 便根據這張 costmap、座標關係、位置座標作為依據,在有限的時間內規劃出安全的路徑。路徑規劃的演算法則從上個世紀60年代以來第一次提出路徑規劃演算法,便不斷精益求精,隨著經驗的積累,更貼近真實應用的能力。但是通常這樣的路徑規劃並沒有顧及實際機器模型在空間中要怎麼移動,這即是所謂的"The piano moving problem"。想像你搬家時,好幾個搬運工辛苦的將你家的平台式鋼琴左僑右僑避開你家的家具搬到電梯口。在機器人導航中,這牽涉到機器人動力運動學(kinodynamics)。由於這攸關於瞬息萬變在空間中移動,所以區域規劃計算的更新率必須比全域規劃頻率還要高,譬如說全域頻率為 20 Hz,但區域頻率就必須提高到 50 Hz甚至更高。

從官方的文件中可以得知,move_base 的 global planner 預設使用 Dijstra 演算法。大家可能會問,欸?那 A* 呢?可能 Willow Garrage 真的太忙了,所以直到 2013年,David Lu! 才把後者寫成一個 Global Planner Plug-in,方才能供大家使用。

抱歉,八卦說的有點多,請先別擔心。

上層傳動機制

這包括:base_control

當區域規劃已經算出一條區域路徑後,便會輸出一個虛擬層的速度 /cmd_vel (原來的名稱:command velocity)。這個速度再透過 base_control轉成真正對每顆馬達輸出訊號,使馬達轉動,讓機體到達目的地。這不只是輪型機器人而已,也可能是機器手臂上每顆馬達的轉動速度。

 

所以…move_base怎麼用?

一般來說,要用的時候必須要啟動建圖、座標轉換、sensor_transform, sensor_source, move_base 等節點節點。如果要使用不同的規劃演算法,可以把演算法寫成 plug-in 加進 move_base 中啟動,這部分之後會再解說。整體怎麼啟動,也會用實際範例解說。