ROS 2 communication with DDS part 3

ROS 2 ardent

以下srv 檔案使用ROS 2 提供現成的，屬於package example_interfaces

先建立一個srv 檔案

描述request 為int a 及int b

reply 則為int sum

AddTwoInts.srv

int64 a int64 b --- int64 sum

ROS 2 會先轉成2個msg 檔案，如下

AddTwoInts_Request.msg

int64 a int64 b

AddTwoInts_Response.msg

int64 sum

ROS 2 再從這兩個msg 檔案產生總共4 個idl

以下會以Request 為例 (Reply 是一樣的)

AddTwoInts_Request_.idl

// generated from rosidl_generator_dds_idl/resource/msg.idl.em   #ifndef __example_interfaces__srv__AddTwoInts_Request__idl__ #define __example_interfaces__srv__AddTwoInts_Request__idl__     module example_interfaces {   module srv {   module dds_ {         struct AddTwoInts_Request_ {     long long a_;   long long b_;   };  // struct AddTwoInts_Request_   #pragma keylist AddTwoInts_Request_   };  // module dds_   };  // module srv   };  // module example_interfaces   #endif  // __example_interfaces__srv__AddTwoInts_Request__idl__

Sample_AddTwoInts_Request_.idl

// generated from rosidl_generator_dds_idl/resource/msg.idl.em   #ifndef __example_interfaces__srv__Sample_AddTwoInts_Request__idl__ #define __example_interfaces__srv__Sample_AddTwoInts_Request__idl__     #include "example_interfaces/srv/dds_opensplice/AddTwoInts_Request_.idl"   module example_interfaces {   module srv {   module dds_ {         struct Sample_AddTwoInts_Request_ {     unsigned long long client_guid_0_;   unsigned long long client_guid_1_;   long long sequence_number_;   example_interfaces::srv::dds_::AddTwoInts_Request_ request_;   };  // struct Sample_AddTwoInts_Request_   #pragma keylist Sample_AddTwoInts_Request_   };  // module dds_   };  // module srv   };  // module example_interfaces   #endif  // __example_interfaces__srv__Sample_AddTwoInts_Request__idl__

使用VortexOpenSplice 或者VortexLite 的generator 這4個idl 轉成要用的DDS 語言 header

idl 內的include 路徑須自行修改

例如用VortexOpenSplice 建立c99 的header 會得到以下檔案

接著coding 必須引入這些檔案

假如我們要做為一個server 端，接收request 及發送reply

其source code 簡單如下

#include "Sample_AddTwoInts_Request_.h" #include "Sample_AddTwoInts_Response_.h"   int main(int argc, char **argv) {     int error = dds_init(argc, argv);     if (error < 0)     {         printf("dds_init %d\n", dds_err_no(error));     }       dds_entity_t pp;     dds_qos_t *pp_qos = dds_qos_create();     dds_qset_userdata(pp_qos, "name=service_dds;", 18); //ROS 2 node name     error = dds_participant_create(&pp, DDS_DOMAIN_DEFAULT, pp_qos, NULL);     if (error < 0)     {         printf("dds_participant_create %d\n", dds_err_no(error));     }       /*     create topics addTwoIntsRequest and addTwoIntsReply     so ros2 can use service list to check addTwoInts     partiton rq for addTwoIntsRequest     partiton rr for addTwoIntsReply     should be reliable too     */       /*=======================================================*/     dds_entity_t topic;     error = dds_topic_create(pp, &topic, &example_interfaces_srv_dds__Sample_AddTwoInts_Request__desc, "addTwoIntsRequest", NULL, NULL);     if (error < 0)     {         printf("dds_topic_create %d\n", dds_err_no(error));     }             dds_qos_t *qos = dds_qos_create();     const char * partitions[1];     partitions[0] = "rq";     dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(1));     dds_qset_durability(qos, DDS_DURABILITY_TRANSIENT_LOCAL);     dds_qset_partition(qos, 1, partitions);     dds_entity_t sub;     error = dds_subscriber_create(pp, &sub, qos, NULL);     if (error < 0)     {         printf("dds_subscriber_create %d\n", dds_err_no(error));     }       dds_entity_t reader;     error = dds_reader_create(sub, &reader, topic, NULL, NULL);     if (error < 0)     {         printf("dds_reader_create %d\n", dds_err_no(error));     }           /*=======================================================*/     error = dds_topic_create(pp, &topic, &example_interfaces_srv_dds__Sample_AddTwoInts_Response__desc, "addTwoIntsReply", NULL, NULL);     if (error < 0)     {         printf("dds_topic_create %d\n", dds_err_no(error));     }             partitions[0] = "rr";     dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(1));     dds_qset_durability(qos, DDS_DURABILITY_TRANSIENT_LOCAL);     dds_qset_partition(qos, 1, partitions);       dds_entity_t pub;     error = dds_publisher_create(pp, &pub, qos, NULL);     if (error < 0)     {         printf("dds_publisher_create %d\n", dds_err_no(error));     }       dds_entity_t writer;     error = dds_writer_create(pub, &writer, topic, NULL, NULL);     if (error < 0)     {         printf("dds_writer_create %d\n", dds_err_no(error));     }     /*=======================================================*/     dds_sample_info_t info = { 0 };     void *samples[1] = { {0} };     example_interfaces_srv_dds__Sample_AddTwoInts_Request_ *sample = NULL;     example_interfaces_srv_dds__Sample_AddTwoInts_Response_ msg = { 0 };     while (1)     {         int ret = dds_take(reader, samples, 1, &info, DDS_ANY_STATE);         if (ret > 0 && info.valid_data)         {             sample = samples[0];             int a = sample->request_.a_;             int b = sample->request_.b_;             int sum = a + b;             printf("%d + %d = %d\n", a, b, sum);             msg.client_guid_0_ = sample->client_guid_0_;             msg.client_guid_1_ = sample->client_guid_1_;             msg.response_.sum_ = sum;             dds_sleepfor(DDS_SECS(1));             dds_write(writer, &msg);         }         dds_sleepfor(DDS_SECS(1));     }   }

編譯成功後並執行

於ROS 2 環境 可以得到以下結果

karlkwchen@karlkwchen-VirtualBox:~$ ros2 node list service_dds     karlkwchen@karlkwchen-VirtualBox:~$ ros2 service list /addTwoInts   karlkwchen@karlkwchen-VirtualBox:~$ ros2 service call /addTwoInts example_interfaces/AddTwoInts "{a: 1, b: 3}" requester: making request: example_interfaces.srv.AddTwoInts_Request(a=1, b=3)   response: example_interfaces.srv.AddTwoInts_Response(sum=4)