From 8d6fb9aab595a503aeea6f88f0b53bdf105c02e2 Mon Sep 17 00:00:00 2001 From: Ross Desmond <44277324+ross-desmond@users.noreply.github.com> Date: Thu, 6 Dec 2018 16:04:29 -0800 Subject: [PATCH] Add node graph api to rcl. (#333) This is an omnibus rollup of all of the previous commits from https://github.com/ros2/rcl/pull/333 --- rcl/include/rcl/graph.h | 163 +++++++++++ rcl/src/rcl/graph.c | 113 ++++++++ rcl/test/CMakeLists.txt | 14 +- rcl/test/cmake/rcl_add_custom_gtest.cmake | 8 +- rcl/test/rcl/test_graph.cpp | 322 +++++++++++++++++++++- rcl/test/rcl/test_guard_condition.cpp | 2 + 6 files changed, 603 insertions(+), 19 deletions(-) diff --git a/rcl/include/rcl/graph.h b/rcl/include/rcl/graph.h index 7ca2fcd2b..8fb9f97a3 100644 --- a/rcl/include/rcl/graph.h +++ b/rcl/include/rcl/graph.h @@ -36,6 +36,169 @@ typedef rmw_names_and_types_t rcl_names_and_types_t; #define rcl_get_zero_initialized_names_and_types rmw_get_zero_initialized_names_and_types +/// Return a list of publisher topic names and their types per node. +/** + * This function returns a list of topic names in the ROS graph for param node_name and their types. + * + * The node parameter must not be `NULL`, and must point to a valid node. + * + * The topic_names_and_types parameter must be allocated and zero initialized. + * The topic_names_and_types is the output for this function, and contains + * allocated memory. + * Therefore, it should be passed to rcl_names_and_types_fini() when + * it is no longer needed. + * Failing to do so will result in leaked memory. + * + * There may be some demangling that occurs when listing the topics from the + * middleware implementation. + * If the no_demangle argument is true, then this will be avoided and the + * topics will be returned as they appear to the middleware. + * + * \see rmw_get_topic_names_and_types for more details on no_demangle + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * 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 strings + * \param[in] no_demangle if true, list all topics without any demangling + * \param[in] node_name of the node to look up topics + * \param[in] node_namespace of the node to look up topics + * \param[out] topic_names_and_types list of topic names and their types + * \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_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_publisher_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types); + +/// Return a list of subcriber topic names and their types per node. +/** + * This function returns a list of topic names in the ROS graph for param node_name and their types. + * + * The node parameter must not be `NULL`, and must point to a valid node. + * + * The topic_names_and_types parameter must be allocated and zero initialized. + * The topic_names_and_types is the output for this function, and contains + * allocated memory. + * Therefore, it should be passed to rcl_names_and_types_fini() when + * it is no longer needed. + * Failing to do so will result in leaked memory. + * + * There may be some demangling that occurs when listing the topics from the + * middleware implementation. + * If the no_demangle argument is true, then this will be avoided and the + * topics will be returned as they appear to the middleware. + * + * \see rmw_get_topic_names_and_types for more details on no_demangle + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * 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 strings + * \param[in] no_demangle if true, list all topics without any demangling + * \param[in] node_name of the node to look up topics + * \param[in] node_namespace of the node to look up topics + * \param[out] topic_names_and_types list of topic names and their types + * \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_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_subscriber_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types); + +/// Return a list of service names and their types per node. +/** + * This function returns a list of service names in the ROS graph for param node_name and their types. + * + * The node parameter must not be `NULL`, and must point to a valid node. + * + * The topic_names_and_types parameter must be allocated and zero initialized. + * The topic_names_and_types is the output for this function, and contains + * allocated memory. + * Therefore, it should be passed to rcl_names_and_types_fini() when + * it is no longer needed. + * Failing to do so will result in leaked memory. + * + * There may be some demangling that occurs when listing the topics from the + * middleware implementation. + * If the no_demangle argument is true, then this will be avoided and the + * topics will be returned as they appear to the middleware. + * + * \see rmw_get_topic_names_and_types for more details on no_demangle + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * 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 strings + * \param[in] node_name of the node to look up topics + * \param[in] node_namespace of the node to look up topics + * \param[out] service_names_and_types list of service names and their types + * \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_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_service_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types); + /// Return a list of topic names and their types. /** * This function returns a list of topic names in the ROS graph and their types. diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index cc89fca86..d41df2529 100644 --- a/rcl/src/rcl/graph.c +++ b/rcl/src/rcl/graph.c @@ -22,6 +22,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcutils/allocator.h" #include "rcutils/types.h" +#include "rmw/get_node_info_and_types.h" #include "rmw/get_service_names_and_types.h" #include "rmw/get_topic_names_and_types.h" #include "rmw/names_and_types.h" @@ -29,6 +30,118 @@ extern "C" #include "./common.h" +rcl_ret_t +rcl_get_publisher_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_publisher_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + no_demangle, + topic_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_subscriber_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_subscriber_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + no_demangle, + topic_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_service_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_service_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + service_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + rcl_ret_t rcl_get_topic_names_and_types( const rcl_node_t * node, diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 0ac72b187..8505c7363 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -85,13 +85,15 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} ) - set(SKIP_TEST "") + set(AMENT_GTEST_ARGS "") # TODO(wjwwood): remove this when the graph API works properly for connext dynamic - if( - rmw_implementation STREQUAL "rmw_connext_dynamic_cpp" - ) + if(rmw_implementation STREQUAL "rmw_connext_dynamic_cpp") message(STATUS "Skipping test_graph${target_suffix} test.") - set(SKIP_TEST "SKIP_TEST") + set(AMENT_GTEST_ARGS "SKIP_TEST") + # TODO(mm318): why rmw_connext tests run much slower than rmw_fastrtps and rmw_opensplice tests + elseif(rmw_implementation STREQUAL "rmw_connext_cpp") + message(STATUS "Increasing test_graph${target_suffix} test timeout.") + set(AMENT_GTEST_ARGS TIMEOUT 90) endif() rcl_add_custom_gtest(test_graph${target_suffix} SRCS rcl/test_graph.cpp @@ -100,7 +102,7 @@ function(test_target_function) APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "test_msgs" - ${SKIP_TEST} + ${AMENT_GTEST_ARGS} ) rcl_add_custom_gtest(test_count_matched${target_suffix} diff --git a/rcl/test/cmake/rcl_add_custom_gtest.cmake b/rcl/test/cmake/rcl_add_custom_gtest.cmake index 773c8c47f..0ebe4bdea 100644 --- a/rcl/test/cmake/rcl_add_custom_gtest.cmake +++ b/rcl/test/cmake/rcl_add_custom_gtest.cmake @@ -49,7 +49,7 @@ set(rcl_add_custom_gtest_INCLUDED TRUE) macro(rcl_add_custom_gtest target) cmake_parse_arguments(_ARG "SKIP_TEST;TRACE" - "" + "TIMEOUT" "SRCS;ENV;APPEND_ENV;APPEND_LIBRARY_DIRS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES" ${ARGN}) if(_ARG_UNPARSED_ARGUMENTS) @@ -69,9 +69,13 @@ macro(rcl_add_custom_gtest target) else() set(_ARG_SKIP_TEST "") endif() + if(_ARG_TIMEOUT) + set(_ARG_TIMEOUT "TIMEOUT" ${_ARG_TIMEOUT}) + endif() # Pass args along to ament_add_gtest(). - ament_add_gtest(${target} ${_ARG_SRCS} ${_ARG_ENV} ${_ARG_APPEND_ENV} ${_ARG_APPEND_LIBRARY_DIRS} ${_ARG_SKIP_TEST}) + ament_add_gtest(${target} ${_ARG_SRCS} ${_ARG_ENV} ${_ARG_APPEND_ENV} ${_ARG_APPEND_LIBRARY_DIRS} + ${_ARG_SKIP_TEST} ${_ARG_TIMEOUT}) # Check if the target was actually created. if(TARGET ${target}) if(_ARG_TRACE) diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index c7e2610da..d86f34be0 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -26,17 +26,19 @@ #include #include #include +#include -#include "rcl/rcl.h" +#include "rcl/error_handling.h" #include "rcl/graph.h" +#include "rcl/rcl.h" #include "rcutils/logging_macros.h" +#include "rcutils/logging.h" #include "test_msgs/msg/primitives.h" #include "test_msgs/srv/primitives.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rcl/error_handling.h" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -48,6 +50,9 @@ bool is_connext = std::string(rmw_get_implementation_identifier()).find("rmw_connext") == 0; +bool is_opensplice = + std::string(rmw_get_implementation_identifier()).find("rmw_opensplice") == 0; + class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: @@ -56,6 +61,8 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test rcl_node_t * old_node_ptr; rcl_node_t * node_ptr; rcl_wait_set_t * wait_set_ptr; + const char * test_graph_node_name = "test_graph_node"; + void SetUp() { rcl_ret_t ret; @@ -92,6 +99,7 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test 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()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } void TearDown() @@ -246,7 +254,7 @@ check_graph_state( bool expected_in_tnat, size_t number_of_tries) { - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.", expected_publisher_count, expected_subscriber_count, @@ -306,7 +314,7 @@ check_graph_state( } 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); + ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, nullptr); 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, @@ -329,13 +337,305 @@ check_graph_state( } } -/* Test graph queries with a hand crafted graph. +/** + * Type define a get topics function. + */ +typedef std::function GetTopicsFunc; + +/** + * Expect a certain number of topics on a given subsystem. + */ +void expect_topics_types( + const rcl_node_t * node, + const GetTopicsFunc & func, + size_t num_topics, + const char * topic_name, + bool expect, + bool & is_success) +{ + rcl_ret_t ret; + rcl_names_and_types_t nat{}; + nat = rcl_get_zero_initialized_names_and_types(); + ret = func(node, topic_name, &nat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + is_success &= num_topics == nat.names.size; + if (expect) { + EXPECT_EQ(num_topics, nat.names.size); + } else { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expected topics %d, actual topics %d", num_topics, + nat.names.size); + } + ret = rcl_names_and_types_fini(&nat); + ASSERT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); +} + +/** + * Expected state of a node. + */ +struct expected_node_state +{ + size_t publishers; + size_t subscribers; + size_t services; +}; + +/** + * Extend the TestGraphFixture with a multi node fixture for node discovery and node-graph perspective. + */ +class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) +{ +public: + const char * remote_node_name = "remote_graph_node"; + std::string topic_name = "/test_node_info_functions__"; + rcl_node_t * remote_node_ptr; + rcl_allocator_t allocator = rcl_get_default_allocator(); + GetTopicsFunc sub_func, pub_func, service_func; + rcl_context_t * remote_context_ptr; + + void SetUp() override + { + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) ::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; + }); + + remote_node_ptr = new rcl_node_t; + *remote_node_ptr = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + + this->remote_context_ptr = new rcl_context_t; + *this->remote_context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->remote_context_ptr); + + ret = rcl_node_init(remote_node_ptr, remote_node_name, "", this->remote_context_ptr, + &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + sub_func = std::bind(rcl_get_subscriber_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + false, + std::placeholders::_2, + "/", + std::placeholders::_3); + pub_func = std::bind(rcl_get_publisher_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + false, + std::placeholders::_2, + "/", + std::placeholders::_3); + service_func = std::bind(rcl_get_service_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + std::placeholders::_2, + "/", + std::placeholders::_3); + WaitForAllNodesAlive(); + } + + void TearDown() override + { + rcl_ret_t ret; + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) ::TearDown(); + ret = rcl_node_fini(this->remote_node_ptr); + + delete this->remote_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Tearing down class"); + + ret = rcl_shutdown(this->remote_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->remote_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->remote_context_ptr; + } + + void WaitForAllNodesAlive() + { + rcl_ret_t ret; + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + ret = rcutils_string_array_fini(&node_names); + ASSERT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces); + ASSERT_EQ(RCUTILS_RET_OK, ret); + }); + // wait for all 3 nodes to be discovered: remote_node, old_node, node + size_t attempts = 0; + size_t max_attempts = 4; + while (node_names.size < 3) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + ret = rcl_get_node_names(this->remote_node_ptr, allocator, &node_names, &node_namespaces); + attempts++; + ASSERT_LE(attempts, max_attempts) << "Unable to attain all required nodes"; + } + } + + /** + * 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 + */ + void VerifySubsystemCount( + const expected_node_state && node_state, + const expected_node_state && remote_node_state) const + { + std::vector node_vec; + node_vec.push_back(this->node_ptr); + if (!(is_opensplice || is_connext)) { + // TODO(ross-desmond): opensplice and connext cannot discover data about + // the current node due to their implementations of Simple Discovery + // Protocol. Should be fixed later. + node_vec.push_back(this->remote_node_ptr); + } + size_t attempts = 20; + bool is_expect = false; + rcl_ret_t ret; + + for (size_t i = 0; i < attempts; ++i) { + if (attempts - 1 == i) {is_expect = true;} + bool is_success = true; + // verify each node contains the same node graph. + for (auto node : node_vec) { + if (!(is_opensplice || is_connext)) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking subscribers from node"); + expect_topics_types(node, sub_func, node_state.subscribers, + test_graph_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking services from node"); + expect_topics_types(node, service_func, node_state.services, + test_graph_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking publishers from node"); + expect_topics_types(node, pub_func, node_state.publishers, + test_graph_node_name, is_expect, is_success); + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking subscribers from remote node"); + expect_topics_types(node, sub_func, remote_node_state.subscribers, + this->remote_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking publishers from remote node"); + expect_topics_types(node, pub_func, remote_node_state.publishers, + this->remote_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking services from remote node"); + expect_topics_types(node, service_func, remote_node_state.services, + this->remote_node_name, is_expect, is_success); + if (!is_success) { + 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, rcl_node_get_graph_guard_condition( + node), NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); + RCUTILS_LOG_DEBUG_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_DEBUG_NAMED(ROS_PACKAGE_NAME, "timeout"); + continue; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "change occurred"); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + break; + } + } + if (is_success) { + break; + } + } + } +}; + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_subscriptions) +{ + rcl_ret_t ret; + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives); + // Create two subscribers. + 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, this->topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops2 = rcl_subscription_get_default_options(); + ret = + rcl_subscription_init(&sub2, this->remote_node_ptr, ts, this->topic_name.c_str(), &sub_ops2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + VerifySubsystemCount(expected_node_state{0, 1, 0}, expected_node_state{0, 1, 0}); + + // Destroy the node's subscriber + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 1, 0}); + + // Destroy the remote node's subdscriber + ret = rcl_subscription_fini(&sub2, this->remote_node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0}); +} + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_publishers) +{ + rcl_ret_t ret; + // Now create a publisher on "topic_name" and check that it is seen. + 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, this->topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{0, 0, 0}); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Destroyed publisher"); + // Destroy the publisher. + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0}); +} + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_services) +{ + rcl_ret_t ret; + const char * service_name = "test_service"; + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + auto ts1 = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, Primitives); + ret = rcl_service_init(&service, this->node_ptr, ts1, service_name, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{0, 0, 1}, expected_node_state{0, 0, 0}); + + // Destroy service. + ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0}); +} + +/* + * Test graph queries with a hand crafted graph. */ -TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) { +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) +{ std::string topic_name("/test_graph_query_functions__"); std::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch(); topic_name += std::to_string(now.count()); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Using topic name: %s", topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Using topic name: %s", topic_name.c_str()); rcl_ret_t ret; const rcl_guard_condition_t * graph_guard_condition = rcl_node_get_graph_guard_condition(this->node_ptr); @@ -345,10 +645,10 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio this->wait_set_ptr, graph_guard_condition, topic_name, - 0, // expected publishers on topic - 0, // expected subscribers on topic - false, // topic expected in graph - 9); // number of retries + 0, // expected publishers on topic + 0, // expected subscribers on topic + false, // topic expected in graph + 9); // number of retries // Now create a publisher on "topic_name" and check that it is seen. rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp index 312ffb770..6612d0549 100644 --- a/rcl/test/rcl/test_guard_condition.cpp +++ b/rcl/test/rcl/test_guard_condition.cpp @@ -203,6 +203,8 @@ TEST_F( rcl_reset_error(); ret = rcl_guard_condition_fini(&guard_condition); EXPECT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); ret = rcl_guard_condition_fini(&guard_condition); EXPECT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); }