Message info with timestamps support in rcl (#619)

* Zero-initialize message_info prior to taking Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Added test for timestamp presence Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * move message_info test to a new test file Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Add conditional timestamp test to normal subscriber test Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Remove dedicated timestamp test Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Use rmw_time_point_value_t instead of rmw_time_t Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Use rcutils for cross-platform compatibility. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Style fix. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * More style fix. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * test fastrtps_dynamic properly; also cmake linter error Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Expect timestamps on cyclonedds as well Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com> * Distinguish received timestamp support. Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com>
ros2 · Apr 23, 2020 · 643f6ec · 643f6ec
1 parent b343368
commit 643f6ec
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 1 deletion.
diff --git a/rcl/src/rcl/subscription.c b/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(

diff --git a/rcl/test/CMakeLists.txt b/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

diff --git a/rcl/test/rcl/test_subscription.cpp b/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
   }
 }