Remove debug prints and add de-serialization to 'rmw_send_response' (r…

…os2#76) Signed-off-by: Simon Hoinkis <[email protected]>
ApexAI · Jan 5, 2023 · 1d4b469 · 1d4b469
1 parent 998edd1
commit 1d4b469
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 32 deletions.
diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp
@@ -127,7 +127,6 @@ rmw_create_client(
     return nullptr;
   }
   memcpy(const_cast<char *>(rmw_client->service_name), service_name, strlen(service_name) + 1);
-  std::cout << "RMW Client created!" << std::endl;
   return rmw_client;
 }
 
@@ -168,7 +167,6 @@ rmw_destroy_client(
   client->service_name = nullptr;
 
   rmw_client_free(client);
-  std::cout << "RMW Client destroyed!" << std::endl;
   return result;
 }
 

diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp
@@ -20,16 +20,16 @@
 #include "./types/iceoryx_client.hpp"
 #include "./types/iceoryx_server.hpp"
 
-#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp"
 #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp"
+#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp"
 
 extern "C"
 {
 rmw_ret_t
 rmw_send_request(
   const rmw_client_t * client,
   const void * ros_request,
-  int64_t * sequence_id) // out going param
+  int64_t * sequence_id)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR);
@@ -58,8 +58,6 @@ rmw_send_request(
     return ret;
   }
 
-  std::cout << "I'm alive!" << std::endl;
-
   iceoryx_client->loan(iceoryx_client_abstraction->request_size_, iceoryx_client_abstraction->request_alignment_)
       .and_then([&](void * requestPayload) {
           auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload);
@@ -70,15 +68,12 @@ rmw_send_request(
 
           if (iceoryx_client_abstraction->is_fixed_size_)
           {
-            std::cout << "FIXED SIZE!" << std::endl;
             memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_);
           }
           else
           {
-            std::cout << "NOT FIXED SIZE!" << std::endl;
             // message is not fixed size, so we have to serialize
             std::vector<char> payload_vector{};
-
             rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector);
             memcpy(requestPayload, payload_vector.data(), payload_vector.size());
           }
@@ -153,23 +148,23 @@ rmw_take_request(
       RMW_SET_ERROR_MSG("rmw_take_request error!");
       ret = RMW_RET_ERROR;
     });
-  return ret;
+
+  if (ret == RMW_RET_ERROR) {
+    return ret;
+  }
 
   // if fixed size, we fetch the data via memcpy
   if (iceoryx_service_abstraction->is_fixed_size_) {
     memcpy(ros_request, user_payload, chunk_header->userPayloadSize());
-    iceoryx_server->releaseRequest(user_payload);
-    *taken = true;
-    ret = RMW_RET_OK;
   } else {
     rmw_iceoryx_cpp::deserialize(
       static_cast<const char *>(user_payload),
       &iceoryx_service_abstraction->type_supports_,
       ros_request);
-    iceoryx_server->releaseRequest(user_payload);
-    *taken = true;
-    ret = RMW_RET_OK;
   }
+  iceoryx_server->releaseRequest(user_payload);
+  *taken = true;
+  ret = RMW_RET_OK;
 
   return ret;
 }

diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp
@@ -18,6 +18,7 @@
 #include "rmw/impl/cpp/macros.hpp"
 #include "rmw/rmw.h"
 
+#include "rmw_iceoryx_cpp/iceoryx_serialize.hpp"
 #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp"
 
 #include "./types/iceoryx_client.hpp"
@@ -92,18 +93,15 @@ rmw_take_response(
     memcpy(request_header, user_payload, sizeof(*request_header));
     /// @todo cast to uint8_t before doing pointer arithmetic?
     memcpy(ros_response, user_payload + sizeof(*request_header), chunk_header->userPayloadSize());
-    iceoryx_client->releaseResponse(user_payload);
-    *taken = true;
-    ret = RMW_RET_OK;
   } else {
     rmw_iceoryx_cpp::deserialize(
-      static_cast<const char *>(user_payload), /// @todo add fourth param for 'request_header'
+      static_cast<const char *>(user_payload), /// @todo add fourth param for 'request_header', but how to find out when header ends, separator char?
       &iceoryx_client_abstraction->type_supports_,
       ros_response);
-    iceoryx_client->releaseResponse(user_payload);
-    *taken = true;
-    ret = RMW_RET_OK;
   }
+  iceoryx_client->releaseResponse(user_payload);
+  *taken = true;
+  ret = RMW_RET_OK;
 
   *taken = false;
   return ret;
@@ -124,28 +122,38 @@ rmw_send_response(
     : service, service->implementation_identifier,
     rmw_get_implementation_identifier(), return RMW_RET_ERROR);
 
-  auto iceoryx_service_abstraction = static_cast<IceoryxServer *>(service->data);
-  if (!iceoryx_service_abstraction) {
+  auto iceoryx_server_abstraction = static_cast<IceoryxServer *>(service->data);
+  if (!iceoryx_server_abstraction) {
     RMW_SET_ERROR_MSG("service data is null");
     return RMW_RET_ERROR;
   }
 
-  auto iceoryx_server = iceoryx_service_abstraction->iceoryx_server_;
+  auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_;
   if (!iceoryx_server) {
     RMW_SET_ERROR_MSG("iceoryx_server is null");
     return RMW_RET_ERROR;
   }
 
   rmw_ret_t ret = RMW_RET_ERROR;
 
-  auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_);
-  iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->response_size_, iceoryx_service_abstraction->response_alignment_)
+  auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_);
+  iceoryx_server->loan(requestHeader, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_)
       .and_then([&](void * responsePayload) {
           /// @todo memcpy or serialize the response
-          // 1) init message like pub/sub?
-          // 2) write | request_header | ros_response | to shared memory
-          memcpy(responsePayload, request_header, sizeof(*request_header));
-          memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_service_abstraction->response_size_);
+          // write |-request_header-|-ros_response-| to shared memory
+          if (iceoryx_server_abstraction->is_fixed_size_)
+          {
+            memcpy(responsePayload, request_header, sizeof(*request_header));
+            memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_server_abstraction->response_size_);
+          }
+          else
+          {
+            // message is not fixed size, so we have to serialize
+            std::vector<char> payload_vector{};
+            rmw_iceoryx_cpp::serializeRequest(request_header, &iceoryx_server_abstraction->type_supports_, payload_vector);
+            rmw_iceoryx_cpp::serializeRequest(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector);
+            memcpy(responsePayload, payload_vector.data(), payload_vector.size());
+          }
           iceoryx_server->send(responsePayload).or_else(
               [&](auto&) {
                 RMW_SET_ERROR_MSG("rmw_send_response send error!");