niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add node graph api to rcl. (#333)

Browse source 
This is an omnibus rollup of all of the previous commits
from https://github.com/ros2/rcl/pull/333
This commit is contained in:
Ross Desmond 2018-12-06 16:04:29 -08:00 • committed by Jacob Perron
parent c5798e4774
commit 8d6fb9aab5
6 changed files with 603 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

163
rcl/include/rcl/graph.h
View file

@ -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.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | Yes
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Maybe [1]
* <i>[1] implementation may need to protect the data structure with a lock</i>
*
* \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.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | Yes
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Maybe [1]
* <i>[1] implementation may need to protect the data structure with a lock</i>
*
* \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.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | Yes
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Maybe [1]
* <i>[1] implementation may need to protect the data structure with a lock</i>
*
* \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.

113
rcl/src/rcl/graph.c
View file

@ -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,

14
rcl/test/CMakeLists.txt
View file

@ -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}

8
rcl/test/cmake/rcl_add_custom_gtest.cmake
View file

@ -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)

322
rcl/test/rcl/test_graph.cpp
View file

@ -26,17 +26,19 @@
#include <future>
#include <string>
#include <thread>
#include <vector>
#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 @@ public:
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 @@ public:
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.
*/
TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) {
typedef std::function<rcl_ret_t(const rcl_node_t *,
const char * node_name,
rcl_names_and_types_t *)> 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<rcl_node_t *> 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)
{
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();

2
rcl/test/rcl/test_guard_condition.cpp
View file

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