Fill in message_info timestamps (ros2#163)

* Fill in message_info timestamps Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]> * Report error when timestamp cannot be obtained. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]> * return 0 for received timestamp to signify lack of support Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>
RoverRobotics-forks · Apr 23, 2020 · 27bc819 · 27bc819
1 parent 0fc4a3f
commit 27bc819
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp
@@ -2137,13 +2137,17 @@ static rmw_ret_t rmw_take_int(
   dds_sample_info_t info;
   while (dds_take(sub->enth, &ros_message, &info, 1, 1) == 1) {
     if (info.valid_data) {
+      *taken = true;
       if (message_info) {
         message_info->publisher_gid.implementation_identifier = eclipse_cyclonedds_identifier;
         memset(message_info->publisher_gid.data, 0, sizeof(message_info->publisher_gid.data));
         assert(sizeof(info.publication_handle) <= sizeof(message_info->publisher_gid.data));
         memcpy(
           message_info->publisher_gid.data, &info.publication_handle,
           sizeof(info.publication_handle));
+        message_info->source_timestamp = info.source_timestamp;
+        // TODO(iluetkeb) add received timestamp, when implemented by Cyclone
+        message_info->received_timestamp = 0;
       }
 #if REPORT_LATE_MESSAGES > 0
       dds_time_t tnow = dds_time();
@@ -2152,7 +2156,6 @@ static rmw_ret_t rmw_take_int(
         fprintf(stderr, "** sample in history for %.fms\n", static_cast<double>(dt) / 1e6);
       }
 #endif
-      *taken = true;
       return RMW_RET_OK;
     }
   }