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..5e93eb6 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -147,14 +147,14 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; - if (!iceoryx_server_abstraction->request_payload_) { + if (iceoryx_server_abstraction->request_payload_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_); + iceoryx_server_abstraction->request_payload_map_[request_header->sequence_number]); iceoryx_server->loan( iceoryx_request_header, iceoryx_server_abstraction->response_size_, @@ -186,9 +186,11 @@ 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( + iceoryx_server_abstraction->request_payload_map_[request_header-> + sequence_number]); + iceoryx_server_abstraction->request_payload_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 + #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 request_payload_map_; }; #endif // TYPES__ICEORYX_SERVER_HPP_