用 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的節點還不太清楚,可以去參考。

 

相關主題文章:

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