用 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)

 

廣告

發表迴響

在下方填入你的資料或按右方圖示以社群網站登入:

WordPress.com 標誌

您的留言將使用 WordPress.com 帳號。 登出 /  變更 )

Google photo

您的留言將使用 Google 帳號。 登出 /  變更 )

Twitter picture

您的留言將使用 Twitter 帳號。 登出 /  變更 )

Facebook照片

您的留言將使用 Facebook 帳號。 登出 /  變更 )

連結到 %s