From 5070188a5f8a885c68ee4cc5adc423313de7d6e5 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 14 Apr 2020 18:09:17 +0200 Subject: [PATCH 01/12] Zero-initialize message_info prior to taking Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/subscription.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index bf744378e..d52a721a9 100644 --- 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( From ac4d27ff75b19863defecb985c73dc8a8deb8d15 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 14 Apr 2020 20:22:03 +0200 Subject: [PATCH 02/12] Added test for timestamp presence Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_subscription.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 1d830b862..09f177668 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -197,9 +197,14 @@ 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); + EXPECT_NE(0u, message_info.source_timestamp.sec); + EXPECT_NE(0u, message_info.source_timestamp.nsec); + EXPECT_NE(0u, message_info.received_timestamp.sec); + EXPECT_NE(0u, message_info.received_timestamp.nsec); } } From 48257f4e008663a315bf808c10745cddad6574cf Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 16 Apr 2020 13:49:47 +0200 Subject: [PATCH 03/12] move message_info test to a new test file Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 17 +++ rcl/test/rcl/test_message_info.cpp | 178 +++++++++++++++++++++++++++++ rcl/test/rcl/test_subscription.cpp | 4 - 3 files changed, 195 insertions(+), 4 deletions(-) create mode 100644 rcl/test/rcl/test_message_info.cpp diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 77505b150..6e9f5823f 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -206,6 +206,23 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + set(AMENT_GTEST_ARGS "SKIP_TEST") + # TODO(wjwwood): remove this when the graph API works properly for connext dynamic + if(rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp") + message(STATUS "Skipping test_graph${target_suffix} test.") + else() + message(STATUS "Disabling test_message_info${target_suffix} for ${rmw_implementation}.") + endif() + rcl_add_custom_gtest(test_message_info${target_suffix} + SRCS rcl/test_message_info.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ${AMENT_GTEST_ARGS} + ) + + rcl_add_custom_gtest(test_events${target_suffix} SRCS rcl/test_events.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_message_info.cpp b/rcl/test/rcl/test_message_info.cpp new file mode 100644 index 000000000..5214f3ef1 --- /dev/null +++ b/rcl/test/rcl/test_message_info.cpp @@ -0,0 +1,178 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rcl/rcl.h" +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/msg/strings.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestMessageInfoFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_message_info_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +void +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms, + bool & success) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + size_t iteration = 0; + do { + ++iteration; + ret = rcl_wait_set_clear(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + success = true; + return; + } + } + } while (iteration < max_tries); + success = false; +} + +/* Basic nominal test of a subscription + */ +TEST_F(CLASSNAME(TestMessageInfoFixture, RMW_IMPLEMENTATION), test_timestamp_take_sub) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_reset_error(); + + // TODO(wjwwood): add logic to wait for the connection to be established + // 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)); + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + bool success; + wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); + ASSERT_TRUE(success); + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + 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); + EXPECT_NE(0u, message_info.source_timestamp.sec); + EXPECT_NE(0u, message_info.source_timestamp.nsec); + EXPECT_NE(0u, message_info.received_timestamp.sec); + EXPECT_NE(0u, message_info.received_timestamp.nsec); + } +} diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 09f177668..9ff157a52 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -201,10 +201,6 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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); - EXPECT_NE(0u, message_info.source_timestamp.sec); - EXPECT_NE(0u, message_info.source_timestamp.nsec); - EXPECT_NE(0u, message_info.received_timestamp.sec); - EXPECT_NE(0u, message_info.received_timestamp.nsec); } } From 6bf80166d27f6e3960f0097583d4117bdc282caa Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:14:37 +0200 Subject: [PATCH 04/12] Add conditional timestamp test to normal subscriber test Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 9 +++++++-- rcl/test/rcl/test_subscription.cpp | 21 +++++++++++++++++++++ 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 6e9f5823f..541a1448f 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -205,9 +205,15 @@ function(test_target_function) LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + if(rmw_implementation STREQUAL "rmw_fastrtps_cpp") + message(STATUS "Enabling message 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() + # enable timestamps test only for middlewares that support it set(AMENT_GTEST_ARGS "SKIP_TEST") - # TODO(wjwwood): remove this when the graph API works properly for connext dynamic if(rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp") message(STATUS "Skipping test_graph${target_suffix} test.") else() @@ -222,7 +228,6 @@ function(test_target_function) ${AMENT_GTEST_ARGS} ) - rcl_add_custom_gtest(test_events${target_suffix} SRCS rcl/test_events.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 9ff157a52..110b14fe9 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -179,6 +179,8 @@ 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)); + timespec pre_publish_time; + EXPECT_EQ(0, clock_gettime(CLOCK_REALTIME, &pre_publish_time)) << " clock_gettime failed"; { test_msgs__msg__BasicTypes msg; test_msgs__msg__BasicTypes__init(&msg); @@ -201,6 +203,25 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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.sec); + EXPECT_TRUE(pre_publish_time.tv_sec <= (time_t)message_info.source_timestamp.sec) << + pre_publish_time.tv_sec << " > " << (time_t)message_info.source_timestamp.sec; + EXPECT_NE(0u, message_info.source_timestamp.nsec); + EXPECT_TRUE(pre_publish_time.tv_nsec <= (long)message_info.source_timestamp.nsec) << + pre_publish_time.tv_nsec << " > " << (time_t)message_info.source_timestamp.nsec; + EXPECT_NE(0u, message_info.received_timestamp.sec); + EXPECT_TRUE(pre_publish_time.tv_sec <= (time_t)message_info.received_timestamp.sec); + EXPECT_TRUE(message_info.source_timestamp.sec <= message_info.received_timestamp.sec); + EXPECT_NE(0u, message_info.received_timestamp.nsec); + EXPECT_TRUE(pre_publish_time.tv_nsec <= (long)message_info.received_timestamp.nsec); + EXPECT_TRUE(message_info.source_timestamp.nsec <= message_info.received_timestamp.nsec); + #else + EXPECT_EQ(0u, message_info.source_timestamp.sec); + EXPECT_EQ(0u, message_info.source_timestamp.nsec); + EXPECT_EQ(0u, message_info.received_timestamp.sec); + EXPECT_EQ(0u, message_info.received_timestamp.nsec); + #endif } } From 840c34ad41c4221b137e206e3a738a9cf3ff124a Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:15:58 +0200 Subject: [PATCH 05/12] Remove dedicated timestamp test Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 16 --- rcl/test/rcl/test_message_info.cpp | 178 ----------------------------- 2 files changed, 194 deletions(-) delete mode 100644 rcl/test/rcl/test_message_info.cpp diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 541a1448f..3879d710f 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -212,22 +212,6 @@ function(test_target_function) message(STATUS "Disabling message timestamp test for ${rmw_implementation}") endif() - # enable timestamps test only for middlewares that support it - set(AMENT_GTEST_ARGS "SKIP_TEST") - if(rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp") - message(STATUS "Skipping test_graph${target_suffix} test.") - else() - message(STATUS "Disabling test_message_info${target_suffix} for ${rmw_implementation}.") - endif() - rcl_add_custom_gtest(test_message_info${target_suffix} - SRCS rcl/test_message_info.cpp - ENV ${rmw_implementation_env_var} - APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} - AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" - ${AMENT_GTEST_ARGS} - ) - rcl_add_custom_gtest(test_events${target_suffix} SRCS rcl/test_events.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_message_info.cpp b/rcl/test/rcl/test_message_info.cpp deleted file mode 100644 index 5214f3ef1..000000000 --- a/rcl/test/rcl/test_message_info.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include - -#include "rcl/subscription.h" - -#include "rcl/rcl.h" -#include "test_msgs/msg/basic_types.h" -#include "test_msgs/msg/strings.h" -#include "rosidl_runtime_c/string_functions.h" - -#include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rcl/error_handling.h" - -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - -class CLASSNAME (TestMessageInfoFixture, RMW_IMPLEMENTATION) : public ::testing::Test -{ -public: - rcl_context_t * context_ptr; - rcl_node_t * node_ptr; - void SetUp() - { - rcl_ret_t ret; - { - rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); - ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; - }); - this->context_ptr = new rcl_context_t; - *this->context_ptr = rcl_get_zero_initialized_context(); - ret = rcl_init(0, nullptr, &init_options, this->context_ptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - this->node_ptr = new rcl_node_t; - *this->node_ptr = rcl_get_zero_initialized_node(); - const char * name = "test_message_info_node"; - rcl_node_options_t node_options = rcl_node_get_default_options(); - ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - - void TearDown() - { - rcl_ret_t ret = rcl_node_fini(this->node_ptr); - delete this->node_ptr; - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_shutdown(this->context_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_context_fini(this->context_ptr); - delete this->context_ptr; - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } -}; - -void -wait_for_subscription_to_be_ready( - rcl_subscription_t * subscription, - rcl_context_t * context, - size_t max_tries, - int64_t period_ms, - bool & success) -{ - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_wait_set_fini(&wait_set); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - size_t iteration = 0; - do { - ++iteration; - ret = rcl_wait_set_clear(&wait_set); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); - if (ret == RCL_RET_TIMEOUT) { - continue; - } - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { - if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { - success = true; - return; - } - } - } while (iteration < max_tries); - success = false; -} - -/* Basic nominal test of a subscription - */ -TEST_F(CLASSNAME(TestMessageInfoFixture, RMW_IMPLEMENTATION), test_timestamp_take_sub) { - rcl_ret_t ret; - rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic = "/chatter"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - ret = rcl_subscription_fini(&subscription, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - rcl_reset_error(); - - // TODO(wjwwood): add logic to wait for the connection to be established - // 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)); - { - test_msgs__msg__BasicTypes msg; - test_msgs__msg__BasicTypes__init(&msg); - msg.int64_value = 42; - ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__BasicTypes__fini(&msg); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - bool success; - wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_TRUE(success); - { - test_msgs__msg__BasicTypes msg; - test_msgs__msg__BasicTypes__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__BasicTypes__fini(&msg); - }); - 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); - EXPECT_NE(0u, message_info.source_timestamp.sec); - EXPECT_NE(0u, message_info.source_timestamp.nsec); - EXPECT_NE(0u, message_info.received_timestamp.sec); - EXPECT_NE(0u, message_info.received_timestamp.nsec); - } -} From 86490c032430018869e31fc964360c76ace0a801 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:27:28 +0200 Subject: [PATCH 06/12] Use rmw_time_point_value_t instead of rmw_time_t Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_subscription.cpp | 32 +++++++++++++----------------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 110b14fe9..ef5ba45fe 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -179,8 +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)); - timespec pre_publish_time; - EXPECT_EQ(0, clock_gettime(CLOCK_REALTIME, &pre_publish_time)) << " clock_gettime failed"; +#ifdef RMW_TIMESTAMPS_SUPPORTED + timespec pre_publish_ts; + EXPECT_EQ(0, clock_gettime(CLOCK_REALTIME, &pre_publish_ts)) << " clock_gettime failed"; + rmw_time_point_value_t pre_publish_time = + RCUTILS_S_TO_NS(pre_publish_ts.tv_sec) + pre_publish_ts.tv_nsec; +#endif { test_msgs__msg__BasicTypes msg; test_msgs__msg__BasicTypes__init(&msg); @@ -204,23 +208,15 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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.sec); - EXPECT_TRUE(pre_publish_time.tv_sec <= (time_t)message_info.source_timestamp.sec) << - pre_publish_time.tv_sec << " > " << (time_t)message_info.source_timestamp.sec; - EXPECT_NE(0u, message_info.source_timestamp.nsec); - EXPECT_TRUE(pre_publish_time.tv_nsec <= (long)message_info.source_timestamp.nsec) << - pre_publish_time.tv_nsec << " > " << (time_t)message_info.source_timestamp.nsec; - EXPECT_NE(0u, message_info.received_timestamp.sec); - EXPECT_TRUE(pre_publish_time.tv_sec <= (time_t)message_info.received_timestamp.sec); - EXPECT_TRUE(message_info.source_timestamp.sec <= message_info.received_timestamp.sec); - EXPECT_NE(0u, message_info.received_timestamp.nsec); - EXPECT_TRUE(pre_publish_time.tv_nsec <= (long)message_info.received_timestamp.nsec); - EXPECT_TRUE(message_info.source_timestamp.nsec <= message_info.received_timestamp.nsec); + EXPECT_NE(0u, message_info.source_timestamp); + EXPECT_TRUE(pre_publish_time <= message_info.source_timestamp) << + pre_publish_time << " > " << message_info.source_timestamp; + 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.source_timestamp.sec); - EXPECT_EQ(0u, message_info.source_timestamp.nsec); - EXPECT_EQ(0u, message_info.received_timestamp.sec); - EXPECT_EQ(0u, message_info.received_timestamp.nsec); + EXPECT_EQ(0u, message_info.source_timestamp); + EXPECT_EQ(0u, message_info.received_timestamp); #endif } } From 0f63607dc5d62ebd1c5ab441017a2b0b0f6575f6 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:34:30 +0200 Subject: [PATCH 07/12] Use rcutils for cross-platform compatibility. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_subscription.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index ef5ba45fe..f1d19426e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -180,10 +180,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription // until then we will sleep for a short period of time std::this_thread::sleep_for(std::chrono::milliseconds(1000)); #ifdef RMW_TIMESTAMPS_SUPPORTED - timespec pre_publish_ts; - EXPECT_EQ(0, clock_gettime(CLOCK_REALTIME, &pre_publish_ts)) << " clock_gettime failed"; - rmw_time_point_value_t pre_publish_time = - RCUTILS_S_TO_NS(pre_publish_ts.tv_sec) + pre_publish_ts.tv_nsec; + rcl_time_point_value_t pre_publish_time; + EXPECT_EQ(0, rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; #endif { test_msgs__msg__BasicTypes msg; From cd2af2ca693bc4aec689d3d960d4f7c9efdeda39 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:43:14 +0200 Subject: [PATCH 08/12] Style fix. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index f1d19426e..26cc8bbca 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -181,7 +181,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription std::this_thread::sleep_for(std::chrono::milliseconds(1000)); #ifdef RMW_TIMESTAMPS_SUPPORTED rcl_time_point_value_t pre_publish_time; - EXPECT_EQ(0, rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; + EXPECT_EQ(0, rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; #endif { test_msgs__msg__BasicTypes msg; From a925935738cdcb63d87e40168b8888cd2ce92af1 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 20:44:24 +0200 Subject: [PATCH 09/12] More style fix. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_subscription.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 26cc8bbca..480f7a4c7 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -181,7 +181,9 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription std::this_thread::sleep_for(std::chrono::milliseconds(1000)); #ifdef RMW_TIMESTAMPS_SUPPORTED rcl_time_point_value_t pre_publish_time; - EXPECT_EQ(0, rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; + EXPECT_EQ( + RCUTILS_RET_OK, + rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; #endif { test_msgs__msg__BasicTypes msg; From 028a1c89018676791bb2ed44cf4e6136abfa6737 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 21 Apr 2020 22:32:25 +0200 Subject: [PATCH 10/12] test fastrtps_dynamic properly; also cmake linter error Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 3879d710f..cea65b791 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -205,9 +205,11 @@ function(test_target_function) LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) - if(rmw_implementation STREQUAL "rmw_fastrtps_cpp") + 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") + target_compile_definitions(test_subscription${target_suffix} + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1") else() message(STATUS "Disabling message timestamp test for ${rmw_implementation}") endif() From 3e433b530efab033f48aac1f1754e349c7b8c9c7 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 23 Apr 2020 00:50:52 +0200 Subject: [PATCH 11/12] Expect timestamps on cyclonedds as well Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index cea65b791..f7f90c493 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -206,7 +206,8 @@ function(test_target_function) 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") + rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp" OR + rmw_implementation STREQUAL "rmw_cyclonedds_cpp") message(STATUS "Enabling message timestamp test for ${rmw_implementation}") target_compile_definitions(test_subscription${target_suffix} PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1") From 9525c731688329e5a98f68fb7abb4d4b3c57a0ea Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 23 Apr 2020 22:41:40 +0200 Subject: [PATCH 12/12] Distinguish received timestamp support. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 13 +++++++++---- rcl/test/rcl/test_subscription.cpp | 4 ++++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index f7f90c493..4e0dedb16 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -206,13 +206,18 @@ function(test_target_function) 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" OR - rmw_implementation STREQUAL "rmw_cyclonedds_cpp") + 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") + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1") else() - message(STATUS "Disabling message timestamp test for ${rmw_implementation}") + 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} diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 480f7a4c7..8adcc8098 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -211,9 +211,13 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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);