我知道我的手臂在哪裡嗎?那我的眼睛呢?

是的,這個問題讓我想到的並不是機器人,而是科學怪人,或者是任何死屍殭屍之類的,剛從土裡面爬出來,然後在地上到處爬,找自己的身體部位裝回自己身上。不過,機器人不也是一塊一塊模組拼起來的嗎?一個完整,有四肢軀幹頭部的機器人,必須知道怎麼操縱各部位協動,才能順利將東西拿起來,並從一點移到另一點,再放下。

機器人各個坐標系的管理

我們人腦經過千萬年的演化,已經可以很輕易且快速的知道身體各部位在哪裡,甚至可以很即時的知道自己相對空間中的位置。但機器人學中,相關的部位在空間中的關係,必須以座標系定位,每一個部位相對其他部位的連結,便可以看作是座標系和座標系之間的距離與角度關係。所以,我們來看一下以下圖片:

TF_範例

請問:

  • 圖中的球體,相對於相機多遠?
  • 相機相對於桌面的關係?
  • 相機以什麼角度面對球體?

這幾個問題,是機器人學家每天都要問的問題之三,上述三個問題,都是某個部位,相對於其他部位在空間中的關係。機器人學中,有個好用的模型來定義,稱作 Denavit-Hartenberg (DH) 模型。簡言之,這個模型把各部位,拆分為連結(link)和關節/軸(joint),並把每個坐標系的起點,放在關節上,關節與關節之間的距離與角度,便是連結的部分,座標採用右手定則,z 軸平行於連結。照這個道理,我們可以把上面那張圖,想像成這樣:

擷取

如此,每個部位,就用一個座標系定義,因此,假設我們知道相機座標相對於橫槓的角度與距離,以及支撐桿的距離、支架底座和桌面左下角的距離,且也知道左下角的座標系,相當於球體的坐標系關係,那這樣,我們可以用下面的關聯法,找出相機和球體之間的關係:

T_obj 相對於 T_cam = T_obj → T1 → T2 → T3 → T_cam

那為什麼我們不直接拿一條尺來量從相機到球體的距離就好了?唉!您不要這麼專業好不好?如果可以,我早就量了,這只是打個比方嘛!不過真實世界中,可以拿 RGBD TOF 攝像機這樣做。但偏偏很多時候,我們沒辦法這麼直接得知兩者之間的關係,所以必須用上述的方法,迂迴得出距離角度。

既然我們都說座標系了,所以位處於哪個座標系就變得相當重要,真是所謂吃這碗看那碗啊(大誤!)。假如換個角度問,支架關節(T3)相對於球體之間的座標關係是?那我們可以用以下轉換求得:

T3 T_obj中的位置 = T3 → T2 → T1 → T_obj

如果 T2 → T_obj 之間的關係也知道,那便可以省掉 T2 → T1 → T_obj 的關係,但假設我們並不知道,也無從求得。再繼續延伸。假設支架底座會動,試問相機與球體之間五秒鐘內的位置變化?讀者應該會想到以我們前面寫的轉換公式,但這時候,我們必須將每個時間內的變化也納入考量,所以,例如,球體在相機座標系的關係,可能就變成

T_obj 相當於 T_cam(t) = T_obj → T1 → T2(t) → T3(t) → T_cam(t)

您是否注意到,有坐標系是靜止的,但有座標系的狀態是會隨著時間改變的。這些座標系由於是位於三次元當中(是的,就跟您的愛人一樣,真實地存在於三次元當中),所以,位置的座標,適用 (x, y, z, roll, pitch, yaw) 這六個變數紀錄,座標轉換上,也都是用矩陣計算。如果每一秒都要靠手動或刻意寫程式計算,那事情就不用做了。所以,ROS 的框架中,已經有一大套相當完善的 TF函式庫,供使用者應用,自動計算及追蹤每個坐標系每個時間點的轉換與位置。

位置與方位

這邊,我們介紹一些觀念。我平時都是用英文字彙,如果以下中文術語有誤,請不吝指教。

  • 姿態(Pose)是一個紀錄卡氏座標的陣列 [x, y, z]^T
  • 方位(Orientation)是一個三圍陣列,裡面的元素分別為x 軸、y軸、z軸上的旋轉角度 [roll, pitch, yaw],其實這個術語,應該是承襲自航空學中的各種轉動角度。下方圖片中,向前為x軸並對應 roll,向右為 y 軸並對應 pitch、向上為 z軸且對應 yaw

Yaw_Axis_Corrected.svg

  • 位置(Position)結合了姿態與方位,變成一個有六個元素的陣列,在機率機器人學中,也稱之為狀態(state),或組態(Configuration)。這六個變數,機器人學中,便稱其為六個自由度。所以,下次,看到有四旋翼的廣告文案上說,這台四旋翼有五個自由度,或者,一個視覺里程計(visual odometry)標榜即時的六個自由度定位,我們便能輕易知道在說什麼。
  • 平移(translation)就是原本位置p(x,y, z)再加上某個定量,也就是說,p+v = [x, y, z]^T + [v1, v2, v3]。這邊,我們僅以p 陣列表示平移量。
  • 旋轉矩陣(rotation matrix,簡言之,就是一個向量大小不變,直接轉向的矩陣。在二維的狀況下,旋轉矩陣為:

Rotation

  • 以z軸為中心的三維旋轉矩陣便寫成

3D_rotation_matrixPNG

  • 轉換矩陣(Transformation Matrix )便結合了旋轉與平移的關係,和寫成一個矩陣,將一個座標中的狀態以另一個坐標系來表示:

transformation matrix

回顧選轉矩陣中,x-y平面上都沒有轉任何角度,二維旋轉矩陣便成為單位矩陣。因此,轉換矩陣便成為:

translation_matrix

還記得上面提到從相機到球體的轉換關係嗎?實際上在作的運算,就是將相機座標的中心點,用上面的轉換矩陣藉由矩陣乘法,以球體坐標系表示。如果要倒過來,求出球體座標中心點,在相機座標中的位置時,便以逆轉換矩陣,作矩陣程法後,得出。這邊,我盡量不寫數學式,因為這篇只是在聊觀念。有興趣看更多實際公式和算法的話,建議讀者可以參考 John Craig所著之Introduction to Robotics,便會有更詳盡的解釋。

 

更多精采文章,請回到目錄瀏覽!

用 Python 撰寫自己的節點

如果要了解如何用 C++ 寫一個簡單的節點的話,可以參考這篇

為了撰寫這篇筆記,我稍微了作了一下功課,發現我手邊的資料,都是教人如何用 C++

寫節點,似乎這樣比較能施展計算的效能,以及與現在的許多主流函式庫接軌。但別忘了,ROS也支援 Python
,而隨著機器學習與深度學習的應用再者幾年來逐漸興起,如

Tensor Flow 等等用 Python



為其主要語言的函式庫也獲得愈來愈多廣泛的採用`我後來更發現,如果還想要加入 Copyleft 版權的圖形化使用者介面(GUI)的話,那
Python 中的 Tkinter 絕對是我的第一且唯一首選。GUI 方面,我會再這篇之後,另外撰寫我的使用心得,而現在,我們只討論幾項基本功。
 


為什麼要用 Python 寫節點?
 


如前所述,Python 的語法簡單,且編譯也是直截了當,又是一個 OOP的語言,很適合程式初學者上手。就我個人經驗而言,Python
有以下優點:
 

  • 儘管身為直譯式語言,速度比不上編譯式語言,如 C/C++ ,但在絕大多數情況,在沒有特別的執行速度下,Python 語法其實是勝任愉快的。 

  • 語法簡單,編譯容易,可以在短時間內打造出簡單的原型軟體(Prototype) 

  • 如前所述,Python 有許多好用的函式庫都以模組化,且都以可直接當場線上下載安裝,使用方便,這樣的資源絕對不要浪費 

  • BSD 版權的 GUI 只有 Python 有,且參考資料眾多,是我目前實作使用者介面的首選。(Qt 為 GPL,一般公司行號會選擇迴避…咦?我說漏嘴了嗎?快畫刪除線~~!) 

  • 在 ROS 中的 Python 節點編譯也超簡單,下面就會看得到。


本篇討論的架構
 

  我們會以 ROS 官方教學的版本為參考,手把手寫出一個有收發功能的節點,之後再加上參數與調整的方法,在最後,我們以實際寫一個簡單的應用收尾。



節點的主幹
 


首先,我們先寫一個 Python 程式的主要架構出來,後面再加上肌肉和內臟。在我們的 Package 路徑下,確認 CMakeLists.txt
裡面的 find_pakage (catkin REQUIRED …) 下有 rospy,再檢查 package.xml
裡,<run_depend>有加入 rospy即可。整個 CMaakeLists.txt 看起來會像這樣:
 

CmakeLists.txt 

cmake_minimum_required(VERSION 2.8.3) 

project(rosnode_tutorial) 


 
find_package(catkin REQUIRED) 


catkin_package() 

 


是的,您沒看錯,既然是直譯式的程式,當然不需要 add_executable 這項。 

package.xml 


<?xml version="1.0″?> 

<package> 


<name>rosnode_tutorial</name>
 


<version>0.1.0</version>
 

<description>A simple exercise to write python
node.</description>
 

<maintainer email="todo@todo.com">Megacephalo</maintainer> 


<license>BSD</license> 

<author>Megacephalo</author> 

<buildtool_depend>catkin</buildtool_depend> 

<run_depend>geometry_msgs</run_depend> 

<run_depend>rospy</run_depend> 

</package> 


  
開一個 scripts/ 的資料夾,在裡面,我們創一個新的檔案,就稱為 rosnode_tutorial.py 好了。 

rosnode_tutorial.py 

#! /usr/bin/env python 

from std_msgs.msg  import String 


def talker(): 

   Print(“")


def main():
 

    try: 

        talker()

    except: rospy.ROSInterruptException:
      
pass 

Main()  # Call main function 

上面的程式指架構了一個主體,目的是確保我們的架構本身並不會出錯。可以看的出來,由於每一句程式碼都沒有 C/C++ 那樣最後加上一個";" ,所以

Python 是一個在語法上對縮排相當挑剔的傢伙。我刻意在 talker() 這個函式下面,只放了一個 print (“")當作 placeholder
,有點像以前的王牌推銷員那樣,把自己的腳卡進家家戶戶的門縫裡,當對方想趕緊把門闔上的時候,就會發現根本沒辦法關起來,而這時推銷員便可以再跟可憐的住戶多推銷幾分鐘。               
 



編譯 


先學會編譯,並不是一件壞事。其實很簡單,首先改這個程式的允許存取權限: 


$ chmod a+x rosnode_tutorial.py 


這時,如果你到這個程式的路徑上,直接執行這個節點的話,如: 


$ ./rosnode_tutorial.py
 


便可以檢查是否有出錯,或執行成功。還沒結束!我們要讓 roscore 連結得到這支程式,因此還是需要 catkin_make 一次,這樣連結才會生效。 


$ catkin_make
 


然後,為了讓 roscore 確實產生連結,這邊有一個小技巧: 


$ rospack profile 


Linked up ! 好,測試一下: 


$ rosrun node_tutorial node_tutorial.py
 

執行,應該會…什麼都看不到,也沒有半點程式執行錯誤時跳出的跡象,這就表示,成功了! 


發布功能
 


好,我們現在加入了 import rospy 以及 talker() 裡面的內臟筋肉後,程式變成了一個 ROS 節點。如下:

rosnode_tutorial.py 

#! /usr/bin/env python 

Import rospy  from std_msgs.msg
import String 


def talker():

rospy.init_node
(‘talker’,
anonymous=True)
    
pub =  
rospy.Publisher(‘chatter’, String, queue_size=10)
 
rate = rospy.Rate(10) # 10hz

while not rospy.is_shutdown():
 
hello_str = “hello world %s" % rospy.get_time()

rospy.loginfo(hello_str)

pub.publish(hello_str)

rate.sleep()
 


def main()

try:
talker()

  
except   rospy.ROSInterruptException:
pass
 


main()   # Call main function
 


橘色的部分,是構成這個 Python 節點的關鍵程式碼。如果有用 C++ 寫過節點的朋友,保證對 


rospy.init_node(‘talker’, anonymous=True)
 


不陌生,顧名思義,就是初始這個節點的辦法。在沒有這行程式碼的情況下,這個節點無法與 roscore 溝通。第一個引述為這個節點的名稱,叫
“talker",一定要是其名稱,不可以包含"/"在前頭。在 ROS 中,每個節點有其獨特的名稱,因此,若有兩個同樣名稱的節點一起被啟動,則後啟動者會踢掉先發者。要避免這種狀況,可以設定成
anonymous = True,讓 rospy 幫我們位同樣的名稱的每一個節點,指定一個單一的名稱。
 


pub =
rospy.Publisher(‘chatter’, String, queue_size=10)
  


第二行也顯而易見,宣告一個 pub 物件發佈一個大小為 10 個字元的字串訊息,名稱為 “chatter"。

這個 pub 物件接著就負責將處理好的訊息,直接發不出來,在這個情況下,是字串變數 hello_str。如下:
 


pub.publish
(hello_str) 



訂閱功能
 


我們已經完成了發佈的功能,接著,我們要讓該節點去訂閱別人的訊息。回到原本的程式碼,讓我們再原本的程式碼上動點手腳:
 

rosnode_tutorial.py 

#! /usr/bin/env python 


Import rospy from std_msgs.msg
import
String 


def talker():

  
rospy.init_node(‘talker’, anonymous=True)    
  
pub = rospy.Publisher(‘chatter’, String,   queue_size=10) 
rospy.Subscriber(“chatter", String, callback)
  
rate = rospy.Rate(10) # 10hz

while not rospy.is_shutdown():

hello_str = “hello world %s" % rospy.get_time()

rospy.loginfo(hello_str)

pub.publish(hello_str)

rate.sleep()


def callback(msg):

rospy.loginfo(rospy.get_caller_id() + “I heard %s", msg.data)
 



def main()
   
try:

talker()

except 
rospy.ROSInterruptException:
pass


main()   # Call main function
 


這種寫法稍微白癡了一點,不過卻也發揮了教學效果。這次,只要注意綠色的那幾行就好。rospy 在這邊,扮演了 C++ 節點中的 NodeHandle 角色,訂閱了一個叫 “chatter" 的 Topic
,那這…這這這..不就自己發出來的訊息自己吃嗎?啊呀!別大驚小怪啊!這世界上無奇不有,就連大象的鼻子從象頭伸出來,為了喝水吃東西,又可以被再度放入象嘴中(這是什麼鬼比喻?),就連蟒蛇(Pit
hon)都可以自己咬自己的尾巴,而因此讓目睹了這個景象的德國有機化學家弗里德里希·奧古斯特·凱庫勒 (Friedrich
August Kekulé) 破解了苯的化學結構。好了,說遠了。就醬!
                                           


rospy.Subscriber(“chatter", String, callback)  

這個"訂閱"函式第一個引數,便是 Topic 的名稱,第二引述,便是該話題的資料型態,最後一個引數,是要將這個 Topic 餵到哪個涵是裡去,通常是一個
callback 函式,如果想多了解這個函式,可以參考我之前寫過的
這篇 


調整變數
 


參數化一些變數的值是寫一支好程式的好習慣,可以增加程式的維護性以及可擴充性。假設我們想讓 Topic
的名稱可以讓使用者任意調整,便不應該讓話題的名稱被寫死。我們將 chatter 這個寫死的 topic 改成一個可以任意變動的參數。我們宣告一個變數:


myTopic
 


然後讓這個變數可以被使用者更改,使用者只要將 “my_topic"這個標籤的值指定值即可,但我們先同時設了一個預設值,叫"chatter": 
 


MyTopic = rospy.get_param (“my_topic", “chatter")
 


然後,套用到我們的節點中,就成為這樣:

rosnode_tutorial.py 

#! /usr/bin/env python 


Import rospy from std_msgs.msg
import String 


def talker():

rospy.init_node
(‘talker’, anonymous=True)
    
# 參數化輸入以及輸出的 Topic

MyTopic = rospy.get_param (“my_topic", “chatter")

pub =
rospy.Publisher
(myTopic, String, queue_size=10) 
rospy.Subscriber(“chatter", String, callback)
  
rate = rospy.Rate(10) # 10hz

while not rospy.is_shutdown():
 
hello_str = “hello world %s" % rospy.get_time()
 

        rospy.loginfo(hello_str)

        pub.publish(hello_str)
rate.sleep()
 



def callback(msg):

rospy.loginfo(rospy.get_caller_id() + “I heard %s", msg.data)
 



def main()
:


try:

talker()

except 
rospy.ROSInterruptException:
pass
 


main()   # Call main function
 


更多關於如何設定、取得參數的方法,可以參考
官方教學網頁 



與 Roslaunch 結合,將變數調整變得有彈性
 


在 C++ 的節點中,也有類似 get_param 這樣的函式,而其實 C++ 以及 Python 寫成的節點,都可以直接從終端機指令中,或者
launch file 裡面作設定。在 launch file 當中,我們現在可以這樣寫,將 myTopic 設定成我們想要的名稱。
 

rosnode_tutorial.launch 

<?xml version="1.0″ ?> 

<launch>
<arg name="my_topic" default="my_chatter" />
 


<node pkg="rosnode_tutorial" type="rosnode_tutorial" name="rosnode_tutorial" >

<param name="my_topic" value="$(arg
my_topic)" />
</node>

</launch>
 

其實。launch file 裡面,可以當場設定 value="" 的值即可,但我們為了未來的可能擴充作準備,因此先用 <arg> 設定參數值,再代入
<param>的方法,讓這個 launch 檔方便未來被其他 launch 檔呼叫時,裡面的參數也方便被修改。這是一個好習慣,請讀者們務必善加利用。
 


實務應用:Teleop Twist
 


我們最後便展示一個可以用鍵盤遠端遙控機器人的節點怎麼寫好了。其中的功能很簡單,便是收到使用者按下什麼鍵,再予以發佈 twist
指令,讓機器人左右倫轉動特定角度,可以想像成我們撥動手中的搖桿,只要一直往前壓著搖桿,每一毫秒搖桿便會讓機器人往前走一小段距離。這邊不細講了,直接看原始碼。喔,順帶一提,這其實是
teleop_twist_keyboard 的程式碼,原始儲存庫在
這邊,不過其實可以直接用
Debian 安裝,所以請踴躍安裝使用。

CMakeLists.txt 

cmake_minimum_required(VERSION 2.8.3) 

project(teleop_twist_keyboard)

find_package(catkin REQUIRED)
catkin_package() 


## Mark executable scripts (Python etc.) for
installation
 


## in contrast to setup.py, you can choose the
destination
 


catkin_install_python(PROGRAMS
teleop_twist_keyboard.py 



DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)


Package.xml 

<?xml version="1.0″?> 

<package> 

<name>teleop_twist_keyboard</name>
<version>0.6.0</version>
 

<description>Generic keyboard teleop for twist robots.</description>

<maintainer email="namniart@gmail.com">Austin Hendrix</maintainer>

<license>BSD</license> 


<author>Graylin Trevor Jay</author>


<buildtool_depend>catkin</buildtool_depend>

<run_depend>geometry_msgs</run_depend>

<run_depend>rospy</run_depend>

</package>

teleop_twist_keyboard.py 

#!/usr/bin/env python 

import roslib; roslib.load_manifest(‘teleop_twist_keyboard’) 

import rospy 

from geometry_msgs.msg import Twist 

import sys, select, termios, tty 


msg = “"" 


Reading from the keyboard  and Publishing to
Twist!

—————————

Moving around:

u    i    o

j    k    l

m    ,    .
 


For Holonomic mode (strafing), hold down the shift
key:

—————————

U    I    O

J    K    L

M    <    >

t : up (+z)

b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%

w/x : increase/decrease only linear speed by 10%

e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

“""

moveBindings = {

‘i’:(1,0,0,0),

‘o’:(1,0,0,-1),

‘j’:(0,0,0,1),

‘l’:(0,0,0,-1),

‘u’:(1,0,0,1),

‘,’:(-1,0,0,0),

‘.’:(-1,0,0,1),

‘m’:(-1,0,0,-1),

‘O’:(1,-1,0,0),

‘I’:(1,0,0,0),

‘J’:(0,1,0,0),

‘L’:(0,-1,0,0),

‘U’:(1,1,0,0),

‘<‘:(-1,0,0,0),

‘>’:(-1,-1,0,0),

‘M’:(-1,1,0,0),

‘t’:(0,0,1,0),

‘b’:(0,0,-1,0),

}
 


speedBindings={

‘q’:(1.1,1.1),

‘z’:(.9,.9),

‘w’:(1.1,1),

‘x’:(.9,1),

‘e’:(1,1.1),

‘c’:(1,.9),

}


def getKey():

tty.setraw(sys.stdin.fileno())

select.select([sys.stdin], [], [], 0)

key = sys.stdin.read(1)

termios.tcsetattr(sys.stdin, termios.TCSADRAIN,
settings)

return key
 
def vels(speed,turn):

return “currently:\tspeed %s\tturn %s " %
(speed,turn)
 

if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)

pub = rospy.Publisher(‘cmd_vel’, Twist, queue_size = 1)
rospy.init_node(‘teleop_twist_keyboard’)

speed = rospy.get_param(“~speed", 0.5)

turn = rospy.get_param(“~turn", 1.0)

x = 0

y = 0

z = 0

th = 0

status = 0


try:

print msg
 
print vels(speed,turn)
 


while(1):
 
key = getKey()
 
if key in moveBindings.keys():
 
x = moveBindings[key][0]
 
y = moveBindings[key][1]
 
z = moveBindings[key][2]
 
th = moveBindings[key][3]
 
elif key in speedBindings.keys():

speed = speed * speedBindings[key][0]

turn = turn * speedBindings[key][1]

    print vels(speed,turn)
if (status == 14):

print msg
 

     status = (status + 1) % 15 
else:

x = 0
 
y = 0
 
z = 0

     th = 0 


if (key == ‘\x03’):

break
 

      

    twist = Twist() 

    twist.linear.x = x*speed; twist.linear.y = y*speed;
twist.linear.z = z*speed;
 


twist.angular.x = 0; twist.angular.y = 0;
twist.angular.z = th*turn
 


pub.publish(twist)
 

    except: 

        print e 


finally:
 

    twist = Twist() 

    twist.linear.x = 0; twist.linear.y = 0;
twist.linear.z = 0
 
twist.angular.x = 0; twist.angular.y = 0;
twist.angular.z = 0
 


pub.publish(twist)

termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

 

撰寫節點的小撇步

這篇是繼撰寫自己的節點這篇而來的,若對如何撰寫ROS的節點還不太清楚,可以去參考。

撰寫節點的小撇步

在撰寫一個節點的時候,有沒有自己問過一些問題,或者是有一點自己的心得?這邊我跟各位讀者分享我的幾個微不足道的觀察,希望多少能給讀者一丁點啟發。

如果要一個節點同時收發多個 Topics 怎麼辦?

ROS 的官方教學畢竟是一個相當簡單的概念,之後的造化,都得靠個人,所以,萬一碰到以下幾個狀況該怎麼辦呢?

接收同一個Topic,但是要同時給多個回呼函式(Callback function),那該怎麼辦?

回乎函式是在上一篇基礎教學中,出現的 Callback function,中文中有兩種稱謂,一個為「回呼函式」,另一個為「回謂函式」,簡單而言,就是一個函式指標,指向你自己寫的一個函式,當有輸入東西的時候,它才會啟動(參考)。但是個函式有個需要注意的特性,那就是它一次只能接收一個引述,或輸入值。一旦給他兩個或以上的輸入,會出錯。

0ef3106510e2e1630eb49744362999f8_r-jpg
Callback function 回呼函式的架構。Image Source

各位讀者還記得,一個訂閱者節點,通常必須先宣告一個 ros::Publisher 物件,並在主程式或建構子中用 NodeHandle 類別中的 advertise() 這個函式來宣告將發布的 topic,才能再度用 publish() 這個函式發布資料。那碰到以下的狀況,我們也可以稍微研究一下要怎麼實作。

寫程式的方式有好幾種,但我們僅以 C++ 的物件導向觀念主導,並僅以 Singleton Pattern 作討論

我想同一個訂閱的 Topic 給多個函式用

ROS 的機制,是用 Node Handle 一次取用一個 Topic,那今天假設我有兩個回呼函式要存取同一個 Topic 的資料怎麼辦?一般的解法,是宣告兩個 ros::Subscriber物件,承接同一個 Topic。所以,假設在最初一對一的情況下,我們是這樣寫的:

#include <iostream>

#include <ros/ros.h>

#include <sensor_msgs/LaserScan.h>

class myClass ()

{

public:

myClass () {

laser_sub_ = nh_.subscribe(“scan", 1, &myClass::laserCallback, this) ;

}

~myClass () {}

laserCallback (const sensor_msgs::LaserScan::ConstPtr& msg) {

// Do something

}

Private:

ros::NodeHandle nh_ ;

ros::Subscriber laser_sub ;

}  ;  /* End of class myClass */

 

// 主程式,程式進入點

int main(int argc, char** argv) {

ros::init(argc, argv, “multiple_subscriber_node”) ;

myClass myObj ;

ros::spin() ;

return 0 ;

} /* End of main */

 

這個程式基本上就是訂閱並接收了雷射訊號的節點,用物件導向的觀念時作成的,所以一切的動作,都在建構子內呼叫與完成。可以看到,在建構子內,Node Handle 訂閱了雷射訊號的 Topic,並將這個變數,指派給指標函式 laserCallback,也就是我們上面提到的回呼函式,這個函式再自己看看怎麼處理。但是當我們有兩個或以上的話,可以把上面的程式做延伸:

#include <iostream>

#include <ros/ros.h>

#include <sensor_msgs/LaserScan.h>

 

class myClass ()

{

public:

myClass () {

laser_sub_ = nh_.subscribe(“scan", 1, &myClass::laserCallback, this) ;

laser_sub2_ = nh_.subscribe(“scan", 1, &myClass:: anExtraCallback, this) ;

}

~myClass () {}

laserCallback (const sensor_msgs::LaserScan::ConstPtr& msg) {

// Do something

}

anExtraCallback (const sensor_msgs::LaserScan::ConstPtr& msg) {

// Do something

}

Private:

ros::NodeHandle nh_ ;

ros::Subscriber laser_sub_ ;

ros::Subscriber laser_sub2_ ;

}  ;  /* End of class myClass */

 

// 主程式,程式進入點

int main(int argc, char** argv) {

ros::init(argc, argv, “multiple_subscriber_node”) ;

myClass myObj ;

ros::spin() ;

return 0 ;

} /* End of main */

 

這邊我們又有另一個新的回呼函式,來接收同一個雷射資訊,然後再作處理。所以,重點就是,請搭配多個訂閱者物件,以及多個回呼函式,來讓多個函式同時接收一個資訊來源。

我們再來看看,那假如一個函式想要同時接收多個 Topic 怎麼辦。

假設我想要一個函式同時接收多個 Topics 怎麼辦?

這種問題其實很籠統。首先,你要拿這幾個 Topics 做什麼?通常一個 Topic 內的 message 其實是一種資料結構,我個人覺得有點類似 C 語言中的 struct 概念,可能會包含這個 Message 的轉換座標系、紀錄的時間點、卡式座標、四元素座標等。

好,先回答上述問題,其實 ROS 中有個工具,可以同時一個回呼函式接收數個話題,叫作 message_filters time synchronizer API,我個人沒用過,不過可以去官方網頁參考一下。

再繼續上面的思路。通常接收多個 Topics 後,會去直接存取每個 message 中的元素於程式的變數中,然後再多加利用。因此,整體結構寫法也就是:

  1. 用多個 NodeHandle Subscriber 去訂閱多個 Topics
  2. 在回呼函式中,將該 Message 的元素拆開來,各別指定給程式內的變數(請參考下一個問題的程式碼與解說。)
  3. 在其他成員函式內,運用這些變數作運算

具體的程式範例,我相當推薦讀者可以去參考 ROS Navigation Stack 的 AMCL,看作者如何處理雷射讀值與里程計(Odometry)讀值來作運算,相當清晰明瞭。

那如果我想要同一個節點發佈多個話題怎麼辦?

其實很像我們的第一個問題,就是再多加幾個 ros::Publisher 物件,然後用 NodeHandle 的 publish() 來發佈話題。我們拿官方給的基本範例程式做延伸,所以,多個發佈者物件寫出來就是這樣:

#include “ros/ros.h"

#include “std_msgs/String.h"

#include <sstream>

#include <sensor_msgs/LaserScan.h>

void laserPublish () {

unsigned int num_readings = 100;  double laser_frequency = 40;  double ranges[num_readings];  double intensities[num_readings];   int count = 0;  ros::Rate r(1.0);

//generate some fake data for our laser scan

for(unsigned int i = 0; i < num_readings; ++i){

ranges[i] = count;

intensities[i] = 100 + count;    }

ros::Time scan_time = ros::Time::now();

//populate the LaserScan message

sensor_msgs::LaserScan scan;

scan.header.stamp = scan_time;

scan.header.frame_id = “laser_frame";

scan.angle_min = -1.57;

scan.angle_max = 1.57;

scan.angle_increment = 3.14 / num_readings;

scan.time_increment = (1 / laser_frequency) / (num_readings);

scan.range_min = 0.0;

scan.range_max = 100.0;

scan.ranges.resize(num_readings);

scan.intensities.resize(num_readings);

for(unsigned int i = 0; i < num_readings; ++i){

scan.ranges[i] = ranges[i];

scan.intensities[i] = intensities[i];

}

// 這邊,我們發佈了我們的話題資訊

scan_pub.publish(scan);    ++count;

}  /* End of laserPublish */

 

// 主程式,程式進入點

int main(int argc, char **argv){ros::init(argc, argv, “talker");ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<std_msgs::String>(“chatter", 1000);

// 我們加了第二個發佈者物件,由於是 NodeHandle 在管理,我們只知道這個

// 話題叫作 scan。

ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>(“scan", 50);

ros::Rate loop_rate(10);

 

int count = 0;

while (ros::ok())   {

std_msgs::String msg;

std::stringstream ss;

ss << “hello world " << count;    msg.data = ss.str();

ROS_INFO(“%s", msg.data.c_str());

chatter_pub.publish(msg);

ros::spinOnce();

loop_rate.sleep();

++count;

// 我們發佈了第二個 Topic,也就是雷射的話題。

laserPublish() ;

}

return 0;

} /* End of Main */

 

我刻意把處理雷射資訊的部分另外寫成一個函式,只在主程式中呼叫,這樣整體程式碼看起來比較好維護與理解。整個程式讀更改有三個重點:

  1. 宣告一個新的發佈者物件,並指定這個 topic 的名稱,在此為”scan”:
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>(“scan", 50);
  1. 宣告一個資料變數,例如在 laserPublish() 裡面,我們宣告了雷射的變數:
sensor_msgs::LaserScan scan

也可以觀察一下,是怎麼指定該 message 中的其他資料的。

  1. 用 Node Handle 類別中的 publish() 來將資訊發佈出去。
scan_pub.publish(scan);

 

其他 Topics 的發佈,也如法炮製即可。

 

把可以被及時調整的變數,用 param 的方式參數化

有時候,我們希望程式中有些變數,可以在終端機上直接指定,或者在 launch 檔中指定。一種常用的方法,就是如果參數沒有必要在建構子的初始化列表內初始化的話,可以用 NodeHandle 類別的 param 來指定。Param 的語法是:

void param ((const std::string &param_name, T &param_val, const T &default_val) const

或者,可以直覺一點的說,就是:

void param (“[給人看的變數名稱]”, [實際的變數], [預設值]) ;

實際 param 能接受的資料類別可以參考這裡

我們仍然以上一個問題中的程式碼作延伸來示範一下:

#include “ros/ros.h"

#include “std_msgs/String.h"

#include <sstream>

#include <sensor_msgs/LaserScan.h>

void laserPublish (int num_readings, double laser_freq, std::string frame_id) {

  int num_readings = num_readings ; 

   double laser_frequency = laser_freq ; 

double ranges[num_readings];

double intensities[num_readings];

int count = 0;  ros::Rate r(1.0);

 

//generate some fake data for our laser scan

for(unsigned int i = 0; i < num_readings; ++i){

ranges[i] = count;

intensities[i] = 100 + count;

}

ros::Time scan_time = ros::Time::now();

//populate the LaserScan message

sensor_msgs::LaserScan scan;

scan.header.stamp = scan_time;

    scan.header.frame_id = frame_id;   

scan.angle_min = -1.57;

scan.angle_max = 1.57;

scan.angle_increment = 3.14 / num_readings;

scan.time_increment = (1 / laser_frequency) / (num_readings);

scan.range_min = 0.0;    scan.range_max = 100.0;

scan.ranges.resize(num_readings);

scan.intensities.resize(num_readings);

for(unsigned int i = 0; i < num_readings; ++i){

scan.ranges[i] = ranges[i];

scan.intensities[i] = intensities[i];

}

// 這邊,我們發佈了我們的話題資訊

scan_pub.publish(scan);    ++count;

}  /* End of laserPublish */

 

// 主程式,程式進入點

int main(int argc, char **argv){  ros::init(argc, argv, “talker");  ros::NodeHandle n;

int num_readings ;

double laser_freq ;

std::string frame_id ;

 

// 設定參數,之後也可以由外部作調整

  n . param <int> (“number_of_readings”, num_readings, 100) ;

  n . param <double> (“laser_frequency”, laser_freq, 40) ;

  n . param <std::string> (“frame_id”, frame_id, “laser_link”) ;

 

ros::Publisher chatter_pub = n.advertise<std_msgs::String>(“chatter", 1000);

// 我們加了第二個發佈者物件,由於是 NodeHandle 在管理,我們只知道這個

// 話題叫作 scan。

ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>(“scan", 50); ros::Rate loop_rate(10);

int count = 0;  while (ros::ok())

std_msgs::String msg;

std::stringstream ss;

ss << “hello world " << count;

msg.data = ss.str();

ROS_INFO(“%s", msg.data.c_str());

chatter_pub.publish(msg);

ros::spinOnce();

loop_rate.sleep();

++count;

// 我們發佈了第二個 Topic,也就是雷射的話題。

    laserPublish(int num_readings, double laser_freq, std::string frame_id) ; 

}

return 0;

} /* End of Main */

 

我們標成紅字的地方,就是修改過的地方。這樣,我們如果要從終端機中調整參數,可以先查詢有哪一些參數可以作調整。首先,rosrun 開啟這個程式節點,在終端機中打:

$ rosparam list

通常,只要看到有列出的參數,就是我們可以調整的參數,通常都是以下格式:

[節點]/[給人看的參數名稱]

這樣,例如上述我們的示範程式,應該就會是

talker/number_of_readings

talker/laser_frequency

talker/frame_id

 

而當我們要修改參數值的時候,就可以直接在終端機上下指令:

$ rosrun myPkg talker /number_of_readings:=50 /frame_id:=/hokuyo_link

 

這樣,我們當場就改了兩個參數的值。有沒有注意到一件也很重要的事情,就是我們也把 TF 座標用 frame_id 參數化,方便隨時更換名稱?這種作法給予應用上相當大的彈性,也只有這種方法,才可以輕鬆替換 TF 的名稱,因為 ROS 中,並沒有 remap TF 這回事,一旦寫死,想要更換 TF 座標就真的沒辦法了。這也是一個有用的撇步。

至於在 Launch 檔中,可以寫成:

<node pkg=”myPkg” type=”talker” name=”talker”>

<param name=”number_of_readings” value=”50” />

<param name=”laser_frequency” value=”150” />

<param name=”” value=”hokuyo_link” />

</node>

其實不一定每項參數都要特別指定,需要更改的再指定就好,反正我們在程式內已經指定了預設值,但倘若不指定的話,那就必須再啟動該節點時特別指定參數值。

好,那我這篇就先寫到這邊,之後有其他有用的撰寫程式技巧,再與各位分享,屆時會連載到另一篇新文章中。

 

這篇是繼撰寫自己的節點這篇而來的,若對如何撰寫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 節點的概念與意義這篇中,釐清其中的概念。

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

 

相關主題文章:

更多精采文章,請回目錄瀏覽!

如何使用節點

繼前一篇 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 都可以發現機器人的移動行為變了!這就表示,你成功了!

 

更多精采文章,請回目錄瀏覽!