implement with_info version of take (ros2#161)

* implement with_info version of take Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]> * Fix function names and signatures. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]> * Return 0 on received for responses as well Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>
RoverRobotics-forks · Apr 24, 2020 · 2638f4a · 2638f4a
1 parent 27bc819
commit 2638f4a
Showing 1 changed file with 9 additions and 6 deletions.
diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp
@@ -2856,7 +2856,7 @@ extern "C" rmw_ret_t rmw_wait(
 /////////////////////////////////////////////////////////////////////////////////////////
 
 static rmw_ret_t rmw_take_response_request(
-  CddsCS * cs, rmw_request_id_t * request_header,
+  CddsCS * cs, rmw_service_info_t * request_header,
   void * ros_data, bool * taken, dds_time_t * source_timestamp,
   dds_instance_handle_t srcfilter)
 {
@@ -2870,9 +2870,12 @@ static rmw_ret_t rmw_take_response_request(
   while (dds_take(cs->sub->enth, &wrap_ptr, &info, 1, 1) == 1) {
     if (info.valid_data) {
       memset(request_header, 0, sizeof(wrap.header));
-      assert(sizeof(wrap.header.guid) <= sizeof(request_header->writer_guid));
-      memcpy(request_header->writer_guid, &wrap.header.guid, sizeof(wrap.header.guid));
-      request_header->sequence_number = wrap.header.seq;
+      assert(sizeof(wrap.header.guid) <= sizeof(request_header->request_id.writer_guid));
+      memcpy(request_header->request_id.writer_guid, &wrap.header.guid, sizeof(wrap.header.guid));
+      request_header->request_id.sequence_number = wrap.header.seq;
+      request_header->source_timestamp = info.source_timestamp;
+      // TODO(iluetkeb) replace with real received timestamp when available in cyclone
+      request_header->received_timestamp = 0;
       if (source_timestamp) {
         *source_timestamp = info.source_timestamp;
       }
@@ -2888,7 +2891,7 @@ static rmw_ret_t rmw_take_response_request(
 
 extern "C" rmw_ret_t rmw_take_response(
   const rmw_client_t * client,
-  rmw_request_id_t * request_header, void * ros_response,
+  rmw_service_info_t * request_header, void * ros_response,
   bool * taken)
 {
   RET_WRONG_IMPLID(client);
@@ -2935,7 +2938,7 @@ static void check_for_blocked_requests(CddsClient & client)
 
 extern "C" rmw_ret_t rmw_take_request(
   const rmw_service_t * service,
-  rmw_request_id_t * request_header, void * ros_request,
+  rmw_service_info_t * request_header, void * ros_request,
   bool * taken)
 {
   RET_WRONG_IMPLID(service);