Create server and client abstraction (ros2#76)

Signed-off-by: Simon Hoinkis <[email protected]>
ApexAI · Jan 5, 2023 · 146ada8 · 146ada8
1 parent 64870a1
commit 146ada8
Show file tree

Hide file tree

Showing 2 changed files with 94 additions and 11 deletions.
diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp
@@ -49,15 +49,37 @@ rmw_create_client(
   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;
+  IceoryxClient * iceoryx_client_abstraction = nullptr;
+
+  bool returnOnError = false;
+
+  auto cleanupAfterError = [&](){
+    if (rmw_client) {
+      if (iceoryx_client) {
+        RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
+          iceoryx_client->~UntypedClient(), iox::popo::UntypedClient)
+        rmw_free(iceoryx_client);
+      }
+      if (iceoryx_client_abstraction) {
+        RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
+          iceoryx_client_abstraction->~IceoryxClient(), IceoryxClient)
+        rmw_free(iceoryx_client_abstraction);
+      }
+      if (rmw_client->service_name) {
+        rmw_free(const_cast<char *>(rmw_client->service_name));
+      }
+      rmw_client_free(rmw_client);
+      returnOnError = true;
+    }
+  };
 
   rmw_client = rmw_client_allocate();
   if (!rmw_client) {
     RMW_SET_ERROR_MSG("failed to allocate memory for client");
+    cleanupAfterError();
     return nullptr;
   }
 
-  auto cleanupAfterError = [](){};
-
   iceoryx_client =
     static_cast<iox::popo::UntypedClient *>(rmw_allocate(
       sizeof(iox::popo::UntypedClient)));
@@ -72,6 +94,27 @@ rmw_create_client(
     cleanupAfterError(), iox::popo::UntypedClient, service_description,
     iox::popo::ClientOptions{
       0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)});
+  if(returnOnError)
+  {
+    return nullptr;
+  }
+
+  iceoryx_client->connect();
+
+  iceoryx_client_abstraction =
+    static_cast<IceoryxClient *>(rmw_allocate(sizeof(IceoryxClient)));
+  if (!iceoryx_client_abstraction) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher");
+    cleanupAfterError();
+    return nullptr;
+  }
+  RMW_TRY_PLACEMENT_NEW(
+    iceoryx_client_abstraction, iceoryx_client_abstraction,
+    cleanupAfterError(), IceoryxClient, type_supports, iceoryx_client);
+  if(returnOnError)
+  {
+    return nullptr;
+  }
 
   rmw_client->implementation_identifier = rmw_get_implementation_identifier();
   rmw_client->data = iceoryx_client;
@@ -82,9 +125,8 @@ rmw_create_client(
     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);
   }
+  memcpy(const_cast<char *>(rmw_client->service_name), service_name, strlen(service_name) + 1);
 
   return rmw_client;
 }

diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp
@@ -48,18 +48,41 @@ rmw_create_service(
   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;
+  IceoryxServer * iceoryx_server_abstraction = nullptr;
+
+  bool returnOnError = false;
+
+  auto cleanupAfterError = [&](){
+    if (rmw_service) {
+      if (iceoryx_server) {
+        RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
+          iceoryx_server->~UntypedServer(), iox::popo::UntypedServer)
+        rmw_free(iceoryx_server);
+      }
+      if (iceoryx_server_abstraction) {
+        RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
+          iceoryx_server_abstraction->~IceoryxServer(), IceoryxServer)
+        rmw_free(iceoryx_server_abstraction);
+      }
+      if (rmw_service->service_name) {
+        rmw_free(const_cast<char *>(rmw_service->service_name));
+      }
+      rmw_service_free(rmw_service);
+      returnOnError = true;
+    }
+  };
 
   rmw_service = rmw_service_allocate();
   if (!rmw_service) {
     RMW_SET_ERROR_MSG("failed to allocate memory for service");
+    cleanupAfterError();
     return 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();
@@ -71,21 +94,39 @@ rmw_create_service(
     cleanupAfterError(), iox::popo::UntypedServer, service_description,
     iox::popo::ServerOptions{
       0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)});
+  if(returnOnError)
+  {
+    return nullptr;
+  }
+
+  iceoryx_server->offer();
+
+  iceoryx_server_abstraction =
+    static_cast<IceoryxServer *>(rmw_allocate(sizeof(IceoryxServer)));
+  if (!iceoryx_server_abstraction) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher");
+    cleanupAfterError();
+    return nullptr;
+  }
+  RMW_TRY_PLACEMENT_NEW(
+    iceoryx_server_abstraction, iceoryx_server_abstraction,
+    cleanupAfterError(), IceoryxServer, type_supports, iceoryx_server);
+  if(returnOnError)
+  {
+    return nullptr;
+  }
 
   rmw_service->implementation_identifier = rmw_get_implementation_identifier();
-  rmw_service->data = iceoryx_server;
+  rmw_service->data = iceoryx_server_abstraction;
 
   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 service name");
     cleanupAfterError();
     return nullptr;
-  } else {
-    memcpy(const_cast<char *>(rmw_service->service_name), service_name, strlen(service_name) + 1);
   }
-
-  /// @todo allocate IceoryxServer here and fill size_t message_size_ and message_alignment_;
+  memcpy(const_cast<char *>(rmw_service->service_name), service_name, strlen(service_name) + 1);
 
   return rmw_service;
 }