From 9fdcf4152d71579c0239268ce00e6e94744771e9 Mon Sep 17 00:00:00 2001
From: Simon Hoinkis <simon.hoinkis@apex.ai>
Date: Fri, 5 Jan 2024 11:15:31 +0100
Subject: [PATCH] Address known limitation in service/client by using a
 'std::map' to store the sample pointers (#105) (#106)

Signed-off-by: Simon Hoinkis <simon.hoinkis@apex.ai>
---
 rmw_iceoryx_cpp/src/rmw_request.cpp          | 14 ++++++++++++--
 rmw_iceoryx_cpp/src/rmw_response.cpp         | 20 ++++++++++++++------
 rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 15 +++++++++------
 3 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp
index 01032e0..ce7e658 100644
--- a/rmw_iceoryx_cpp/src/rmw_request.cpp
+++ b/rmw_iceoryx_cpp/src/rmw_request.cpp
@@ -175,7 +175,17 @@ rmw_take_request(
       }
 
       // Hold the loaned request till we send the response in 'rmw_send_response'
-      iceoryx_server_abstraction->request_payload_ = iceoryx_request_payload;
+      auto result =
+      iceoryx_server_abstraction->request_payload_map_.insert(
+        {request_header->request_id.
+          sequence_number, iceoryx_request_payload});
+
+      if (result.second == false) {
+        *taken = false;
+        RMW_SET_ERROR_MSG("rmw_take_request: Could not store the sample pointer in the map!");
+        ret = RMW_RET_ERROR;
+        return;
+      }
 
       *taken = true;
       ret = RMW_RET_OK;
@@ -183,7 +193,7 @@ rmw_take_request(
   .or_else(
     [&](auto &) {
       *taken = false;
-      RMW_SET_ERROR_MSG("rmw_take_request error!");
+      RMW_SET_ERROR_MSG("rmw_take_request: Taking the sample failed!");
       ret = RMW_RET_ERROR;
     });
 
diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp
index ae817fc..ff3fbc9 100644
--- a/rmw_iceoryx_cpp/src/rmw_response.cpp
+++ b/rmw_iceoryx_cpp/src/rmw_response.cpp
@@ -146,15 +146,23 @@ rmw_send_response(
   }
 
   rmw_ret_t ret = RMW_RET_ERROR;
+  auto & payload_ptr_map = iceoryx_server_abstraction->request_payload_map_;
 
-  if (!iceoryx_server_abstraction->request_payload_) {
+  if (payload_ptr_map.empty()) {
     RMW_SET_ERROR_MSG("'rmw_take_request' needs to be called before 'rmw_send_response'!");
     ret = RMW_RET_ERROR;
     return ret;
   }
 
-  auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload(
-    iceoryx_server_abstraction->request_payload_);
+  auto it = payload_ptr_map.find(request_header->sequence_number);
+
+  if (it == payload_ptr_map.end()) {
+    RMW_SET_ERROR_MSG("Could not find the payload pointer in the map");
+    ret = RMW_RET_ERROR;
+    return ret;
+  }
+
+  auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload((*it).second);
 
   iceoryx_server->loan(
     iceoryx_request_header, iceoryx_server_abstraction->response_size_,
@@ -186,9 +194,9 @@ rmw_send_response(
       ret = RMW_RET_ERROR;
     });
 
-  // Release the hold request
-  iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_);
-  iceoryx_server_abstraction->request_payload_ = nullptr;
+  // Release the hold request and delete the entry from the map
+  iceoryx_server->releaseRequest((*it).second);
+  payload_ptr_map.erase(request_header->sequence_number);
 
   return ret;
 }
diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp
index d44a34d..2073fef 100644
--- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp
+++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp
@@ -15,6 +15,8 @@
 #ifndef TYPES__ICEORYX_SERVER_HPP_
 #define TYPES__ICEORYX_SERVER_HPP_
 
+#include <map>
+
 #include "iceoryx_posh/popo/untyped_server.hpp"
 
 #include "rmw/rmw.h"
@@ -31,17 +33,18 @@ struct IceoryxServer
     iceoryx_server_(iceoryx_server),
     is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)),
     response_size_(rmw_iceoryx_cpp::iceoryx_get_response_size(type_supports))
-  {}
+  {
+  }
 
   rosidl_service_type_support_t type_supports_;
   iox::popo::UntypedServer * const iceoryx_server_;
   bool is_fixed_size_;
   size_t response_size_;
-  /// @todo The request payload pointer could also be added to 'rmw_request_id_t' if accepeted in
-  /// ros2/rmw. For now, the limitation exists to always call 'rmw_take_request' followed by
-  /// 'rmw_send_response' and not call e.g. 2x times 'rmw_take_request' and then
-  /// 2x 'rmw_send_response'
-  const void * request_payload_{nullptr};
+  /// @brief The map stores the sequence numbers together with the corresponding
+  ///        sample pointer pointing to the shared memory. This is due to the fact that
+  ///        'rmw_request_id_t' misses a place to store the sample pointer, which is not
+  ///        typical with DDS implementations.
+  std::map<int64_t, const void *> request_payload_map_;
 };
 
 #endif  // TYPES__ICEORYX_SERVER_HPP_