support wait_for_service (#165)

* fix some unrelated cmake/infrastructure issues

* initial implementation of service_server_is_available and the graph guard condition

* disable rmw_service_server_is_available until local entities notify

* remove debug

* add missing dependencies in the package.xml

* style fixup

rmw_connext_cpp/CMakeLists.txt

@@ -38,7 +38,9 @@ ament_export_dependencies(
   rmw
   rmw_connext_shared_cpp
   rosidl_generator_cpp
-  rosidl_typesupport_connext_cpp)
+  rosidl_typesupport_connext_c
+  rosidl_typesupport_connext_cpp
+)
 
 register_rmw_implementation(
   "c:rosidl_typesupport_connext_c"

rmw_connext_cpp/package.xml

@@ -17,12 +17,14 @@
   <build_depend>rmw</build_depend>
   <build_depend>rmw_connext_shared_cpp</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_typesupport_connext_c</build_depend>
   <build_depend>rosidl_typesupport_connext_cpp</build_depend>
   <build_depend>rosidl_generator_dds_idl</build_depend>
 
   <build_export_depend>connext_cmake_module</build_export_depend>
   <build_export_depend>libndds52</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_typesupport_connext_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_connext_cpp</build_export_depend>
 
   <exec_depend>rmw</exec_depend>

rmw_connext_cpp/src/functions.cpp

@@ -1515,4 +1515,111 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re
   *result = (matches == DDS_BOOLEAN_TRUE);
   return RMW_RET_OK;
 }
+
+const rmw_guard_condition_t *
+rmw_node_get_graph_guard_condition(const rmw_node_t * node)
+{
+  if (!node) {
+    RMW_SET_ERROR_MSG("node handle is null");
+    return nullptr;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node handle,
+    node->implementation_identifier, rti_connext_identifier,
+    return nullptr)
+
+  return node_get_graph_guard_condition(node);
+}
+
+rmw_ret_t
+rmw_service_server_is_available(
+  const rmw_node_t * node,
+  const rmw_client_t * client,
+  bool * is_available)
+{
+  // TODO(wjwwood): remove this once local graph changes are detected.
+  RMW_SET_ERROR_MSG("not implemented");
+  return RMW_RET_ERROR;
+
+  if (!node) {
+    RMW_SET_ERROR_MSG("node handle is null");
+    return RMW_RET_ERROR;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node handle,
+    node->implementation_identifier, rti_connext_identifier,
+    return RMW_RET_ERROR)
+  if (!client) {
+    RMW_SET_ERROR_MSG("client handle is null");
+    return RMW_RET_ERROR;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client handle,
+    client->implementation_identifier, rti_connext_identifier,
+    return RMW_RET_ERROR)
+
+  if (!is_available) {
+    RMW_SET_ERROR_MSG("is_available is null");
+    return RMW_RET_ERROR;
+  }
+
+  ConnextStaticClientInfo * client_info =
+    static_cast<ConnextStaticClientInfo *>(client->data);
+  if (!client_info) {
+    RMW_SET_ERROR_MSG("client info handle is null");
+    return RMW_RET_ERROR;
+  }
+
+  const service_type_support_callbacks_t * callbacks = client_info->callbacks_;
+  if (!callbacks) {
+    RMW_SET_ERROR_MSG("callbacks handle is null");
+    return RMW_RET_ERROR;
+  }
+  void * requester = client_info->requester_;
+  if (!requester) {
+    RMW_SET_ERROR_MSG("requester handle is null");
+    return RMW_RET_ERROR;
+  }
+  const char * request_topic_name = callbacks->get_request_topic_name(requester);
+  if (!request_topic_name) {
+    RMW_SET_ERROR_MSG("could not get request topic name");
+    return RMW_RET_ERROR;
+  }
+
+  *is_available = false;
+  // In the Connext RPC implementation, a server is ready when:
+  //   - At least one server is subscribed to the request topic.
+  //   - At least one server is publishing to the reponse topic.
+  size_t number_of_request_subscribers = 0;
+  rmw_ret_t ret = rmw_count_subscribers(
+    node,
+    request_topic_name,
+    &number_of_request_subscribers);
+  if (ret != RMW_RET_OK) {
+    // error string already set
+    return ret;
+  }
+  if (number_of_request_subscribers == 0) {
+    // not ready
+    return RMW_RET_OK;
+  }
+
+  size_t number_of_response_publishers = 0;
+  ret = rmw_count_publishers(
+    node,
+    client_info->response_datareader_->get_topicdescription()->get_name(),
+    &number_of_response_publishers);
+  if (ret != RMW_RET_OK) {
+    // error string already set
+    return ret;
+  }
+  if (number_of_response_publishers == 0) {
+    // not ready
+    return RMW_RET_OK;
+  }
+
+  // all conditions met, there is a service server available
+  *is_available = true;
+  return RMW_RET_OK;
+}
 }  // extern "C"

rmw_connext_dynamic_cpp/src/functions.cpp

@@ -3302,4 +3302,95 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re
   *result = (matches == DDS_BOOLEAN_TRUE);
   return RMW_RET_OK;
 }
+
+const rmw_guard_condition_t *
+rmw_node_get_graph_guard_condition(const rmw_node_t * node)
+{
+  if (!node) {
+    RMW_SET_ERROR_MSG("node handle is null");
+    return nullptr;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node handle,
+    node->implementation_identifier, rti_connext_dynamic_identifier,
+    return nullptr)
+
+  return node_get_graph_guard_condition(node);
+}
+
+rmw_ret_t
+rmw_service_server_is_available(
+  const rmw_node_t * node,
+  const rmw_client_t * client,
+  bool * is_available)
+{
+  // TODO(wjwwood): remove this once local graph changes are detected.
+  RMW_SET_ERROR_MSG("not implemented");
+  return RMW_RET_ERROR;
+
+  if (!node) {
+    RMW_SET_ERROR_MSG("node handle is null");
+    return RMW_RET_ERROR;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node handle,
+    node->implementation_identifier, rti_connext_dynamic_identifier,
+    return RMW_RET_ERROR)
+  if (!client) {
+    RMW_SET_ERROR_MSG("client handle is null");
+    return RMW_RET_ERROR;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client handle,
+    client->implementation_identifier, rti_connext_dynamic_identifier,
+    return RMW_RET_ERROR)
+
+  if (!is_available) {
+    RMW_SET_ERROR_MSG("is_available is null");
+    return RMW_RET_ERROR;
+  }
+
+  ConnextDynamicClientInfo * client_info =
+    static_cast<ConnextDynamicClientInfo *>(client->data);
+  if (!client_info) {
+    RMW_SET_ERROR_MSG("client info handle is null");
+    return RMW_RET_ERROR;
+  }
+
+  *is_available = false;
+  // In the Connext RPC implementation, a server is ready when:
+  //   - At least one server is subscribed to the request topic.
+  //   - At least one server is publishing to the reponse topic.
+  size_t number_of_request_subscribers = 0;
+  rmw_ret_t ret = rmw_count_subscribers(
+    node,
+    client_info->requester_->get_request_datawriter()->get_topic()->get_name(),
+    &number_of_request_subscribers);
+  if (ret != RMW_RET_OK) {
+    // error string already set
+    return ret;
+  }
+  if (number_of_request_subscribers == 0) {
+    // not ready
+    return RMW_RET_OK;
+  }
+
+  size_t number_of_response_publishers = 0;
+  ret = rmw_count_publishers(
+    node,
+    client_info->response_datareader_->get_topicdescription()->get_name(),
+    &number_of_response_publishers);
+  if (ret != RMW_RET_OK) {
+    // error string already set
+    return ret;
+  }
+  if (number_of_response_publishers == 0) {
+    // not ready
+    return RMW_RET_OK;
+  }
+
+  // all conditions met, there is a service server available
+  *is_available = true;
+  return RMW_RET_OK;
+}
 }  // extern "C"

rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/shared_functions.hpp

@@ -39,6 +39,7 @@
 # pragma GCC diagnostic pop
 #endif
 
+#include "rmw/allocators.h"
 #include "rmw/rmw.h"
 #include "rmw/types.h"
 
@@ -517,4 +518,8 @@ count_subscribers(const char * implementation_identifier,
   const char * topic_name,
   size_t * count);
 
+RMW_CONNEXT_SHARED_CPP_PUBLIC
+const rmw_guard_condition_t *
+node_get_graph_guard_condition(const rmw_node_t * node);
+
 #endif  // RMW_CONNEXT_SHARED_CPP__SHARED_FUNCTIONS_HPP_

rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/types.hpp

@@ -71,21 +71,42 @@ class CustomPublisherListener
   : public CustomDataReaderListener
 {
 public:
+  explicit CustomPublisherListener(
+    const char * implementation_identifier, rmw_guard_condition_t * graph_guard_condition)
+  : implementation_identifier_(implementation_identifier),
+    graph_guard_condition_(graph_guard_condition)
+  {}
+
   virtual void on_data_available(DDSDataReader * reader);
+
+private:
+  const char * implementation_identifier_;
+  rmw_guard_condition_t * graph_guard_condition_;
 };
 
 class CustomSubscriberListener
   : public CustomDataReaderListener
 {
 public:
+  explicit CustomSubscriberListener(
+    const char * implementation_identifier, rmw_guard_condition_t * graph_guard_condition)
+  : implementation_identifier_(implementation_identifier),
+    graph_guard_condition_(graph_guard_condition)
+  {}
+
   virtual void on_data_available(DDSDataReader * reader);
+
+private:
+  const char * implementation_identifier_;
+  rmw_guard_condition_t * graph_guard_condition_;
 };
 
 struct ConnextNodeInfo
 {
   DDSDomainParticipant * participant;
   CustomPublisherListener * publisher_listener;
   CustomSubscriberListener * subscriber_listener;
+  rmw_guard_condition_t * graph_guard_condition;
 };
 
 struct ConnextPublisherGID