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

add graph functions to support wait_for_service (#57)

Browse source 
* add graph functions to support wait_for_service

* is_ready -> is_available

* add function to create rcl_guard_condition from an existing rmw_guard_condition

* create graph guard condition in node automatically

* tests for graph API

* style fixup

* remove debug stuff from test_graph

* only enable test_graph for OpenSplice

* fixup

* add rcl_node_is_valid function

* add tests for new graph API functions

* fixup

* reorder the destruction of node vs waitset

this also addressed the segfault issues, but the
proper fix was committed to rmw_opensplice_cpp
while this ordering is not strictly necessary it
is slightly more correct

* disable missing-field-intializers warning

it's too strict and will be relaxed in newer gcc's

* use to_string since type changes with OS

* use more specific gtest assert to avoid warning

* added a secondary check in the test_graph.test_rcl_service_server_is_available test

* uncrustify

* doc fixup

* condition pragma on not _WIN32
This commit is contained in:
William Woodall 2016-05-28 15:14:38 -07:00
parent 2811eb1312
commit 818b15463b
11 changed files with 1088 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

1
rcl/CMakeLists.txt
View file

@ -20,6 +20,7 @@ set(${PROJECT_NAME}_sources
src/rcl/allocator.c src/rcl/allocator.c
src/rcl/client.c src/rcl/client.c
src/rcl/common.c src/rcl/common.c
src/rcl/graph.c
src/rcl/guard_condition.c src/rcl/guard_condition.c
src/rcl/node.c src/rcl/node.c
src/rcl/publisher.c src/rcl/publisher.c

188
rcl/include/rcl/graph.h Normal file
View file

@ -0,0 +1,188 @@
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCL__GRAPH_H_
#define RCL__GRAPH_H_
#if __cplusplus
extern "C"
{
#endif
#include <rmw/rmw.h>
#include <rmw/types.h>
#include "rosidl_generator_c/service_type_support.h"
#include "rcl/macros.h"
#include "rcl/client.h"
#include "rcl/node.h"
#include "rcl/visibility_control.h"
typedef rmw_topic_names_and_types_t rcl_topic_names_and_types_t;
/// Return a list of topic names and their types.
/* This function returns a list of topic names in the ROS graph and their types.
*
* The node parameter must not be NULL, and must point to a valid node.
*
* The topic_names_and_types parameter must not be NULL, and must point to an
* already allocated rcl_topic_names_and_types_t struct.
* The topic_names_and_types is the output for this function, and contains
* allocated memory.
* Therefore, it should be passed to rcl_destroy_topic_names_and_types() when
* it is no longer needed.
* Failing to do so will result in leaked memory.
*
* This function does manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \param[in] node the handle to the node being used to query the ROS graph
* \param[out] topic_names_and_types list of topic names and their types
* \return RCL_RET_OK if the query was successful, or
* RCL_RET_NODE_INVALID if the node is invalid, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_get_topic_names_and_types(
const rcl_node_t * node,
rcl_topic_names_and_types_t * topic_names_and_types);
/// Destroy a struct which was previously given to rcl_get_topic_names_and_types.
/* The topic_names_and_types parameter must not be NULL, and must point to an
* already allocated rcl_topic_names_and_types_t struct that was previously
* passed to a successful rcl_get_topic_names_and_types() call.
*
* This function does manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \param[inout] topic_names_and_types struct to be destroyed
* \return RCL_RET_OK if successful, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_destroy_topic_names_and_types(
rcl_topic_names_and_types_t * topic_names_and_types);
/// Return the number of publishers on a given topic.
/* This function returns the number of publishers on a given topic.
*
* The node parameter must not be NULL, and must point to a valid node.
*
* The topic_name parameter must not be NULL, and must not be an empty string.
* It should also follow the topic name rules.
* \TODO(wjwwood): link to the topic name rules.
*
* The count parameter must not be NULL, and must point to a valid bool.
* The count parameter is the output for this function and will be set.
*
* This function may manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \param[in] node the handle to the node being used to query the ROS graph
* \param[in] topic_name the name of the topic in question
* \param[out] count number of publishers on the given topic
* \return RCL_RET_OK if the query was successful, or
* RCL_RET_NODE_INVALID if the node is invalid, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_count_publishers(
const rcl_node_t * node,
const char * topic_name,
size_t * count);
/// Return the number of subscriptions on a given topic.
/* This function returns the number of subscriptions on a given topic.
*
* The node parameter must not be NULL, and must point to a valid node.
*
* The topic_name parameter must not be NULL, and must not be an empty string.
* It should also follow the topic name rules.
* \TODO(wjwwood): link to the topic name rules.
*
* The count parameter must not be NULL, and must point to a valid bool.
* The count parameter is the output for this function and will be set.
*
* This function may manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \param[in] node the handle to the node being used to query the ROS graph
* \param[in] topic_name the name of the topic in question
* \param[out] count number of subscriptions on the given topic
* \return RCL_RET_OK if the query was successful, or
* RCL_RET_NODE_INVALID if the node is invalid, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_count_subscribers(
const rcl_node_t * node,
const char * topic_name,
size_t * count);
/// 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
* available for the given client.
*
* The node parameter must not be NULL, and must point to a valid node.
*
* The client parameter must not be NULL, and must point to a valid client.
*
* The given client and node must match, i.e. the client must have been created
* using the given node.
*
* The is_available parameter must not be NULL, and must point a bool variable.
* The result of the check will be stored in the is_available parameter.
*
* This function does manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \param[in] node the handle to the node being used to query the ROS graph
* \param[in] client the handle to the service client being queried
* \param[out] is_available set to true if there is a service server available, else false
* \return RCL_RET_OK if the check was made successfully (regardless of the service readiness), or
* RCL_RET_NODE_INVALID if the node is invalid, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_service_server_is_available(
const rcl_node_t * node,
const rcl_client_t * client,
bool * is_available);
#if __cplusplus
}
#endif
#endif // RCL__GRAPH_H_

29
rcl/include/rcl/guard_condition.h
View file

@ -67,7 +67,6 @@ rcl_get_zero_initialized_guard_condition(void);
* This function is not thread-safe. * This function is not thread-safe.
* This function is lock-free. * This function is lock-free.
* *
*
* \param[inout] guard_condition preallocated guard_condition structure * \param[inout] guard_condition preallocated guard_condition structure
* \param[in] options the guard_condition's options * \param[in] options the guard_condition's options
* \return RCL_RET_OK if guard_condition was initialized successfully, or * \return RCL_RET_OK if guard_condition was initialized successfully, or
@ -83,6 +82,34 @@ rcl_guard_condition_init(
rcl_guard_condition_t * guard_condition, rcl_guard_condition_t * guard_condition,
const rcl_guard_condition_options_t options); const rcl_guard_condition_options_t options);
/// Same as rcl_guard_condition_init(), but reusing an existing rmw handle.
/* In addition to the documentation for rcl_guard_condition_init(), the
* rmw_guard_condition parameter must not be null and must point to a valid
* rmw guard condition.
*
* Also the life time of the rcl guard condition is tied to the life time of
* the rmw guard condition.
* So if the rmw guard condition is destroyed before the rcl guard condition,
* the rcl guard condition becomes invalid.
*
* Similarly if the resulting rcl guard condition is fini'ed before the rmw
* guard condition, then the rmw guard condition is no longer valid.
*
* \param[inout] guard_condition preallocated guard_condition structure
* \param[in] rmw_guard_condition existing rmw guard condition to reuse
* \param[in] options the guard_condition's options
* \return RCL_RET_OK if guard_condition was initialized successfully, or
* RCL_RET_ALREADY_INIT if the guard condition is already initialized, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
rcl_ret_t
rcl_guard_condition_init_from_rmw(
rcl_guard_condition_t * guard_condition,
const rmw_guard_condition_t * rmw_guard_condition,
const rcl_guard_condition_options_t options);
/// Finalize a rcl_guard_condition_t. /// Finalize a rcl_guard_condition_t.
/* After calling, calls to rcl_trigger_guard_condition() will fail when using /* After calling, calls to rcl_trigger_guard_condition() will fail when using
* this guard condition. * this guard condition.

59
rcl/include/rcl/node.h
View file

@ -29,6 +29,7 @@ extern "C"
#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX #define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX
struct rcl_guard_condition_t;
struct rcl_node_impl_t; struct rcl_node_impl_t;
/// Handle for a ROS node. /// Handle for a ROS node.
@ -142,6 +143,37 @@ RCL_PUBLIC
rcl_node_options_t rcl_node_options_t
rcl_node_get_default_options(void); rcl_node_get_default_options(void);
/// Return true if the node is valid, else false.
/* Also return false if the node pointer is nullptr.
*
* A node is invalid if:
* - the implementation is null (rcl_node_init not called or failed)
* - rcl_shutdown has been called since the node has been initialized
* - the node has been finalized with rcl_node_fini
*
* There is a possible validity race condition.
* Consider:
*
* assert(rcl_node_is_valid(node)); <-- thread 1
* rcl_shutdown(); <-- thread 2
* // use node as if valid <-- thread 1
*
* In the third line the node is now invalid, even though on the previous line
* of thread 1 it was checked to be valid.
* This is why this function is considered not thread-safe.
*
* This function does not manipulate heap memory.
* This function is not thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \param[in] node handle to the node to validated
* \return true if the node is valid, otherwise false.
*/
RCL_PUBLIC
bool
rcl_node_is_valid(const rcl_node_t * node);
/// Get the name of the node. /// Get the name of the node.
/* This function returns the node's internal name string. /* This function returns the node's internal name string.
* This function can fail, and therefore return NULL, if: * This function can fail, and therefore return NULL, if:
@ -263,6 +295,33 @@ RCL_WARN_UNUSED
uint64_t uint64_t
rcl_node_get_rcl_instance_id(const rcl_node_t * node); rcl_node_get_rcl_instance_id(const rcl_node_t * node);
/// Return a guard condition which is triggered when the ROS graph changes.
/* The handle returned is a pointer to an internally held rcl guard condition.
* This function can fail, and therefore return NULL, if:
* - node is NULL
* - node is invalid
*
* The returned handle is made invalid if the node is finialized or if
* rcl_shutdown() is called.
*
* The guard condition will be triggered anytime a change to the ROS graph occurs.
* A ROS graph change includes things like (but not limited to) a new publisher
* advertises, a new subscription is created, a new service becomes available,
* a subscription is canceled, etc.
*
* This function does not manipulate heap memory.
* This function is thread-safe.
* This function is lock-free.
*
* \param[in] node pointer to the rcl node
* \return rcl guard condition handle if successful, otherwise NULL
*
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const struct rcl_guard_condition_t *
rcl_node_get_graph_guard_condition(const rcl_node_t * node);
#if __cplusplus #if __cplusplus
} }
#endif #endif

99
rcl/src/rcl/graph.c Normal file
View file

@ -0,0 +1,99 @@
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#if __cplusplus
extern "C"
{
#endif
#include "rcl/graph.h"
#include "./common.h"
rcl_ret_t
rcl_get_topic_names_and_types(
const rcl_node_t * node,
rcl_topic_names_and_types_t * topic_names_and_types)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID;
}
RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT);
return rmw_get_topic_names_and_types(
rcl_node_get_rmw_handle(node),
topic_names_and_types
);
}
rcl_ret_t
rcl_destroy_topic_names_and_types(
rcl_topic_names_and_types_t * topic_names_and_types)
{
RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT);
return rmw_destroy_topic_names_and_types(topic_names_and_types);
}
rcl_ret_t
rcl_count_publishers(
const rcl_node_t * node,
const char * topic_name,
size_t * count)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID;
}
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
return rmw_count_publishers(rcl_node_get_rmw_handle(node), topic_name, count);
}
rcl_ret_t
rcl_count_subscribers(
const rcl_node_t * node,
const char * topic_name,
size_t * count)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID;
}
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
return rmw_count_subscribers(rcl_node_get_rmw_handle(node), topic_name, count);
}
rcl_ret_t
rcl_service_server_is_available(
const rcl_node_t * node,
const rcl_client_t * client,
bool * is_available)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID;
}
RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT);
return rmw_service_server_is_available(
rcl_node_get_rmw_handle(node),
rcl_client_get_rmw_handle(client),
is_available
);
}
#if __cplusplus
}
#endif

41
rcl/src/rcl/guard_condition.c
View file

@ -37,10 +37,13 @@ rcl_get_zero_initialized_guard_condition()
} }
rcl_ret_t rcl_ret_t
rcl_guard_condition_init( __rcl_guard_condition_init_from_rmw_impl(
rcl_guard_condition_t * guard_condition, rcl_guard_condition_t * guard_condition,
const rmw_guard_condition_t * rmw_guard_condition,
const rcl_guard_condition_options_t options) const rcl_guard_condition_options_t options)
{ {
// This function will create an rmw_guard_condition if the parameter is null.
// Perform argument validation. // Perform argument validation.
RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
const rcl_allocator_t * allocator = &options.allocator; const rcl_allocator_t * allocator = &options.allocator;
@ -61,18 +64,42 @@ rcl_guard_condition_init(
return RCL_RET_BAD_ALLOC; return RCL_RET_BAD_ALLOC;
} }
// Create the rmw guard condition. // Create the rmw guard condition.
guard_condition->impl->rmw_handle = rmw_create_guard_condition(); if (rmw_guard_condition) {
if (!guard_condition->impl->rmw_handle) { // If given, just assign (cast away const).
// Deallocate impl and exit. guard_condition->impl->rmw_handle = (rmw_guard_condition_t *)rmw_guard_condition;
allocator->deallocate(guard_condition->impl, allocator->state); } else {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); // Otherwise create one.
return RCL_RET_ERROR; guard_condition->impl->rmw_handle = rmw_create_guard_condition();
if (!guard_condition->impl->rmw_handle) {
// Deallocate impl and exit.
allocator->deallocate(guard_condition->impl, allocator->state);
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
} }
// Copy options into impl. // Copy options into impl.
guard_condition->impl->options = options; guard_condition->impl->options = options;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t
rcl_guard_condition_init(
rcl_guard_condition_t * guard_condition,
const rcl_guard_condition_options_t options)
{
// NULL indicates "create a new rmw guard condition".
return __rcl_guard_condition_init_from_rmw_impl(guard_condition, NULL, options);
}
rcl_ret_t
rcl_guard_condition_init_from_rmw(
rcl_guard_condition_t * guard_condition,
const rmw_guard_condition_t * rmw_guard_condition,
const rcl_guard_condition_options_t options)
{
return __rcl_guard_condition_init_from_rmw_impl(guard_condition, rmw_guard_condition, options);
}
rcl_ret_t rcl_ret_t
rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition) rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition)
{ {

92
rcl/src/rcl/node.c
View file

@ -20,6 +20,7 @@ extern "C"
#include "rcl/node.h" #include "rcl/node.h"
#include <limits.h> #include <limits.h>
#include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
@ -31,9 +32,10 @@ extern "C"
typedef struct rcl_node_impl_t typedef struct rcl_node_impl_t
{ {
rcl_node_options_t options; rcl_node_options_t options;
size_t actual_domain_id;
rmw_node_t * rmw_node_handle; rmw_node_t * rmw_node_handle;
uint64_t rcl_instance_id; uint64_t rcl_instance_id;
size_t actual_domain_id; rcl_guard_condition_t * graph_guard_condition;
} rcl_node_impl_t; } rcl_node_impl_t;
rcl_node_t rcl_node_t
@ -48,6 +50,9 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o
{ {
size_t domain_id = 0; size_t domain_id = 0;
const char * ros_domain_id; const char * ros_domain_id;
const rmw_guard_condition_t * rmw_graph_guard_condition = NULL;
rcl_guard_condition_options_t graph_guard_condition_options =
rcl_guard_condition_get_default_options();
rcl_ret_t ret; rcl_ret_t ret;
rcl_ret_t fail_ret = RCL_RET_ERROR; rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT);
@ -70,6 +75,8 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o
// Allocate space for the implementation struct. // Allocate space for the implementation struct.
node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state); node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
node->impl->rmw_node_handle = NULL;
node->impl->graph_guard_condition = NULL;
// Initialize node impl. // Initialize node impl.
// node name // node name
size_t name_len = strlen(name); size_t name_len = strlen(name);
@ -97,14 +104,56 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o
} else { } else {
domain_id = node->impl->options.domain_id; domain_id = node->impl->options.domain_id;
} }
// actual domain id
node->impl->actual_domain_id = domain_id; node->impl->actual_domain_id = domain_id;
node->impl->rmw_node_handle = rmw_create_node(name, domain_id); node->impl->rmw_node_handle = rmw_create_node(name, domain_id);
RCL_CHECK_FOR_NULL_WITH_MSG( RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail); node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail);
// instance id
node->impl->rcl_instance_id = rcl_get_instance_id(); node->impl->rcl_instance_id = rcl_get_instance_id();
// graph guard condition
rmw_graph_guard_condition = rmw_node_get_graph_guard_condition(node->impl->rmw_node_handle);
if (!rmw_graph_guard_condition) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
goto fail;
}
node->impl->graph_guard_condition = (rcl_guard_condition_t *)allocator->allocate(
sizeof(rcl_guard_condition_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->graph_guard_condition,
"allocating memory failed",
goto fail
);
*node->impl->graph_guard_condition = rcl_get_zero_initialized_guard_condition();
graph_guard_condition_options.allocator = *allocator;
ret = rcl_guard_condition_init_from_rmw(
node->impl->graph_guard_condition,
rmw_graph_guard_condition,
graph_guard_condition_options);
if (ret != RCL_RET_OK) {
// error message already set
goto fail;
}
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (node->impl) { if (node->impl) {
if (node->impl->rmw_node_handle) {
ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (ret != RMW_RET_OK) {
fprintf(stderr,
"failed to fini rmw node in error recovery: %s\n", rmw_get_error_string_safe()
);
}
}
if (node->impl->graph_guard_condition) {
ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
if (ret != RCL_RET_OK) {
fprintf(stderr,
"failed to fini guard condition in error recovery: %s\n", rcl_get_error_string_safe()
);
}
allocator->deallocate(node->impl->graph_guard_condition, allocator->state);
}
allocator->deallocate(node->impl, allocator->state); allocator->deallocate(node->impl, allocator->state);
} }
*node = rcl_get_zero_initialized_node(); *node = rcl_get_zero_initialized_node();
@ -133,6 +182,18 @@ rcl_node_fini(rcl_node_t * node)
return result; return result;
} }
bool
rcl_node_is_valid(const rcl_node_t * node)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, false);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "rcl node implementation is invalid", return false);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return false;
}
return true;
}
rcl_node_options_t rcl_node_options_t
rcl_node_get_default_options() rcl_node_get_default_options()
{ {
@ -147,10 +208,7 @@ rcl_node_get_default_options()
const char * const char *
rcl_node_get_name(const rcl_node_t * node) rcl_node_get_name(const rcl_node_t * node)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); if (!rcl_node_is_valid(node)) {
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL; return NULL;
} }
return node->impl->rmw_node_handle->name; return node->impl->rmw_node_handle->name;
@ -159,10 +217,7 @@ rcl_node_get_name(const rcl_node_t * node)
const rcl_node_options_t * const rcl_node_options_t *
rcl_node_get_options(const rcl_node_t * node) rcl_node_get_options(const rcl_node_t * node)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); if (!rcl_node_is_valid(node)) {
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL; return NULL;
} }
return &node->impl->options; return &node->impl->options;
@ -173,10 +228,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG( if (!rcl_node_is_valid(node)) {
node->impl, "node implementation is invalid", return RCL_RET_NODE_INVALID);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return RCL_RET_NODE_INVALID; return RCL_RET_NODE_INVALID;
} }
*domain_id = node->impl->actual_domain_id; *domain_id = node->impl->actual_domain_id;
@ -186,10 +238,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id)
rmw_node_t * rmw_node_t *
rcl_node_get_rmw_handle(const rcl_node_t * node) rcl_node_get_rmw_handle(const rcl_node_t * node)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); if (!rcl_node_is_valid(node)) {
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL; return NULL;
} }
return node->impl->rmw_node_handle; return node->impl->rmw_node_handle;
@ -203,6 +252,15 @@ rcl_node_get_rcl_instance_id(const rcl_node_t * node)
return node->impl->rcl_instance_id; return node->impl->rcl_instance_id;
} }
const struct rcl_guard_condition_t *
rcl_node_get_graph_guard_condition(const rcl_node_t * node)
{
if (!rcl_node_is_valid(node)) {
return NULL;
}
return node->impl->graph_guard_condition;
}
#if __cplusplus #if __cplusplus
} }
#endif #endif

2
rcl/src/rcl/wait.c
View file

@ -325,7 +325,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
wait_set->size_of_ ## Type ## s = 0; \ wait_set->size_of_ ## Type ## s = 0; \
RCL_SET_ERROR_MSG("allocating memory failed"); \ RCL_SET_ERROR_MSG("allocating memory failed"); \
return RCL_RET_BAD_ALLOC; \ return RCL_RET_BAD_ALLOC; \
} \ }
/* Implementation-specific notes: /* Implementation-specific notes:
* *

14
rcl/test/CMakeLists.txt
View file

@ -67,6 +67,19 @@ function(test_target_function)
AMENT_DEPENDENCIES ${rmw_implementation} AMENT_DEPENDENCIES ${rmw_implementation}
) )
# TODO(wjwwood): remove this when the graph API works properly for more than OpenSplice.
if("${rmw_implementation} " STREQUAL "rmw_opensplice_cpp ")
rcl_add_custom_gtest(test_graph${target_suffix}
SRCS rcl/test_graph.cpp
ENV ${extra_test_env}
APPEND_LIBRARY_DIRS ${extra_lib_dirs}
LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces" "std_msgs"
)
else()
message(STATUS "Skipping test_graph${target_suffix} test.")
endif()
rcl_add_custom_gtest(test_rcl${target_suffix} rcl_add_custom_gtest(test_rcl${target_suffix}
SRCS rcl/test_rcl.cpp SRCS rcl/test_rcl.cpp
ENV ${extra_test_env} ENV ${extra_test_env}
@ -142,7 +155,6 @@ function(test_target_function)
ENV ${extra_test_env} ENV ${extra_test_env}
APPEND_LIBRARY_DIRS ${extra_lib_dirs} APPEND_LIBRARY_DIRS ${extra_lib_dirs}
TIMEOUT 15 TIMEOUT 15
#WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
) )
endfunction() endfunction()

550
rcl/test/rcl/test_graph.cpp Normal file
View file

@ -0,0 +1,550 @@
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Disable -Wmissing-field-initializers, as it is overly strict and will be
// relaxed in future versions of GCC (it is not a warning for clang).
// See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=36750#c13
#ifndef _WIN32
# pragma GCC diagnostic ignored "-Wmissing-field-initializers"
#endif
#include <gtest/gtest.h>
#include <algorithm>
#include <chrono>
#include <future>
#include <string>
#include <thread>
#include "rcl/rcl.h"
#include "rcl/graph.h"
#include "std_msgs/msg/string.h"
#include "example_interfaces/srv/add_two_ints.h"
#include "../memory_tools/memory_tools.hpp"
#include "../scope_exit.hpp"
#include "rcl/error_handling.h"
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
#else
# define CLASSNAME(NAME, SUFFIX) NAME
#endif
class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
rcl_node_t * old_node_ptr;
rcl_node_t * node_ptr;
rcl_wait_set_t * wait_set_ptr;
void SetUp()
{
stop_memory_checking();
rcl_ret_t ret;
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
this->old_node_ptr = new rcl_node_t;
*this->old_node_ptr = rcl_get_zero_initialized_node();
const char * old_name = "old_node_name";
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->old_node_ptr, old_name, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_shutdown(); // after this, the old_node_ptr should be invalid
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
this->node_ptr = new rcl_node_t;
*this->node_ptr = rcl_get_zero_initialized_node();
const char * name = "node_name";
ret = rcl_node_init(this->node_ptr, name, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
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());
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
rcl_ret_t ret;
ret = rcl_node_fini(this->old_node_ptr);
delete this->old_node_ptr;
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_fini(this->wait_set_ptr);
delete this->wait_set_ptr;
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_node_fini(this->node_ptr);
delete this->node_ptr;
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
};
/* Test the rcl_get_topic_names_and_types and rcl_destroy_topic_names_and_types functions.
*
* This does not test content of the rcl_topic_names_and_types_t structure.
*/
TEST_F(
CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION),
test_rcl_get_and_destroy_topic_names_and_types
) {
stop_memory_checking();
rcl_ret_t ret;
rcl_topic_names_and_types_t tnat {};
rcl_node_t zero_node = rcl_get_zero_initialized_node();
// invalid node
ret = rcl_get_topic_names_and_types(nullptr, &tnat);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_get_topic_names_and_types(&zero_node, &tnat);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_get_topic_names_and_types(this->old_node_ptr, &tnat);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// invalid topic_names_and_types
ret = rcl_get_topic_names_and_types(this->node_ptr, nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// invalid argument to rcl_destroy_topic_names_and_types
ret = rcl_destroy_topic_names_and_types(nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// valid calls
ret = rcl_get_topic_names_and_types(this->node_ptr, &tnat);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_destroy_topic_names_and_types(&tnat);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
/* Test the rcl_count_publishers function.
*
* This does not test content the response.
*/
TEST_F(
CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION),
test_rcl_count_publishers
) {
stop_memory_checking();
rcl_ret_t ret;
rcl_node_t zero_node = rcl_get_zero_initialized_node();
const char * topic_name = "topic_test_rcl_count_publishers";
size_t count;
// invalid node
ret = rcl_count_publishers(nullptr, topic_name, &count);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_count_publishers(&zero_node, topic_name, &count);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_count_publishers(this->old_node_ptr, topic_name, &count);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// invalid topic name
ret = rcl_count_publishers(this->node_ptr, nullptr, &count);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// TODO(wjwwood): test valid strings with invalid topic names in them
// invalid count
ret = rcl_count_publishers(this->node_ptr, topic_name, nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// valid call
ret = rcl_count_publishers(this->node_ptr, topic_name, &count);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
}
/* Test the rcl_count_subscribers function.
*
* This does not test content the response.
*/
TEST_F(
CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION),
test_rcl_count_subscribers
) {
stop_memory_checking();
rcl_ret_t ret;
rcl_node_t zero_node = rcl_get_zero_initialized_node();
const char * topic_name = "topic_test_rcl_count_subscribers";
size_t count;
// invalid node
ret = rcl_count_subscribers(nullptr, topic_name, &count);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_count_subscribers(&zero_node, topic_name, &count);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_count_subscribers(this->old_node_ptr, topic_name, &count);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// invalid topic name
ret = rcl_count_subscribers(this->node_ptr, nullptr, &count);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// TODO(wjwwood): test valid strings with invalid topic names in them
// invalid count
ret = rcl_count_subscribers(this->node_ptr, topic_name, nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// valid call
ret = rcl_count_subscribers(this->node_ptr, topic_name, &count);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
}
void
check_graph_state(
const rcl_node_t * node_ptr,
rcl_wait_set_t * wait_set_ptr,
const rcl_guard_condition_t * graph_guard_condition,
std::string & topic_name,
size_t expected_publisher_count,
size_t expected_subscriber_count,
bool expected_in_tnat,
size_t number_of_tries)
{
printf(
"Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n",
expected_publisher_count,
expected_subscriber_count,
expected_in_tnat ? "" : " not"
);
size_t publisher_count = 0;
size_t subscriber_count = 0;
bool is_in_tnat = false;
rcl_topic_names_and_types_t tnat {};
rcl_ret_t ret;
for (size_t i = 0; i < number_of_tries; ++i) {
ret = rcl_count_publishers(node_ptr, topic_name.c_str(), &publisher_count);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_count_subscribers(node_ptr, topic_name.c_str(), &subscriber_count);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_get_topic_names_and_types(node_ptr, &tnat);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
is_in_tnat = false;
for (size_t i = 0; RCL_RET_OK == ret && i < tnat.topic_count; ++i) {
if (topic_name == std::string(tnat.topic_names[i])) {
ASSERT_FALSE(is_in_tnat) << "duplicates in the tnat"; // Found it more than once!
is_in_tnat = true;
}
}
if (RCL_RET_OK == ret) {
ret = rcl_destroy_topic_names_and_types(&tnat);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
}
printf(
" Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n",
i + 1,
publisher_count,
subscriber_count,
is_in_tnat ? "" : " not"
);
if (
expected_publisher_count == publisher_count &&
expected_subscriber_count == subscriber_count &&
expected_in_tnat == is_in_tnat)
{
printf(" state correct!\n");
break;
}
// Wait for graph change before trying again.
if ((i + 1) == number_of_tries) {
// Don't wait for the graph to change on the last loop because we won't check again.
continue;
}
ret = rcl_wait_set_clear_guard_conditions(wait_set_ptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
printf(
" 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) {
printf("timeout\n");
continue;
}
printf("change occurred\n");
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
EXPECT_EQ(expected_publisher_count, publisher_count);
EXPECT_EQ(expected_subscriber_count, subscriber_count);
if (expected_in_tnat) {
EXPECT_TRUE(is_in_tnat);
} else {
EXPECT_FALSE(is_in_tnat);
}
}
/* Test graph queries with a hand crafted graph.
*/
TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) {
stop_memory_checking();
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());
printf("Using topic name: %s\n", 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);
// First assert the "topic_name" is not in use.
check_graph_state(
this->node_ptr,
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
// 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(std_msgs, String);
ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// Check the graph.
check_graph_state(
this->node_ptr,
this->wait_set_ptr,
graph_guard_condition,
topic_name,
1, // expected publishers on topic
0, // expected subscribers on topic
true, // topic expected in graph
9); // number of retries
// Now create a subscriber.
rcl_subscription_t sub = rcl_get_zero_initialized_subscription();
rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options();
ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// Check the graph again.
check_graph_state(
this->node_ptr,
this->wait_set_ptr,
graph_guard_condition,
topic_name,
1, // expected publishers on topic
1, // expected subscribers on topic
true, // topic expected in graph
9); // number of retries
// Destroy the publisher.
ret = rcl_publisher_fini(&pub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// Check the graph again.
check_graph_state(
this->node_ptr,
this->wait_set_ptr,
graph_guard_condition,
topic_name,
0, // expected publishers on topic
1, // expected subscribers on topic
true, // topic expected in graph
9); // number of retries
// Destroy the subscriber.
ret = rcl_subscription_fini(&sub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
rcl_reset_error();
// Check the graph again.
check_graph_state(
this->node_ptr,
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
}
/* Test the graph guard condition notices topic changes.
*
* Note: this test could be impacted by other communications on the same ROS Domain.
*/
TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_topics) {
stop_memory_checking();
rcl_ret_t ret;
// Create a thread to sleep for a time, then create a publisher, sleep more, then a subscriber,
// sleep more, destroy the subscriber, sleep more, and then destroy the publisher.
std::promise<bool> topic_changes_promise;
std::thread topic_thread([this, &topic_changes_promise]() {
// sleep
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// create the publisher
rcl_publisher_t pub = rcl_get_zero_initialized_publisher();
rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
rcl_ret_t ret = rcl_publisher_init(
&pub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, String),
"chatter_test_graph_guard_condition_topics", &pub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// sleep
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// create the subscription
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, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, String),
"chatter_test_graph_guard_condition_topics", &sub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// sleep
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// destroy the subscription
ret = rcl_subscription_fini(&sub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// sleep
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// destroy the publication
ret = rcl_publisher_fini(&pub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// notify that the thread is done
topic_changes_promise.set_value(true);
});
// Wait for the graph state to change, expecting it to do so at least 4 times,
// once for each change in the topics thread.
const rcl_guard_condition_t * graph_guard_condition =
rcl_node_get_graph_guard_condition(this->node_ptr);
ASSERT_NE(nullptr, graph_guard_condition) << rcl_get_error_string_safe();
std::shared_future<bool> future = topic_changes_promise.get_future();
size_t graph_changes_count = 0;
// while the topic thread is not done, wait and count the graph changes
while (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
ret = rcl_wait_set_clear_guard_conditions(this->wait_set_ptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
printf(
"waiting up to '%s' nanoseconds for graph changes\n",
std::to_string(time_to_sleep.count()).c_str());
ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count());
if (ret == RCL_RET_TIMEOUT) {
continue;
}
graph_changes_count++;
}
topic_thread.join();
// expect at least 4 changes
ASSERT_GE(graph_changes_count, 4ul);
}
/* Test the rcl_service_server_is_available function.
*/
TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_is_available) {
stop_memory_checking();
rcl_ret_t ret;
// First create a client which will be used to call the function.
rcl_client_t client = rcl_get_zero_initialized_client();
auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, AddTwoInts);
const char * service_name = "service_test_rcl_service_server_is_available";
rcl_client_options_t client_options = rcl_client_get_default_options();
ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
auto client_exit = make_scope_exit([&client, this]() {
stop_memory_checking();
rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
});
// Check, knowing there is no service server (created by us at least).
bool is_available;
ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ASSERT_FALSE(is_available);
// Setup function to wait for service state to change using graph guard condition.
const rcl_guard_condition_t * graph_guard_condition =
rcl_node_get_graph_guard_condition(this->node_ptr);
ASSERT_NE(nullptr, graph_guard_condition) << rcl_get_error_string_safe();
auto wait_for_service_state_to_change = [this, &graph_guard_condition, &client](
bool expected_state,
bool & is_available)
{
is_available = false;
auto start = std::chrono::steady_clock::now();
auto end = start + std::chrono::seconds(10);
while (std::chrono::steady_clock::now() < end) {
// We wait multiple times in case other graph changes are occurring simultaneously.
std::chrono::nanoseconds time_left = end - start;
std::chrono::nanoseconds time_to_sleep = time_left;
std::chrono::seconds min_sleep(1);
if (time_to_sleep > min_sleep) {
time_to_sleep = min_sleep;
}
rcl_ret_t ret = rcl_wait_set_clear_guard_conditions(this->wait_set_ptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
printf(
"waiting up to '%s' nanoseconds for graph changes\n",
std::to_string(time_to_sleep.count()).c_str());
ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count());
if (ret == RCL_RET_TIMEOUT) {
continue;
}
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
if (is_available == expected_state) {
break;
}
}
};
{
// Create the service server.
rcl_service_t service = rcl_get_zero_initialized_service();
rcl_service_options_t service_options = rcl_service_get_default_options();
ret = rcl_service_init(&service, this->node_ptr, ts, service_name, &service_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
auto service_exit = make_scope_exit([&service, this]() {
stop_memory_checking();
rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
});
// Wait for and then assert that it is available.
wait_for_service_state_to_change(true, is_available);
ASSERT_TRUE(is_available);
}
// Assert the state goes back to "not available" after the service is removed.
wait_for_service_state_to_change(false, is_available);
ASSERT_FALSE(is_available);
}

40
rcl/test/rcl/test_node.cpp
View file

@ -112,6 +112,20 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors)
rcl_ret_t ret = rcl_node_fini(&node); rcl_ret_t ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret); EXPECT_EQ(RCL_RET_OK, ret);
}); });
// Test rcl_node_is_valid().
bool is_valid;
is_valid = rcl_node_is_valid(nullptr);
EXPECT_FALSE(is_valid);
rcl_reset_error();
is_valid = rcl_node_is_valid(&zero_node);
EXPECT_FALSE(is_valid);
rcl_reset_error();
is_valid = rcl_node_is_valid(&invalid_node);
EXPECT_FALSE(is_valid);
rcl_reset_error();
is_valid = rcl_node_is_valid(&node);
EXPECT_TRUE(is_valid);
rcl_reset_error();
// Test rcl_node_get_name(). // Test rcl_node_get_name().
const char * actual_node_name; const char * actual_node_name;
actual_node_name = rcl_node_get_name(nullptr); actual_node_name = rcl_node_get_name(nullptr);
@ -229,6 +243,32 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors)
assert_no_free_end(); assert_no_free_end();
stop_memory_checking(); stop_memory_checking();
EXPECT_NE(0u, instance_id); EXPECT_NE(0u, instance_id);
// Test rcl_node_get_graph_guard_condition
std::string rmw_id(rmw_get_implementation_identifier());
if (rmw_id.find("opensplice") != std::string::npos) {
// Only test with opensplice for now, as connext and fastrtps are not working.
// TODO(wjwwood): remove this check when more middlewares implement this
const rcl_guard_condition_t * graph_guard_condition = nullptr;
graph_guard_condition = rcl_node_get_graph_guard_condition(nullptr);
EXPECT_EQ(nullptr, graph_guard_condition);
rcl_reset_error();
graph_guard_condition = rcl_node_get_graph_guard_condition(&zero_node);
EXPECT_EQ(nullptr, graph_guard_condition);
rcl_reset_error();
graph_guard_condition = rcl_node_get_graph_guard_condition(&invalid_node);
EXPECT_EQ(nullptr, graph_guard_condition);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
graph_guard_condition = rcl_node_get_graph_guard_condition(&node);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(nullptr, graph_guard_condition);
}
} }
/* Tests the node life cycle, including rcl_node_init() and rcl_node_fini(). /* Tests the node life cycle, including rcl_node_init() and rcl_node_fini().