Implement 'rmw_send_request', 'rmw_take_request' and 'rmw_take_response'

(ros2#76)

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

rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp

@@ -16,6 +16,7 @@
 #define RMW_ICEORYX_CPP__ICEORYX_DESERIALIZE_HPP_
 
 struct rosidl_message_type_support_t;
+struct rosidl_service_type_support_t;
 
 namespace rmw_iceoryx_cpp
 {
@@ -26,5 +27,11 @@ void deserialize(
   const rosidl_message_type_support_t * type_supports,
   void * ros_message);
 
+/// @todo use a variant here for the 2nd parameter to avoid code duplication
+void deserialize(
+  const char * serialized_msg,
+  const rosidl_service_type_support_t * type_supports,
+  void * ros_message);
+
 }  // namespace rmw_iceoryx_cpp
 #endif  // RMW_ICEORYX_CPP__ICEORYX_DESERIALIZE_HPP_

rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp

@@ -47,6 +47,8 @@ bool iceoryx_is_fixed_size(const rosidl_service_type_support_t * type_supports);
 
 bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports);
 
+bool iceoryx_is_valid_type_support(const rosidl_service_type_support_t * type_supports);
+
 size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_supports);
 
 size_t iceoryx_get_message_size(const rosidl_service_type_support_t * type_supports);

rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp

@@ -17,6 +17,7 @@
 
 #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/identifier.hpp"
 #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
@@ -50,4 +51,22 @@ void deserialize(
   }
 }
 
+void deserialize(
+  const char * serialized_msg,
+  const rosidl_service_type_support_t * type_supports,
+  void * ros_message)
+{
+  auto ts = get_type_support(type_supports);
+
+  if (ts.first == TypeSupportLanguage::CPP) {
+    auto members_cpp =
+      static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *>(ts.second->data);
+    rmw_iceoryx_cpp::details_cpp::deserializeResponse(serialized_msg, members_cpp, ros_message);
+  } else if (ts.first == TypeSupportLanguage::C) {
+    auto members_c =
+      static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *>(ts.second->data);
+    rmw_iceoryx_cpp::details_c::deserializeResponse(serialized_msg, members_c, ros_message);
+  }
+}
+
 }  // namespace rmw_iceoryx_cpp

rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp

@@ -239,6 +239,22 @@ const char * deserialize(
   return serialized_msg;
 }
 
+const char* deserializeResponse(
+  const char * serialized_msg,
+  const rosidl_typesupport_introspection_c__ServiceMembers * service_members,
+  void * ros_message)
+{
+  return deserialize(serialized_msg, service_members->response_members_, ros_message);
+}
+
+const char* deserializeRequest(
+  const char * serialized_msg,
+  const rosidl_typesupport_introspection_c__ServiceMembers * service_members,
+  void * ros_message)
+{
+  return deserialize(serialized_msg, service_members->request_members_, ros_message);
+}
+
 }  // namespace details_c
 }  // namespace rmw_iceoryx_cpp
 #endif  // INTERNAL__ICEORYX_DESERIALIZE_TYPESUPPORT_C_HPP_

rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp

@@ -24,7 +24,7 @@
 #include <vector>
 
 #include "rosidl_typesupport_introspection_cpp/field_types.hpp"
-#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp"
 
 #include "./iceoryx_serialization_common.hpp"
 
@@ -309,6 +309,22 @@ const char * deserialize(
   return serialized_msg;
 }
 
+const char* deserializeResponse(
+  const char * serialized_msg,
+  const rosidl_typesupport_introspection_cpp::ServiceMembers * service_members,
+  void * ros_message)
+{
+  return deserialize(serialized_msg, service_members->response_members_, ros_message);
+}
+
+const char* deserializeRequest(
+  const char * serialized_msg,
+  const rosidl_typesupport_introspection_cpp::ServiceMembers * service_members,
+  void * ros_message)
+{
+  return deserialize(serialized_msg, service_members->request_members_, ros_message);
+}
+
 }  // namespace details_cpp
 }  // namespace rmw_iceoryx_cpp
 #endif  // INTERNAL__ICEORYX_DESERIALIZE_TYPESUPPORT_CPP_HPP_

rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp

@@ -319,6 +319,17 @@ bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_su
   return true;
 }
 
+bool iceoryx_is_valid_type_support(const rosidl_service_type_support_t * type_supports)
+{
+  try {
+    get_type_support(type_supports);
+  } catch (...) {
+    return false;
+  }
+
+  return true;
+}
+
 void iceoryx_init_message(
   const rosidl_message_type_support_t * type_supports,
   void * message)

rmw_iceoryx_cpp/src/rmw_publish.cpp

@@ -1,5 +1,5 @@
 // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved.
-// Copyright (c) 2021 by Apex.AI Inc. All rights reserved.
+// Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

rmw_iceoryx_cpp/src/rmw_request.cpp

@@ -17,9 +17,45 @@
 #include "rmw/impl/cpp/macros.hpp"
 #include "rmw/rmw.h"
 
+#include "./types/iceoryx_client.hpp"
+#include "./types/iceoryx_server.hpp"
+
 extern "C"
 {
-/// @todo Use the new request/response API of iceoryx v2.0 here
+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,
@@ -28,13 +64,48 @@ rmw_send_request(
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR);
-  RCUTILS_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_ERROR);  // can be null?
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_ERROR);
+
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_send_request
+    : client,
+    client->implementation_identifier,
+    rmw_get_implementation_identifier(),
+    return RMW_RET_ERROR);
+
+  rmw_ret_t ret = RMW_RET_ERROR;
 
-  RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support requests.");
-  return RMW_RET_UNSUPPORTED;
+  auto iceoryx_client_abstraction = static_cast<IceoryxClient *>(client->data);
+  if (!iceoryx_client_abstraction) {
+    RMW_SET_ERROR_MSG("client data is null");
+    ret = RMW_RET_ERROR;
+    return ret;
+  }
+
+  auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_;
+  if (!iceoryx_client) {
+    RMW_SET_ERROR_MSG("iceoryx_client is null");
+    ret = RMW_RET_ERROR;
+    return ret;
+  }
+
+  iceoryx_client->loan(iceoryx_client_abstraction->message_size_, iceoryx_client_abstraction->message_alignment_)
+      .and_then([&](auto& requestPayload) {
+          auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload);
+          requestHeader->setSequenceId(*sequence_id);
+          /// @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(
+              [&](auto& error) { ret = RMW_RET_ERROR; });
+      })
+      .or_else([&](auto& error) { ret = RMW_RET_ERROR; });
+
+  return ret;
 }
 
-/// @todo Use the new request/response API of iceoryx v2.0 here
 rmw_ret_t
 rmw_take_request(
   const rmw_service_t * service,
@@ -47,7 +118,43 @@ rmw_take_request(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR);
 
-  RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support requests.");
-  return RMW_RET_UNSUPPORTED;
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    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) {
+    RMW_SET_ERROR_MSG("service data is null");
+    return RMW_RET_ERROR;
+  }
+
+  auto iceoryx_server = iceoryx_service_abstraction->iceoryx_server_;
+  if (!iceoryx_server) {
+    RMW_SET_ERROR_MSG("iceoryx_server is null");
+    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{};
+
+  // rmw_iceoryx_cpp::serialize(ros_message, &iceoryx_publisher->type_supports_, payload_vector);
+
+  // // send composed payload
+  // return details::send_payload(iceoryx_sender, payload_vector.data(), payload_vector.size());
+
+  *taken = true;
+  return RMW_RET_OK;
 }
 }  // extern "C"