Add fixed-size path for 'rmw_send_response' and 'rmw_take_response' (r…

…os2#76)

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

rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp

@@ -25,10 +25,13 @@
 #include "rosidl_typesupport_introspection_c/field_types.h"
 #include "rosidl_typesupport_introspection_c/identifier.h"
 #include "rosidl_typesupport_introspection_c/message_introspection.h"
+#include "rosidl_typesupport_introspection_c/service_introspection.h"
 
 #include "rosidl_typesupport_introspection_cpp/field_types.hpp"
 #include "rosidl_typesupport_introspection_cpp/identifier.hpp"
 #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp"
+
 
 #include "rcutils/error_handling.h"
 
@@ -349,6 +352,25 @@ void iceoryx_init_message(
   }
 }
 
+void iceoryx_init_message(
+  const rosidl_service_type_support_t * type_supports,
+  void * message)
+{
+  auto ts = get_type_support(type_supports);
+
+  if (ts.first == TypeSupportLanguage::CPP) {
+    auto members =
+      static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *>(ts.second->data);
+    members->request_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL);
+    return;
+  } else if (ts.first == TypeSupportLanguage::C) {
+    auto members =
+      static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *>(ts.second->data);
+    members->request_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL);
+    return;
+  }
+}
+
 void iceoryx_fini_message(
   const rosidl_message_type_support_t * type_supports,
   void * message)

rmw_iceoryx_cpp/src/rmw_request.cpp

@@ -20,47 +20,15 @@
 #include "./types/iceoryx_client.hpp"
 #include "./types/iceoryx_server.hpp"
 
+#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp"
+
 extern "C"
 {
-namespace details
-{
-/// @todo move this to the method as we need to store the ptr to the payload
-
-rmw_ret_t
-take_request(
-  iox::popo::UntypedServer * iceoryx_server,
-  const void * serialized_ros_msg,
-  size_t size)
-{
-  if (serialized_ros_msg == nullptr) {
-    RMW_SET_ERROR_MSG("serialized message pointer is null");
-    return RMW_RET_ERROR;
-  }
-  rmw_ret_t ret = RMW_RET_ERROR;
-  iceoryx_server->take()
-  .and_then(
-    [&](auto& requestPayload) {
-      // memcpy(userPayload, serialized_ros_msg, size);
-      // iceoryx_server->publish(userPayload);
-
-      // Hier die calculate response Methode aufrufen?
-      // nope eher sample abspeichern im struct
-      ret = RMW_RET_OK;
-    })
-  .or_else(
-    [&](auto&) {
-      RMW_SET_ERROR_MSG("take_request error!");
-      ret = RMW_RET_ERROR;
-    });
-  return ret;
-}
-}
-
 rmw_ret_t
 rmw_send_request(
   const rmw_client_t * client,
   const void * ros_request,
-  int64_t * sequence_id)
+  int64_t * sequence_id) // out going param
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR);
@@ -90,19 +58,21 @@ rmw_send_request(
   }
 
   iceoryx_client->loan(iceoryx_client_abstraction->message_size_, iceoryx_client_abstraction->message_alignment_)
-      .and_then([&](auto& requestPayload) {
+      .and_then([&](void * requestPayload) {
           auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload);
-          requestHeader->setSequenceId(*sequence_id);
+          requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_);
+          iceoryx_client_abstraction->sequence_id_ += 1;
           /// @todo memcpy or serialize the response
-          void * request;
-          // auto request = static_cast<AddRequest*>(requestPayload);
-          // request->augend = fibonacciLast;
-          // request->addend = fibonacciCurrent;
-          iceoryx_client->send(request).or_else(
+          // 1) init message
+          // 2) write ros_response to shared memory
+          memcpy(requestPayload, ros_request, iceoryx_client_abstraction->message_size_);
+          iceoryx_client->send(requestPayload).or_else(
               [&](auto& error) { ret = RMW_RET_ERROR; });
       })
       .or_else([&](auto& error) { ret = RMW_RET_ERROR; });
 
+  *sequence_id = iceoryx_client_abstraction->sequence_id_;
+
   return ret;
 }
 
@@ -135,26 +105,49 @@ rmw_take_request(
     return RMW_RET_ERROR;
   }
 
-  // if messages have a fixed size, we can just memcpy
-  if (iceoryx_service_abstraction->is_fixed_size_) {
-    return details::take_request(iceoryx_server, request_header, iceoryx_service_abstraction->message_size_);
-  }
-
   // 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_)) {
     RMW_SET_ERROR_MSG("Use either C typesupport or CPP typesupport");
     return RMW_RET_ERROR;
   }
 
-  // // message is neither loaned nor fixed size, so we have to serialize
-  // std::vector<char> payload_vector{};
+  const iox::mepoo::ChunkHeader * chunk_header = nullptr;
+  const iox::popo::RequestHeader * iceoryx_request_header = nullptr;
+  const void * user_payload = nullptr;
 
-  // rmw_iceoryx_cpp::serialize(ros_message, &iceoryx_publisher->type_supports_, payload_vector);
+  rmw_ret_t ret = RMW_RET_ERROR;
 
-  // // send composed payload
-  // return details::send_payload(iceoryx_sender, payload_vector.data(), payload_vector.size());
+  iceoryx_server->take()
+  .and_then(
+    [&](const void * requestPayload) {
+      user_payload = requestPayload;
+      chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload);
+      iceoryx_request_header = iox::popo::RequestHeader::fromPayload(user_payload);
+      ret = RMW_RET_OK;
+    })
+  .or_else(
+    [&](auto&) {
+      RMW_SET_ERROR_MSG("rmw_take_request error!");
+      ret = RMW_RET_ERROR;
+    });
+  return ret;
 
-  *taken = true;
-  return RMW_RET_OK;
+  // 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;
+  }
+
+  return ret;
 }
 }  // extern "C"

rmw_iceoryx_cpp/src/rmw_response.cpp

@@ -89,13 +89,15 @@ rmw_take_response(
 
   // if fixed size, we fetch the data via memcpy
   if (iceoryx_client_abstraction->is_fixed_size_) {
-    memcpy(ros_response, user_payload, chunk_header->userPayloadSize());
+    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),
+      static_cast<const char *>(user_payload), /// @todo add fourth param for 'request_header'
       &iceoryx_client_abstraction->type_supports_,
       ros_response);
     iceoryx_client->releaseResponse(user_payload);
@@ -138,16 +140,23 @@ rmw_send_response(
 
   auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_);
   iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->message_size_, iceoryx_service_abstraction->message_alignment_)
-      .and_then([&](auto& responsePayload) {
+      .and_then([&](void * responsePayload) {
           /// @todo memcpy or serialize the response
-          void * response;
-          // auto response = static_cast<AddResponse*>(responsePayload);
-          // response->sum = request->augend + request->addend;
-          iceoryx_server->send(response).or_else(
-              [&](auto& error) { ret = RMW_RET_ERROR; });
+          // 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->message_size_);
+          iceoryx_server->send(responsePayload).or_else(
+              [&](auto&) {
+                RMW_SET_ERROR_MSG("rmw_send_response send error!");
+                ret = RMW_RET_ERROR;
+              });
       })
       .or_else(
-          [&](auto& error) { ret = RMW_RET_ERROR; });
+          [&](auto&) {
+            RMW_SET_ERROR_MSG("rmw_send_response loan error!");
+            ret = RMW_RET_ERROR;
+          });
 
   return ret;
 }

rmw_iceoryx_cpp/src/rmw_service.cpp

@@ -85,6 +85,8 @@ rmw_create_service(
     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_;
+
   return rmw_service;
 }
 

rmw_iceoryx_cpp/src/types/iceoryx_client.hpp

@@ -38,7 +38,7 @@ struct IceoryxClient
   bool is_fixed_size_;
   size_t message_size_;
   uint32_t message_alignment_;
-  /// @todo add sample here to take the the response later? nope im server!
+  uint64_t sequence_id_;
 };
 
 #endif  // TYPES__ICEORYX_CLIENT_HPP_

rmw_iceoryx_cpp/src/types/iceoryx_server.hpp

@@ -37,7 +37,8 @@ struct IceoryxServer
   iox::popo::UntypedServer * const iceoryx_server_;
   bool is_fixed_size_;
   size_t message_size_;
-  uint32_t message_alignment_;
+  /// @todo Is there a way to get the aligment for a complete type via the rosidl?
+  uint32_t message_alignment_{8};
   void * request_payload_;
 };