From a2ac13455703cfb2aa53faf3319e4d014bf51da7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 8 Nov 2018 15:25:35 -0600 Subject: [PATCH 1/9] Methods to retrieve matched counts on pub/sub. --- rcl/include/rcl/publisher.h | 7 ++ rcl/include/rcl/subscription.h | 7 ++ rcl/src/rcl/publisher.c | 23 ++++ rcl/src/rcl/subscription.c | 22 ++++ rcl/test/CMakeLists.txt | 9 ++ rcl/test/rcl/test_count_matched.cpp | 180 ++++++++++++++++++++++++++++ 6 files changed, 248 insertions(+) create mode 100644 rcl/test/rcl/test_count_matched.cpp diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 722289547..f4c5b2c89 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -397,6 +397,13 @@ RCL_PUBLIC bool rcl_publisher_is_valid(const rcl_publisher_t * publisher); +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_ret_t +rcl_publisher_get_subscription_count( + const rcl_publisher_t * publisher, + size_t * subscription_count); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 7ea1e7c6d..03ac2f451 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -407,6 +407,13 @@ RCL_PUBLIC bool rcl_subscription_is_valid(const rcl_subscription_t * subscription); +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_ret_t +rcl_subscription_get_publisher_count( + const rcl_subscription_t * subscription, + size_t * publisher_count); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 8c0efd24a..1328f7cf3 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -302,6 +302,29 @@ rcl_publisher_is_valid(const rcl_publisher_t * publisher) return true; } +rmw_ret_t +rcl_publisher_get_subscription_count( + const rcl_publisher_t * publisher, + size_t * subscription_count) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher pointer is invalid", + return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(subscription_count, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl, "publisher's implementation is invalid", + return RCL_RET_ERROR); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, "publisher's rmw handle is invalid", + return RCL_RET_ERROR); + + rmw_ret_t ret = rmw_count_matched_subscriptions(publisher->impl->rmw_handle, subscription_count); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 4c337d194..2b7178e49 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -337,6 +337,28 @@ rcl_subscription_is_valid(const rcl_subscription_t * subscription) return true; } +rmw_ret_t +rcl_subscription_get_publisher_count( + const rcl_subscription_t * subscription, + size_t * publisher_count) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(subscription, "subscription pointer is invalid", + return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(publisher_count, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription's implementation is invalid", return RCL_RET_ERROR); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl->rmw_handle, "subscription's rmw handle is invalid", return RCL_RET_ERROR); + + rmw_ret_t ret = rmw_count_matched_publishers(subscription->impl->rmw_handle, publisher_count); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index b9c7f3430..50ce76904 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -102,6 +102,15 @@ function(test_target_function) ${SKIP_TEST} ) + rcl_add_custom_gtest(test_count_matched${target_suffix} + SRCS rcl/test_count_matched.cpp + INCLUDE_DIRS ${osrf_testing_tools_cpp_INCLUDE_DIRS} + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "test_msgs" + ) + rcl_add_custom_gtest(test_rcl${target_suffix} SRCS rcl/test_rcl.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp new file mode 100644 index 000000000..278654a02 --- /dev/null +++ b/rcl/test/rcl/test_count_matched.cpp @@ -0,0 +1,180 @@ +// Copyright 2016 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 "rcl/rcl.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rcutils/logging_macros.h" + +#include "test_msgs/msg/primitives.h" +#include "test_msgs/srv/primitives.h" + +#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 (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_node_t * old_node_ptr; + rcl_node_t * node_ptr; + rcl_wait_set_t * wait_set_ptr; + void SetUp() + { + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->old_node_ptr = new rcl_node_t; + *this->old_node_ptr = rcl_get_zero_initialized_node(); + const char * old_name = "old_node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->old_node_ptr, old_name, "", &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(); // after this, the old_node_ptr should be invalid + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + 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_graph_node"; + ret = rcl_node_init(this->node_ptr, name, "", &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->wait_set_ptr = new rcl_wait_set_t; + *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(this->wait_set_ptr, 0, 1, 0, 0, 0, rcl_get_default_allocator()); + } + + void TearDown() + { + rcl_ret_t ret; + ret = rcl_node_fini(this->old_node_ptr); + delete this->old_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_fini(this->wait_set_ptr); + delete this->wait_set_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + 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(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + + +TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_functions) { + std::string topic_name("/test_count_matched_functions__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + { + size_t subscription_count; + ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(0u, subscription_count); + } + + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + { + size_t subscription_count; + ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(1u, subscription_count); + } + + { + size_t publisher_count; + ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(1u, publisher_count); + } + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub2, this->node_ptr, ts, topic_name.c_str(), &sub2_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + { + size_t subscription_count; + ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(2u, subscription_count); + } + + { + size_t publisher_count; + ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(1u, publisher_count); + } + + { + size_t publisher_count; + ret = rcl_subscription_get_publisher_count(&sub2, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(1u, publisher_count); + } + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + { + size_t publisher_count; + ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(0u, publisher_count); + } + + { + size_t publisher_count; + ret = rcl_subscription_get_publisher_count(&sub2, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(0u, publisher_count); + } +} From 8bacb009c5102e7772ab7f3493c5afde2f16f78b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 26 Nov 2018 13:01:26 -0600 Subject: [PATCH 2/9] Add documentation. --- rcl/include/rcl/subscription.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 03ac2f451..6468cbd9f 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -407,6 +407,26 @@ RCL_PUBLIC bool rcl_subscription_is_valid(const rcl_subscription_t * subscription); +/// Get the number of publishers matched to a subscription. +/** + * Used to get the internal count of publishers matched to a subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] only if the underlying rmw doesn't make use of this feature + * + * \param[in] subscription pointer to the rcl subscription + * \param[out] publisher_count number of matched publishers + * \return `RCL_RET_OK` if the message was published, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ RCL_PUBLIC RCL_WARN_UNUSED rmw_ret_t From f3c7ada43534c24de51d0c456ac9fddf82aaa7e4 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 26 Nov 2018 22:36:53 -0600 Subject: [PATCH 3/9] Use simplified error checking. --- rcl/src/rcl/publisher.c | 13 +++++-------- rcl/src/rcl/subscription.c | 13 +++++-------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 1328f7cf3..cee129ff6 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -307,16 +307,13 @@ rcl_publisher_get_subscription_count( const rcl_publisher_t * publisher, size_t * subscription_count) { - RCL_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher pointer is invalid", - return RCL_RET_INVALID_ARGUMENT); + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; + } RCL_CHECK_ARGUMENT_FOR_NULL(subscription_count, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl, "publisher's implementation is invalid", - return RCL_RET_ERROR); - RCL_CHECK_FOR_NULL_WITH_MSG( - publisher->impl->rmw_handle, "publisher's rmw handle is invalid", - return RCL_RET_ERROR); - rmw_ret_t ret = rmw_count_matched_subscriptions(publisher->impl->rmw_handle, subscription_count); + rmw_ret_t ret = rmw_publisher_count_matched_subscriptions(publisher->impl->rmw_handle, + subscription_count); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 2b7178e49..76f42a6b1 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -342,15 +342,12 @@ rcl_subscription_get_publisher_count( const rcl_subscription_t * subscription, size_t * publisher_count) { - RCL_CHECK_FOR_NULL_WITH_MSG(subscription, "subscription pointer is invalid", - return RCL_RET_INVALID_ARGUMENT); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } RCL_CHECK_ARGUMENT_FOR_NULL(publisher_count, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG( - subscription->impl, "subscription's implementation is invalid", return RCL_RET_ERROR); - RCL_CHECK_FOR_NULL_WITH_MSG( - subscription->impl->rmw_handle, "subscription's rmw handle is invalid", return RCL_RET_ERROR); - - rmw_ret_t ret = rmw_count_matched_publishers(subscription->impl->rmw_handle, publisher_count); + rmw_ret_t ret = rmw_subscription_count_matched_publishers(subscription->impl->rmw_handle, + publisher_count); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); From b06f0ef2d6718690da8db46c8478d258c4d52855 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 08:58:53 -0600 Subject: [PATCH 4/9] Add sleep for opensplice and connext. --- rcl/test/rcl/test_count_matched.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index 278654a02..f5266ebc8 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include +#include #include +#include #include "rcl/rcl.h" #include "rcl/publisher.h" @@ -112,6 +114,10 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); + // This sleep is currently needed to allow opensplice and connext to correctly fire + // the on_publication_matched/on_subscription_matched functions. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + { size_t subscription_count; ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); @@ -134,6 +140,10 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); + // This sleep is currently needed to allow opensplice and connext to correctly fire + // the on_publication_matched/on_subscription_matched functions. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + { size_t subscription_count; ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); @@ -162,6 +172,10 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); + // This sleep is currently needed to allow opensplice and connext to correctly fire + // the on_publication_matched/on_subscription_matched functions. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + { size_t publisher_count; ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); From ab3988cb2df2e39ea23bdca83bb3be2ba92ed8cf Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 09:19:55 -0600 Subject: [PATCH 5/9] Fix docs. --- rcl/include/rcl/publisher.h | 20 ++++++++++++++++++++ rcl/include/rcl/subscription.h | 2 +- rcl/test/rcl/test_count_matched.cpp | 2 +- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index f4c5b2c89..d6a829af5 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -397,6 +397,26 @@ RCL_PUBLIC bool rcl_publisher_is_valid(const rcl_publisher_t * publisher); +/// Get the number of subscriptions matched to a publisher. +/** + * Used to get the internal count of subscriptions matched to a publisher. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] only if the underlying rmw doesn't make use of this feature + * + * \param[in] publisher pointer to the rcl publisher + * \param[out] subscription_count number of matched subscriptions + * \return `RCL_RET_OK` if the count was retrieved, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ RCL_PUBLIC RCL_WARN_UNUSED rmw_ret_t diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 6468cbd9f..306117578 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -422,7 +422,7 @@ rcl_subscription_is_valid(const rcl_subscription_t * subscription); * * \param[in] subscription pointer to the rcl subscription * \param[out] publisher_count number of matched publishers - * \return `RCL_RET_OK` if the message was published, or + * \return `RCL_RET_OK` if the count was retrieved, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index f5266ebc8..ce30e10d4 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2018 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. From 297ed6089408ac008b4e38b245df0a2e83f42214 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 15:53:41 -0600 Subject: [PATCH 6/9] Address reviewer feedback. --- rcl/src/rcl/publisher.c | 3 ++- rcl/src/rcl/subscription.c | 3 ++- rcl/test/rcl/test_count_matched.cpp | 25 +------------------------ 3 files changed, 5 insertions(+), 26 deletions(-) diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index cee129ff6..33308feca 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -22,6 +22,7 @@ extern "C" #include #include +#include "common.h" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/expand_topic_name.h" @@ -317,7 +318,7 @@ rcl_publisher_get_subscription_count( if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } return RCL_RET_OK; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 76f42a6b1..93578eb4a 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -21,6 +21,7 @@ extern "C" #include +#include "common.h" #include "rcl/error_handling.h" #include "rcl/expand_topic_name.h" #include "rcl/remap.h" @@ -351,7 +352,7 @@ rcl_subscription_get_publisher_count( if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } return RCL_RET_OK; } diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index ce30e10d4..21c7379da 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -38,47 +38,25 @@ class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: - rcl_node_t * old_node_ptr; rcl_node_t * node_ptr; rcl_wait_set_t * wait_set_ptr; void SetUp() { rcl_ret_t ret; - ret = rcl_init(0, nullptr, rcl_get_default_allocator()); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - this->old_node_ptr = new rcl_node_t; - *this->old_node_ptr = rcl_get_zero_initialized_node(); - const char * old_name = "old_node_name"; rcl_node_options_t node_options = rcl_node_get_default_options(); - ret = rcl_node_init(this->old_node_ptr, old_name, "", &node_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_shutdown(); // after this, the old_node_ptr should be invalid - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_init(0, nullptr, rcl_get_default_allocator()); 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_graph_node"; + const char * name = "test_count_node"; ret = rcl_node_init(this->node_ptr, name, "", &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - this->wait_set_ptr = new rcl_wait_set_t; - *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init(this->wait_set_ptr, 0, 1, 0, 0, 0, rcl_get_default_allocator()); } void TearDown() { rcl_ret_t ret; - ret = rcl_node_fini(this->old_node_ptr); - delete this->old_node_ptr; - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - ret = rcl_wait_set_fini(this->wait_set_ptr); - delete this->wait_set_ptr; - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_node_fini(this->node_ptr); delete this->node_ptr; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -88,7 +66,6 @@ class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test } }; - TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_functions) { std::string topic_name("/test_count_matched_functions__"); rcl_ret_t ret; From 3087ec2bda38fc29ecd5df5d61a28c8e2e008a6a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 18:29:52 -0600 Subject: [PATCH 7/9] Wait on guard condition rather than static timeout. --- rcl/test/rcl/test_count_matched.cpp | 150 +++++++++++++++------------- 1 file changed, 78 insertions(+), 72 deletions(-) diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index 21c7379da..d9578a60d 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -35,6 +35,66 @@ # define CLASSNAME(NAME, SUFFIX) NAME #endif +void check_state( + rcl_wait_set_t * wait_set_ptr, + rcl_publisher_t * publisher, + rcl_subscription_t * subscriber, + const rcl_guard_condition_t * graph_guard_condition, + size_t expected_subscriber_count, + size_t expected_publisher_count, + size_t number_of_tries) +{ + size_t subscriber_count = -1; + size_t publisher_count = -1; + + rcl_ret_t ret; + + for (size_t i = 0; i < number_of_tries; ++i) { + if (publisher) { + ret = rcl_publisher_get_subscription_count(publisher, &subscriber_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if (subscriber) { + ret = rcl_subscription_get_publisher_count(subscriber, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if ( + expected_publisher_count == publisher_count && + expected_subscriber_count == subscriber_count) + { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!"); + break; + } + + if ((i + 1) == number_of_tries) { + // Don't wait for the graph to change on the last loop because we won't check again. + continue; + } + + ret = rcl_wait_set_clear(wait_set_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, + " state wrong, waiting up to '%s' nanoseconds for graph changes... ", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout"); + continue; + } + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "change occurred"); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + EXPECT_EQ(expected_publisher_count, publisher_count); + EXPECT_EQ(expected_subscriber_count, subscriber_count); +} + class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: @@ -52,11 +112,20 @@ class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test const char * name = "test_count_node"; ret = rcl_node_init(this->node_ptr, name, "", &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->wait_set_ptr = new rcl_wait_set_t; + *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(this->wait_set_ptr, 0, 1, 0, 0, 0, rcl_get_default_allocator()); } void TearDown() { rcl_ret_t ret; + + ret = rcl_wait_set_fini(this->wait_set_ptr); + delete this->wait_set_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->node_ptr); delete this->node_ptr; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -77,13 +146,10 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - { - size_t subscription_count; - ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(0u, subscription_count); - } + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(this->node_ptr); + + check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); @@ -91,25 +157,7 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - // This sleep is currently needed to allow opensplice and connext to correctly fire - // the on_publication_matched/on_subscription_matched functions. - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - { - size_t subscription_count; - ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(1u, subscription_count); - } - - { - size_t publisher_count; - ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(1u, publisher_count); - } + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 1, 1, 9); rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); @@ -117,55 +165,13 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - // This sleep is currently needed to allow opensplice and connext to correctly fire - // the on_publication_matched/on_subscription_matched functions. - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - { - size_t subscription_count; - ret = rcl_publisher_get_subscription_count(&pub, &subscription_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(2u, subscription_count); - } - - { - size_t publisher_count; - ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(1u, publisher_count); - } - - { - size_t publisher_count; - ret = rcl_subscription_get_publisher_count(&sub2, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(1u, publisher_count); - } + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 2, 1, 9); + check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 2, 1, 9); ret = rcl_publisher_fini(&pub, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - // This sleep is currently needed to allow opensplice and connext to correctly fire - // the on_publication_matched/on_subscription_matched functions. - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - { - size_t publisher_count; - ret = rcl_subscription_get_publisher_count(&sub, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(0u, publisher_count); - } - - { - size_t publisher_count; - ret = rcl_subscription_get_publisher_count(&sub2, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - EXPECT_EQ(0u, publisher_count); - } + check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); + check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); } From 503825bcd54ebefe56b05820ac06c2531f659a7b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 19:28:56 -0600 Subject: [PATCH 8/9] Attempt to force mismatched QoS settings. --- rcl/test/rcl/test_count_matched.cpp | 62 +++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index d9578a60d..029065522 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -175,3 +175,65 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_funct check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); } + +TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), + test_count_matched_functions_mismatched_qos) { + std::string topic_name("/test_count_matched_functions_mismatched_qos__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + + rcl_publisher_options_t pub_ops; + pub_ops.qos = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 10, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + false + }; + pub_ops.allocator = rcl_get_default_allocator(); + + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(this->node_ptr); + + check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); + + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + + rcl_subscription_options_t sub_ops; + sub_ops.qos = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 10, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + false + }; + sub_ops.allocator = rcl_get_default_allocator(); + + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 1, 1, 9); + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub2, this->node_ptr, ts, topic_name.c_str(), &sub2_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 2, 1, 9); + check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 2, 1, 9); + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); + check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); +} From 52c2328bf9ed46ec28165096beb7dec2b03a3e94 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Nov 2018 20:03:13 -0600 Subject: [PATCH 9/9] Fix mismatched QoS test. --- rcl/src/rcl/publisher.c | 2 +- rcl/src/rcl/subscription.c | 2 +- rcl/test/rcl/test_count_matched.cpp | 39 +++++++++++++---------------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 33308feca..c3d695251 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -22,7 +22,7 @@ extern "C" #include #include -#include "common.h" +#include "./common.h" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/expand_topic_name.h" diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 93578eb4a..a78bd6812 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -21,7 +21,7 @@ extern "C" #include -#include "common.h" +#include "./common.h" #include "rcl/error_handling.h" #include "rcl/expand_topic_name.h" #include "rcl/remap.h" diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index 029065522..dbfdfb67f 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -184,18 +184,16 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); rcl_publisher_options_t pub_ops; - pub_ops.qos = { - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 10, - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - false - }; + pub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + pub_ops.qos.depth = 10; + pub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + pub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + pub_ops.qos.avoid_ros_namespace_conventions = false; pub_ops.allocator = rcl_get_default_allocator(); auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives); ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); const rcl_guard_condition_t * graph_guard_condition = @@ -206,20 +204,19 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub_ops; - sub_ops.qos = { - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 10, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - false - }; + sub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + sub_ops.qos.depth = 10; + sub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + sub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + sub_ops.qos.avoid_ros_namespace_conventions = false; sub_ops.allocator = rcl_get_default_allocator(); ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 1, 1, 9); + // Expect that no publishers or subscribers should be matched due to qos. + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); @@ -227,13 +224,11 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 2, 1, 9); - check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 2, 1, 9); + // Even multiple subscribers should not match + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); + check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 0, 0, 9); ret = rcl_publisher_fini(&pub, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - - check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); - check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); }