2019年7月1日 星期一

ROS 2 communication with DDS part 1

因為ROS 2 底層是DDS,當你懂DDS 時就可以直接互通
搭配工具是Prismtech 的OpenSplice 的ospltunner
可以用這個工具收送 ROS 2 的Topic / Service .. etc
也可以觀察QoS 等等
DDS 與 ROS 2 能不能互通的關鍵因素就跟DDS 本身一樣,有以下5 點
  • Topic Name:ROS 2 的Topic Name 會有一定的規則可循
  • Partition:ROS 2 的Partition 會有一定的規則可循
  • Topic type:ROS 2 是用msg 或srv 表示data format,當ROS 2 編譯時,會將msg / srv 轉成idl 檔案,因此只要參考這個檔案,就能取得對應的DDS 的idl format
  • Domain ID:ROS 2 預設domain id 也是0,當然也可以修改,規則與DDS 一樣
  • QoS:ROS 2 依照不同的模式會有不同的default QoS,可以查表就知道對方的QoS,或者用rclcpp API 修改QoS

Topic Name 及 Partition

ROS 底下的DDS 有自己的partition name,依據ROS 的功能不同會有不同的partition
table 如下
ROS 2 Subsystem
DDS Partition
ROS Topicsrt
ROS Service Requestrq
ROS Service Responserr
ROS Servicers
ROS Parameterrp
ROS Actionra
在ROS 2 publisher subscriber 模式下,一般情況下Topic name 就是直接對應DDS 的Topic name,partition 則會是照table 為rt
例外情況為如下程式碼的pub 2,Topic name 的"hello" 前有一個斜線,因此會把DDS parititon 改為rt/foo,而Topic name 仍維持hello
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("node_name");
    auto pub1 = node->create_publisher<std_msgs::msg::String>("hello", 10);
    auto pub2 = node->create_publisher<std_msgs::msg::String>("foo/hello", 10);
}
如果是Service / Client 模式,因為一個service 會拆成一個request topic 及reply topic
因此會產生兩個後墜詞 是Request 及Reply 的Topic 
以下面的例子,server1 會產生兩個Topic,Topic name 分別為killRequest 及killReply
partion 的規則則跟publiser subscriber 模式一樣
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("node_name");
    auto server1 = node->create_service<std_msgs::msg::String>("kill", 10);
    auto server2 = node->create_service<std_msgs::msg::String>("foo/kill", 10);
}
 用ospltun 測試方式
1.先開啟ROS 2 端的程式,再啟動ospl 及ospltun
2.你會看到ROS 2 的Topic (假設domain ID 一樣),例如killRequest
3.右鍵選擇create Reader-Writer,先選擇Partition expression 
4.Partiton expression 的欄位輸入 * (一個米字號 ),代表所有的partiton 都接收,然後按下ok
5.選擇上面的Edit 再選start monitoring,接著ospltun 就會接受所有關於killRequest 的topic
6.然後到ROS 2 的環境,發送一個kill 的servcie,就會看到底下有收到的sample
7.你可以觀察收到的sample 內容有甚麼
8.把視窗關掉之後回到步驟3,但這次選擇Existing Partition
9.這次上面的Partition 欄位有個下拉選單,就會顯示ROS 2 所用的partiton name 了



Topic type

如果要讓DDS 與ROS 2 互通,第三個重要因子是,資料結構
不管你是用ROS 2 的srv 或者msg 格式,都會轉出idl 檔案及對應的C++ header
可以選擇直接把header 拿去製作DDS 的程式,或者拿idl 檔案重新gen 出header
如果你的msg / srv 是自己建立的type,可以在$ROS2_WS/install/include$PACKAGE/msg 或者$ROS2_WS/install/include/$PACKAGE/srv 找到產生的header
幸運的是,預設是會產生OpenSplice 的檔案
idl 的檔案位置,則是在$ROS2_WS/install/share$PACKAGE/msg/dds_opensplice 內找到idl 檔案
ROS 2 預設提供的topic type 則是在$ROS2/include 及 $ROS2/share 找到對應的package 內就有檔案了

用ospltun 測試方式
1.一樣右鍵你要看的topic,這是選擇Display Entity
2.選擇分頁Data type 就能看到在ROS msg /srv 對應到OpenSplice 的資料結構


Domain ID

因為ROS 2 一樣會去使用ospl.xml 檔案,所以修改查詢方式與DDS 一樣

QoS

ROS 2 目前提供的QoS 只有Reliability、Durability 及History 三種 (基本DDS 應用也都是圍繞這三種)
預設值一般是Reliable 、Volatile 及keep last 1
ROS 2 有提供幾個profile 供使用,參考qos_profile.h 
或者呼叫rclcpp API 修改
int main(int argc, char **argv){
  rclcpp::init(argc, argv);
  
  auto node = rclcpp::Node::make_shared("talker");//node name = talker
  
  rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
  custom_qos_profile.depth = 7;
  custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
  
  auto chatter_pub = node->create_publisher<karl_demo::msg::Custom>("chatter", custom_qos_profile);//topic name = chatter
}

沒有留言:

張貼留言

NoSQL Redis intro

Redis是一個使用ANSI C編寫的開源、支援網路、基於記憶體、可選永續性的鍵值對儲存資料庫。 支援rdb 及aof 兩種儲存方式 From  https://zh.wikipedia.org/wiki/Redis Redis 目前擁有兩種資料...