Properly save the sample in 'rwm_take_request' (ros2#76)

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

rmw_iceoryx_cpp/src/rmw_request.cpp

@@ -77,19 +77,20 @@ rmw_send_request(
             rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector);
             memcpy(requestPayload, payload_vector.data(), payload_vector.size());
           }
-          std::cout << "Client sent request!" << std::endl;
-          iceoryx_client->send(requestPayload).or_else(
-              [&](auto&) {
-                RMW_SET_ERROR_MSG("rmw_send_request error!");
-                ret = RMW_RET_ERROR;
-              });
+          iceoryx_client->send(requestPayload).and_then([&]{
+            std::cout << "Client sent request!" << std::endl;
+            ret = RMW_RET_OK;
+          }).or_else(
+            [&](auto&) {
+              RMW_SET_ERROR_MSG("rmw_send_request error!");
+              ret = RMW_RET_ERROR;
+            });
       })
       .or_else([&](auto&) {
         RMW_SET_ERROR_MSG("rmw_send_request error!");
         ret = RMW_RET_ERROR;
       });
 
-  ret = RMW_RET_OK;
   return ret;
 }
 
@@ -110,20 +111,20 @@ rmw_take_request(
     : 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;
   }
 
   // this should never happen if checked already at rmw_create_service
-  if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_service_abstraction->type_supports_)) {
+  if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_server_abstraction->type_supports_)) {
     RMW_SET_ERROR_MSG("Use either C typesupport or CPP typesupport");
     return RMW_RET_ERROR;
   }
@@ -155,21 +156,23 @@ rmw_take_request(
   }
 
   // if fixed size, we fetch the data via memcpy
-  if (iceoryx_service_abstraction->is_fixed_size_) {
+  if (iceoryx_server_abstraction->is_fixed_size_) {
     memcpy(ros_request, user_payload, chunk_header->userPayloadSize());
   } else {
     rmw_iceoryx_cpp::deserializeRequest(
       static_cast<const char *>(user_payload),
-      &iceoryx_service_abstraction->type_supports_,
+      &iceoryx_server_abstraction->type_supports_,
       ros_request);
   }
   request_header->source_timestamp = 0; // Unsupported until needed
   rcutils_system_time_now(&request_header->received_timestamp);
   request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId();
   request_header->request_id.writer_guid[0] = 42; /// @todo
 
+  // Hold the loaned request till we send the response in 'rmw_send_response'
+  iceoryx_server_abstraction->request_payload_ = user_payload;
+
   *taken = true;
-  ret = RMW_RET_OK;
   std::cout << "Server took request!" << std::endl;
 
   return ret;

rmw_iceoryx_cpp/src/rmw_response.cpp

@@ -105,7 +105,6 @@ rmw_take_response(
   }
   iceoryx_client->releaseResponse(user_payload);
   *taken = true;
-  ret = RMW_RET_OK;
   std::cout << "Client took response!" << std::endl;
 
   return ret;
@@ -140,10 +139,12 @@ rmw_send_response(
 
   rmw_ret_t ret = RMW_RET_ERROR;
 
-  auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_);
-  requestHeader->setSequenceId(request_header->sequence_number);
+  auto* iceoryx_request_header = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_);
+  /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, we need to compare
+  ///       the user-provided sequence id with the one from the 'iceoryx_request_header'
+  //iceoryx_request_header->setSequenceId(request_header->sequence_number);
 
-  iceoryx_server->loan(requestHeader, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_)
+  iceoryx_server->loan(iceoryx_request_header, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_)
       .and_then([&](void * responsePayload) {
           if (iceoryx_server_abstraction->is_fixed_size_)
           {
@@ -156,8 +157,10 @@ rmw_send_response(
             rmw_iceoryx_cpp::serializeResponse(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector);
             memcpy(responsePayload, payload_vector.data(), payload_vector.size());
           }
-          std::cout << "Server sent response!" << std::endl;
-          iceoryx_server->send(responsePayload).or_else(
+          iceoryx_server->send(responsePayload).and_then([&]{
+            std::cout << "Server sent response!" << std::endl;
+            ret = RMW_RET_OK;
+          }).or_else(
               [&](auto&) {
                 RMW_SET_ERROR_MSG("rmw_send_response send error!");
                 ret = RMW_RET_ERROR;
@@ -170,6 +173,7 @@ rmw_send_response(
             ret = RMW_RET_ERROR;
           });
 
+  // Release the hold request
   iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_);
 
   return ret;

rmw_iceoryx_cpp/src/types/iceoryx_client.hpp

@@ -39,7 +39,7 @@ struct IceoryxClient
   size_t request_size_;
   /// @todo How can the full type be aquired via rosidl to do an 'alignof()'?
   uint32_t request_alignment_{8};
-  uint64_t sequence_id_;
+  int64_t sequence_id_;
 };
 
 #endif  // TYPES__ICEORYX_CLIENT_HPP_

rmw_iceoryx_cpp/src/types/iceoryx_server.hpp

@@ -39,7 +39,7 @@ struct IceoryxServer
   size_t response_size_;
   /// @todo Is there a way to get the aligment for a complete type via the rosidl?
   uint32_t response_alignment_{8};
-  void * request_payload_;
+  const void * request_payload_;
 };
 
 #endif  // TYPES__ICEORYX_SERVER_HPP_