撰寫節點的小撇步

這篇是繼撰寫自己的節點這篇而來的,若對如何撰寫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的節點還不太清楚,可以去參考。

 

相關主題文章:

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

廣告

發表迴響

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

WordPress.com 標誌

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

Google photo

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

Twitter picture

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

Facebook照片

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

連結到 %s