diff --git a/rcl/include/rcl/graph.h b/rcl/include/rcl/graph.h index 28b13a4f7..9309e169b 100644 --- a/rcl/include/rcl/graph.h +++ b/rcl/include/rcl/graph.h @@ -22,6 +22,7 @@ extern "C" #include #include +#include #include "rcutils/types.h" @@ -420,8 +421,8 @@ rcl_names_and_types_fini(rcl_names_and_types_t * names_and_types); * * \param[in] node the handle to the node being used to query the ROS graph * \param[in] allocator used to control allocation and deallocation of names - * \param[out] node_names struct storing discovered node names. - * \param[out] node_namesspaces struct storing discovered node namespaces. + * \param[out] node_names struct storing discovered node names + * \param[out] node_namesspaces struct storing discovered node namespaces * \return `RCL_RET_OK` if the query was successful, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ @@ -524,6 +525,126 @@ rcl_count_subscribers( const char * topic_name, size_t * count); +/// Return a list of all publishers to a topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`. + * + * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic name + * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + * ROS topic name conventions. + * In either case, the topic name should always be fully qualified. + * + * Each element in the `publishers_info` array will contain the node name, node namespace, + * topic type, gid and the qos profile of the publisher. + * It is the responsibility of the caller to ensure that `publishers_info` parameter points + * to a valid struct of type rmw_topic_endpoint_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `publishers_info`. + * Moreover, every const char * member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for + * the array inside publishers_info + * \param[in] topic_name the name of the topic in question + * \param[in] no_mangle if true, the topic_name will be expanded to its fully qualified name + * \param[out] publishers_info a struct representing a list of publisher information + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_publishers_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info); + +/// Return a list of all subscriptions to a topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`. + * + * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic name + * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + * ROS topic name conventions. + * In either case, the topic name should always be fully qualified. + * + * Each element in the `subscriptions_info` array will contain the node name, node namespace, + * topic type, gid and the qos profile of the subscription. + * It is the responsibility of the caller to ensure that `subscriptions_info` parameter points + * to a valid struct of type rmw_topic_endpoint_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `subscriptions_info`. + * Moreover, every const char * member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `subscriptions_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for + * the array inside publishers_info + * \param[in] topic_name the name of the topic in question + * \param[in] no_mangle if true, the topic_name will be expanded to its fully qualified name + * \param[out] subscriptions_info a struct representing a list of subscriptions information + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_subscriptions_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info); + /// Check if a service server is available for the given service client. /** * This function will return true for `is_available` if there is a service server diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index 331d78027..43ab9dcc2 100644 --- a/rcl/src/rcl/graph.c +++ b/rcl/src/rcl/graph.c @@ -25,9 +25,11 @@ extern "C" #include "rmw/error_handling.h" #include "rmw/get_node_info_and_types.h" #include "rmw/get_service_names_and_types.h" +#include "rmw/get_topic_endpoint_info.h" #include "rmw/get_topic_names_and_types.h" #include "rmw/names_and_types.h" #include "rmw/rmw.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" @@ -375,6 +377,90 @@ rcl_count_subscribers( return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } +typedef rmw_ret_t (* get_topic_endpoint_info_func_t)( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * info_array); + +rcl_ret_t +__rcl_get_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * info_array, + get_topic_endpoint_info_func_t get_topic_endpoint_info) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set. + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (!node_options) { + return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + rmw_error_string_t error_string; + rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(info_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "rmw_topic_endpoint_info_array_t must be zero initialized: %s,\n" + "Use rmw_get_zero_initialized_topic_endpoint_info_array", + error_string.str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rmw_ret = get_topic_endpoint_info( + rcl_node_get_rmw_handle(node), + allocator, + topic_name, + no_mangle, + info_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG(error_string.str); + } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_publishers_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info) +{ + return __rcl_get_info_by_topic( + node, + allocator, + topic_name, + no_mangle, + publishers_info, + rmw_get_publishers_info_by_topic); +} + +rcl_ret_t +rcl_get_subscriptions_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info) +{ + return __rcl_get_info_by_topic( + node, + allocator, + topic_name, + no_mangle, + subscriptions_info, + rmw_get_subscriptions_info_by_topic); +} + rcl_ret_t rcl_service_server_is_available( const rcl_node_t * node, diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 401cc3b62..f534ee5cf 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -101,6 +101,21 @@ function(test_target_function) ${AMENT_GTEST_ARGS} ) + set(AMENT_GTEST_ARGS "") + # TODO(mm318): why rmw_connext tests run much slower than rmw_fastrtps and rmw_opensplice tests + if(rmw_implementation STREQUAL "rmw_connext_cpp") + message(STATUS "Increasing test_info_by_topic${target_suffix} test timeout.") + set(AMENT_GTEST_ARGS TIMEOUT 120) + endif() + rcl_add_custom_gtest(test_info_by_topic${target_suffix} + SRCS rcl/test_info_by_topic.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_count_matched${target_suffix} SRCS rcl/test_count_matched.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 28f7cdae7..68d3403a1 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -916,8 +916,8 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME /** * Verify the number of subsystems each node should have. * - * @param node_state expected state of node - * @param remote_node_state expected state of remote node + * \param node_state expected state of node + * \param remote_node_state expected state of remote node */ void VerifySubsystemCount( const expected_node_state && node_state, diff --git a/rcl/test/rcl/test_info_by_topic.cpp b/rcl/test/rcl/test_info_by_topic.cpp new file mode 100644 index 000000000..69f0118c8 --- /dev/null +++ b/rcl/test/rcl/test_info_by_topic.cpp @@ -0,0 +1,373 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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. + +#ifndef _WIN32 +# pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#endif + +#include + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/rcl.h" + +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/error_handling.h" + +#include "test_msgs/msg/strings.h" +#include "rosidl_generator_c/string_functions.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#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 (TestInfoByTopicFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t old_context; + rcl_context_t context; + rcl_node_t old_node; + rcl_node_t node; + const char * test_graph_node_name = "test_graph_node"; + rmw_topic_endpoint_info_array_t topic_endpoint_info_array; + const char * const topic_name = "valid_topic_name"; + bool is_fastrtps; + + void SetUp() + { + is_fastrtps = (std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0); + 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->old_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->old_context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->old_node = 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, old_name, "", &this->old_context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(&this->old_context); // after this, the old_node should be invalid + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->context = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, nullptr, &init_options, &this->context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node = rcl_get_zero_initialized_node(); + const char * name = "test_graph_node"; + ret = rcl_node_init(&this->node, name, "", &this->context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + ret = rcl_node_fini(&this->old_node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&this->node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // old_context was supposed to have been shutdown already during SetUp() + if (rcl_context_is_valid(&this->old_context)) { + ret = rcl_shutdown(&this->old_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ret = rcl_context_fini(&this->old_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void assert_qos_equality(rmw_qos_profile_t qos_profile1, rmw_qos_profile_t qos_profile2) + { + EXPECT_EQ(qos_profile1.deadline.sec, qos_profile2.deadline.sec); + EXPECT_EQ(qos_profile1.deadline.nsec, qos_profile2.deadline.nsec); + EXPECT_EQ(qos_profile1.lifespan.sec, qos_profile2.lifespan.sec); + EXPECT_EQ(qos_profile1.lifespan.nsec, qos_profile2.lifespan.nsec); + EXPECT_EQ(qos_profile1.reliability, qos_profile2.reliability); + EXPECT_EQ(qos_profile1.liveliness, qos_profile2.liveliness); + EXPECT_EQ(qos_profile1.liveliness_lease_duration.sec, + qos_profile2.liveliness_lease_duration.sec); + EXPECT_EQ(qos_profile1.liveliness_lease_duration.nsec, + qos_profile2.liveliness_lease_duration.nsec); + EXPECT_EQ(qos_profile1.durability, qos_profile2.durability); + } +}; + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_node) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic(nullptr, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_node) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic(nullptr, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_invalid_node) +{ + // this->old_node is an invalid node. + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic(&this->old_node, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_invalid_node) +{ + // this->old_node is an invalid node. + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic(&this->old_node, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_allocator) +{ + const auto ret = rcl_get_publishers_info_by_topic(&this->node, nullptr, this->topic_name, + false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_allocator) +{ + const auto ret = rcl_get_subscriptions_info_by_topic(&this->node, nullptr, this->topic_name, + false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_topic) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic(&this->node, + &allocator, nullptr, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_topic) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic(&this->node, + &allocator, nullptr, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_participants) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic(&this->node, + &allocator, this->topic_name, false, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_participants) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic(&this->node, + &allocator, this->topic_name, false, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_invalid_participants) +{ + // topic_endpoint_info_array is invalid because it is expected to be zero initialized + // and the info_array variable inside it is expected to be null. + this->topic_endpoint_info_array.info_array = new rmw_topic_endpoint_info_t(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + free(this->topic_endpoint_info_array.info_array); + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic(&this->node, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_ERROR, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_invalid_participants) +{ + // topic_endpoint_info_array is invalid because it is expected to be zero initialized + // and the info_array variable inside it is expected to be null. + this->topic_endpoint_info_array.info_array = new rmw_topic_endpoint_info_t(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + free(this->topic_endpoint_info_array.info_array); + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic(&this->node, + &allocator, this->topic_name, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_ERROR, ret); +} + +TEST_F(CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_subscription_info_by_topic) +{ + // This is implemented only in fastrtps currently. + if (!is_fastrtps) { + GTEST_SKIP(); + } + rmw_qos_profile_t default_qos_profile; + default_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + default_qos_profile.depth = 0; + default_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + default_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + default_qos_profile.lifespan = {10, 0}; + default_qos_profile.deadline = {11, 0}; + default_qos_profile.liveliness_lease_duration = {20, 0}; + default_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + + rcl_ret_t ret; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = default_qos_profile; + ret = rcl_publisher_init( + &publisher, + &this->node, + ts, + this->topic_name, + &publisher_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos = default_qos_profile; + ret = rcl_subscription_init( + &subscription, + &this->node, + ts, + this->topic_name, + &subscription_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const std::string fqdn = std::string("/") + this->topic_name; + // Get publishers info by topic + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_pub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = rcl_get_publishers_info_by_topic(&this->node, + &allocator, fqdn.c_str(), false, &topic_endpoint_info_array_pub); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(topic_endpoint_info_array_pub.count, 1u) << "Expected one publisher"; + rmw_topic_endpoint_info_t topic_endpoint_info_pub = topic_endpoint_info_array_pub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_pub.node_name, this->test_graph_node_name); + EXPECT_STREQ(topic_endpoint_info_pub.node_namespace, "/"); + EXPECT_STREQ(topic_endpoint_info_pub.topic_type, "test_msgs/msg/Strings"); + assert_qos_equality(topic_endpoint_info_pub.qos_profile, default_qos_profile); + + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = rcl_get_subscriptions_info_by_topic(&this->node, + &allocator, fqdn.c_str(), false, &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(topic_endpoint_info_array_sub.count, 1u) << "Expected one subscription"; + rmw_topic_endpoint_info_t topic_endpoint_info_sub = topic_endpoint_info_array_sub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_sub.node_name, this->test_graph_node_name); + EXPECT_STREQ(topic_endpoint_info_sub.node_namespace, "/"); + EXPECT_STREQ(topic_endpoint_info_sub.topic_type, "test_msgs/msg/Strings"); + assert_qos_equality(topic_endpoint_info_sub.qos_profile, default_qos_profile); + + // clean up + rmw_ret_t rmw_ret = + rmw_topic_endpoint_info_array_fini(&topic_endpoint_info_array_pub, &allocator); + EXPECT_EQ(rmw_ret, RMW_RET_OK) << rmw_get_error_string().str; + rmw_ret = rmw_topic_endpoint_info_array_fini(&topic_endpoint_info_array_sub, &allocator); + EXPECT_EQ(rmw_ret, RMW_RET_OK) << rmw_get_error_string().str; + ret = rcl_subscription_fini(&subscription, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +}