Message info with timestamps support in rcl (#619)

* Zero-initialize message_info prior to taking

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Added test for timestamp presence

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* move message_info test to a new test file

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Add conditional timestamp test to normal subscriber test

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Remove dedicated timestamp test

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Use rmw_time_point_value_t instead of rmw_time_t

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Use rcutils for cross-platform compatibility.

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Style fix.

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* More style fix.

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* test fastrtps_dynamic properly; also cmake linter error

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Expect timestamps on cyclonedds as well

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

* Distinguish received timestamp support.

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <[email protected]>

Author: iluetkeb

rcl/src/rcl/subscription.c

@@ -263,6 +263,7 @@ rcl_take(
   // If message_info is NULL, use a place holder which can be discarded.
   rmw_message_info_t dummy_message_info;
   rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+  *message_info_local = rmw_get_zero_initialized_message_info();
   // Call rmw_take_with_info.
   bool taken = false;
   rmw_ret_t ret = rmw_take_with_info(
@@ -298,6 +299,7 @@ rcl_take_serialized_message(
   // If message_info is NULL, use a place holder which can be discarded.
   rmw_message_info_t dummy_message_info;
   rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+  *message_info_local = rmw_get_zero_initialized_message_info();
   // Call rmw_take_with_info.
   bool taken = false;
   rmw_ret_t ret = rmw_take_serialized_message_with_info(
@@ -335,6 +337,7 @@ rcl_take_loaned_message(
   // If message_info is NULL, use a place holder which can be discarded.
   rmw_message_info_t dummy_message_info;
   rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+  *message_info_local = rmw_get_zero_initialized_message_info();
   // Call rmw_take_with_info.
   bool taken = false;
   rmw_ret_t ret = rmw_take_loaned_message_with_info(

rcl/test/CMakeLists.txt

@@ -205,6 +205,20 @@ function(test_target_function)
     LIBRARIES ${PROJECT_NAME}
     AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
   )
+  if(rmw_implementation STREQUAL "rmw_fastrtps_cpp" OR
+    rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp")
+    message(STATUS "Enabling message timestamp test for ${rmw_implementation}")
+    target_compile_definitions(test_subscription${target_suffix}
+      PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1")
+  else()
+    if(rmw_implementation STREQUAL "rmw_cyclonedds_cpp")
+      message(STATUS "Enabling only source timestamp test for ${rmw_implementation}")
+      target_compile_definitions(test_subscription${target_suffix}
+        PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1")
+    else()
+      message(STATUS "Disabling message timestamp test for ${rmw_implementation}")
+    endif()
+  endif()
 
   rcl_add_custom_gtest(test_events${target_suffix}
     SRCS rcl/test_events.cpp

rcl/test/rcl/test_subscription.cpp

@@ -179,6 +179,12 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
   //                probably using the count_subscriptions busy wait mechanism
   //                until then we will sleep for a short period of time
   std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+#ifdef RMW_TIMESTAMPS_SUPPORTED
+  rcl_time_point_value_t pre_publish_time;
+  EXPECT_EQ(
+    RCUTILS_RET_OK,
+    rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed";
+#endif
   {
     test_msgs__msg__BasicTypes msg;
     test_msgs__msg__BasicTypes__init(&msg);
@@ -197,9 +203,25 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     {
       test_msgs__msg__BasicTypes__fini(&msg);
     });
-    ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+    rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+    ret = rcl_take(&subscription, &msg, &message_info, nullptr);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     ASSERT_EQ(42, msg.int64_value);
+  #ifdef RMW_TIMESTAMPS_SUPPORTED
+    EXPECT_NE(0u, message_info.source_timestamp);
+    EXPECT_TRUE(pre_publish_time <= message_info.source_timestamp) <<
+      pre_publish_time << " > " << message_info.source_timestamp;
+  #ifdef RMW_RECEIVED_TIMESTAMP_SUPPORTED
+    EXPECT_NE(0u, message_info.received_timestamp);
+    EXPECT_TRUE(pre_publish_time <= message_info.received_timestamp);
+    EXPECT_TRUE(message_info.source_timestamp <= message_info.received_timestamp);
+  #else
+    EXPECT_EQ(0u, message_info.received_timestamp);
+  #endif
+  #else
+    EXPECT_EQ(0u, message_info.source_timestamp);
+    EXPECT_EQ(0u, message_info.received_timestamp);
+  #endif
   }
 }