Implement create and destroy methods for service and client (ros2#76)

Signed-off-by: Simon Hoinkis <[email protected]>

rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp

@@ -23,6 +23,7 @@
 #include "iceoryx_hoofs/cxx/string.hpp"
 
 struct rosidl_message_type_support_t;
+struct rosidl_service_type_support_t;
 
 namespace rmw_iceoryx_cpp
 {
@@ -71,5 +72,10 @@ get_iceoryx_service_description(
   const std::string & topic,
   const rosidl_message_type_support_t * type_supports);
 
+iox::capro::ServiceDescription
+get_iceoryx_service_description(
+  const std::string & topic,
+  const rosidl_service_type_support_t * type_supports);
+
 }  // namespace rmw_iceoryx_cpp
 #endif  // RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_

rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp

@@ -21,6 +21,7 @@
 #include "rcpputils/split.hpp"
 
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
+#include "rosidl_typesupport_cpp/service_type_support.hpp"
 
 #include "rosidl_typesupport_introspection_c/field_types.h"
 #include "rosidl_typesupport_introspection_c/identifier.h"
@@ -159,6 +160,16 @@ get_service_description_from_name_n_type(
   return std::make_tuple(service, instance, event);
 }
 
+iox::capro::ServiceDescription make_service_description(const std::string & topic_name, const std::string & type_name)
+{
+  auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name);
+
+  return iox::capro::ServiceDescription(
+    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)),
+    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)),
+    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple)));
+}
+
 iox::capro::ServiceDescription
 get_iceoryx_service_description(
   const std::string & topic_name,
@@ -169,12 +180,21 @@ get_iceoryx_service_description(
   extract_type(type_supports, package_name, type_name);
   type_name = package_name + "/" + type_name;
 
-  auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name);
+  return make_service_description(topic_name, type_name);
+}
 
-  return iox::capro::ServiceDescription(
-    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)),
-    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)),
-    iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple)));
+iox::capro::ServiceDescription
+get_iceoryx_service_description(
+  const std::string & topic_name,
+  const rosidl_service_type_support_t * type_supports)
+{
+  std::string package_name;
+  std::string type_name;
+  /// @todo Implement type extracts for ROS services
+  //extract_type(type_supports, package_name, type_name);
+  type_name = package_name + "/" + type_name;
+
+  return make_service_description(topic_name, type_name);
 }
 
 }  // namespace rmw_iceoryx_cpp

rmw_iceoryx_cpp/src/rmw_client.cpp

@@ -20,6 +20,10 @@
 #include "rmw/impl/cpp/macros.hpp"
 #include "rmw/rmw.h"
 
+#include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp"
+
+#include "iceoryx_posh/popo/untyped_client.hpp"
+
 extern "C"
 {
 rmw_client_t *
@@ -34,8 +38,55 @@ rmw_create_client(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, NULL);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, NULL);
 
-  RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support clients.");
-  return NULL;
+    RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+      rmw_create_client
+      : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr);
+
+  // create the iceoryx service description for a sender
+  auto service_description =
+    rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports);
+
+  std::string node_full_name = std::string(node->namespace_) + std::string(node->name);
+  rmw_client_t * rmw_client = nullptr;
+  iox::popo::UntypedClient * iceoryx_client = nullptr;
+
+  rmw_client = rmw_client_allocate();
+  if (!rmw_client) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for client");
+    return nullptr;
+  }
+
+  auto cleanupAfterError = [](){};
+
+  iceoryx_client =
+    static_cast<iox::popo::UntypedClient *>(rmw_allocate(
+      sizeof(iox::popo::UntypedClient)));
+  if (!iceoryx_client) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx client");
+    cleanupAfterError();
+    return nullptr;
+  }
+
+  RMW_TRY_PLACEMENT_NEW(
+    iceoryx_client, iceoryx_client,
+    cleanupAfterError(), iox::popo::UntypedClient, service_description,
+    iox::popo::ClientOptions{
+      0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)});
+
+  rmw_client->implementation_identifier = rmw_get_implementation_identifier();
+  rmw_client->data = iceoryx_client;
+
+  rmw_client->service_name =
+    static_cast<const char *>(rmw_allocate(sizeof(char) * strlen(service_name) + 1));
+  if (!rmw_client->service_name) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for service name");
+    cleanupAfterError();
+    return nullptr;
+  } else {
+    memcpy(const_cast<char *>(rmw_client->service_name), service_name, strlen(service_name) + 1);
+  }
+
+  return rmw_client;
 }
 
 rmw_ret_t
@@ -46,8 +97,30 @@ rmw_destroy_client(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR);
 
-  RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support clients.");
-  return RMW_RET_UNSUPPORTED;
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_destroy_client
+    : client, client->implementation_identifier,
+    rmw_get_implementation_identifier(), return RMW_RET_ERROR);
+
+  rmw_ret_t result = RMW_RET_OK;
+
+  iox::popo::UntypedClient * iceoryx_client = static_cast<iox::popo::UntypedClient *>(client->data);
+  if (iceoryx_client) {
+      RMW_TRY_DESTRUCTOR(
+        iceoryx_client->~UntypedClientImpl(),
+        iceoryx_client,
+        result = RMW_RET_ERROR)
+      rmw_free(iceoryx_client);
+  }
+
+  client->data = nullptr;
+
+  rmw_free(const_cast<char *>(client->service_name));
+  client->service_name = nullptr;
+
+  rmw_client_free(client);
+
+  return result;
 }
 
 rmw_ret_t rmw_client_request_publisher_get_actual_qos(

rmw_iceoryx_cpp/src/rmw_service.cpp

@@ -19,9 +19,12 @@
 #include "rmw/impl/cpp/macros.hpp"
 #include "rmw/rmw.h"
 
+#include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp"
+
+#include "iceoryx_posh/popo/untyped_server.hpp"
+
 extern "C"
 {
-/// @todo Use the new request/response API of iceoryx v2.0 here instead of dummy services
 rmw_service_t *
 rmw_create_service(
   const rmw_node_t * node,
@@ -34,23 +37,50 @@ rmw_create_service(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr);
 
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+      rmw_create_service
+      : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr);
+
+  // create the iceoryx service description for a sender
+  auto service_description =
+    rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports);
+
+  std::string node_full_name = std::string(node->namespace_) + std::string(node->name);
   rmw_service_t * rmw_service = nullptr;
+  iox::popo::UntypedServer * iceoryx_server = nullptr;
 
   rmw_service = rmw_service_allocate();
   if (!rmw_service) {
     RMW_SET_ERROR_MSG("failed to allocate memory for service");
     return nullptr;
   }
 
-  void * info = nullptr;
+  auto cleanupAfterError = [](){};
+
+  iceoryx_server =
+    static_cast<iox::popo::UntypedServer *>(rmw_allocate(
+      sizeof(iox::popo::UntypedServer)));
+  if (!iceoryx_server) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx server");
+    cleanupAfterError();
+    return nullptr;
+  }
+
+  RMW_TRY_PLACEMENT_NEW(
+    iceoryx_server, iceoryx_server,
+    cleanupAfterError(), iox::popo::UntypedServer, service_description,
+    iox::popo::ServerOptions{
+      0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)});
 
   rmw_service->implementation_identifier = rmw_get_implementation_identifier();
-  rmw_service->data = info;
+  rmw_service->data = iceoryx_server;
 
   rmw_service->service_name =
     static_cast<const char *>(rmw_allocate(sizeof(char) * strlen(service_name) + 1));
   if (!rmw_service->service_name) {
-    RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name");
+    RMW_SET_ERROR_MSG("failed to allocate memory for service name");
+    cleanupAfterError();
+    return nullptr;
   } else {
     memcpy(const_cast<char *>(rmw_service->service_name), service_name, strlen(service_name) + 1);
   }
@@ -71,6 +101,17 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service)
 
   rmw_ret_t result = RMW_RET_OK;
 
+  iox::popo::UntypedServer * iceoryx_server = static_cast<iox::popo::UntypedServer *>(service->data);
+  if (iceoryx_server) {
+      RMW_TRY_DESTRUCTOR(
+        iceoryx_server->~UntypedServerImpl(),
+        iceoryx_server,
+        result = RMW_RET_ERROR)
+      rmw_free(iceoryx_server);
+  }
+
+  service->data = nullptr;
+
   rmw_free(const_cast<char *>(service->service_name));
   service->service_name = nullptr;