add graph functions to support wait_for_service

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

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 <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_

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.

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