diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index c3168dc..5a0ec15 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -20,6 +20,7 @@ set(${PROJECT_NAME}_sources src/rcl/allocator.c src/rcl/client.c src/rcl/common.c + src/rcl/graph.c src/rcl/guard_condition.c src/rcl/node.c src/rcl/publisher.c diff --git a/rcl/include/rcl/graph.h b/rcl/include/rcl/graph.h new file mode 100644 index 0000000..c5395ff --- /dev/null +++ b/rcl/include/rcl/graph.h @@ -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 +#include + +#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_ diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index f7bd781..f5b23af 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -67,7 +67,6 @@ rcl_get_zero_initialized_guard_condition(void); * This function is not thread-safe. * This function is lock-free. * - * * \param[inout] guard_condition preallocated guard_condition structure * \param[in] options the guard_condition's options * \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, 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. /* After calling, calls to rcl_trigger_guard_condition() will fail when using * this guard condition. diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 291b021..2357531 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -29,6 +29,7 @@ extern "C" #define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX +struct rcl_guard_condition_t; struct rcl_node_impl_t; /// Handle for a ROS node. @@ -142,6 +143,37 @@ RCL_PUBLIC rcl_node_options_t 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. /* This function returns the node's internal name string. * This function can fail, and therefore return NULL, if: @@ -263,6 +295,33 @@ RCL_WARN_UNUSED uint64_t 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 } #endif diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c new file mode 100644 index 0000000..4b62830 --- /dev/null +++ b/rcl/src/rcl/graph.c @@ -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 diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 51ff2c3..76557e5 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -37,10 +37,13 @@ rcl_get_zero_initialized_guard_condition() } rcl_ret_t -rcl_guard_condition_init( +__rcl_guard_condition_init_from_rmw_impl( rcl_guard_condition_t * guard_condition, + const rmw_guard_condition_t * rmw_guard_condition, const rcl_guard_condition_options_t options) { + // This function will create an rmw_guard_condition if the parameter is null. + // Perform argument validation. RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); const rcl_allocator_t * allocator = &options.allocator; @@ -61,18 +64,42 @@ rcl_guard_condition_init( return RCL_RET_BAD_ALLOC; } // Create the rmw guard condition. - 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; + if (rmw_guard_condition) { + // If given, just assign (cast away const). + guard_condition->impl->rmw_handle = (rmw_guard_condition_t *)rmw_guard_condition; + } else { + // Otherwise create one. + 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. guard_condition->impl->options = options; 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_guard_condition_fini(rcl_guard_condition_t * guard_condition) { diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 090134c..76327ed 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -20,6 +20,7 @@ extern "C" #include "rcl/node.h" #include +#include #include #include @@ -31,9 +32,10 @@ extern "C" typedef struct rcl_node_impl_t { rcl_node_options_t options; + size_t actual_domain_id; rmw_node_t * rmw_node_handle; uint64_t rcl_instance_id; - size_t actual_domain_id; + rcl_guard_condition_t * graph_guard_condition; } rcl_node_impl_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; 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 fail_ret = RCL_RET_ERROR; 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. 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); + node->impl->rmw_node_handle = NULL; + node->impl->graph_guard_condition = NULL; // Initialize node impl. // node 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 { domain_id = node->impl->options.domain_id; } + // actual domain id node->impl->actual_domain_id = domain_id; node->impl->rmw_node_handle = rmw_create_node(name, domain_id); RCL_CHECK_FOR_NULL_WITH_MSG( node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail); + // 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; fail: 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); } *node = rcl_get_zero_initialized_node(); @@ -133,6 +182,18 @@ rcl_node_fini(rcl_node_t * node) 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_get_default_options() { @@ -147,10 +208,7 @@ rcl_node_get_default_options() const char * rcl_node_get_name(const rcl_node_t * node) { - RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); - 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"); + if (!rcl_node_is_valid(node)) { return NULL; } 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 * rcl_node_get_options(const rcl_node_t * node) { - RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); - 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"); + if (!rcl_node_is_valid(node)) { return NULL; } 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(domain_id, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG( - 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"); + if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } *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 * rcl_node_get_rmw_handle(const rcl_node_t * node) { - RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); - 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"); + if (!rcl_node_is_valid(node)) { return NULL; } 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; } +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 } #endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 8219966..d016fb8 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -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; \ RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ - } \ + } /* Implementation-specific notes: * diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 620a3c3..a3aeab8 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -67,6 +67,19 @@ function(test_target_function) 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} SRCS rcl/test_rcl.cpp ENV ${extra_test_env} @@ -142,7 +155,6 @@ function(test_target_function) ENV ${extra_test_env} APPEND_LIBRARY_DIRS ${extra_lib_dirs} TIMEOUT 15 - #WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ) endfunction() diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp new file mode 100644 index 0000000..9d87c32 --- /dev/null +++ b/rcl/test/rcl/test_graph.cpp @@ -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 + +#include +#include +#include +#include +#include + +#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 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 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); +} diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 5cf495e..de7909d 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -112,6 +112,20 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) rcl_ret_t ret = rcl_node_fini(&node); 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(). const char * actual_node_name; 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(); stop_memory_checking(); 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().