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();
}