diff --git a/rmw_iceoryx_cpp/CMakeLists.txt b/rmw_iceoryx_cpp/CMakeLists.txt index 6968d78..ede9f41 100644 --- a/rmw_iceoryx_cpp/CMakeLists.txt +++ b/rmw_iceoryx_cpp/CMakeLists.txt @@ -6,12 +6,11 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Set C++ standard in accordance with iceoryx -# https://github.com/eclipse-iceoryx/iceoryx/blob/v2.0.3/iceoryx_hoofs/cmake/IceoryxPlatform.cmake#L26-L38 +# Set C++ standard in accordance with iceoryx's 'ICEORYX_CXX_STANDARD' CMake variable if(LINUX) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) elseif(QNX) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) elseif(WIN32) set(CMAKE_CXX_STANDARD 17) elseif(APPLE) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp index 6636524..be3f8dd 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp @@ -22,7 +22,7 @@ #include "iceoryx_posh/capro/service_description.hpp" #include "iceoryx_hoofs/cxx/string.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" struct rosidl_message_type_support_t; struct rosidl_service_type_support_t; diff --git a/rmw_iceoryx_cpp/src/rmw_init.cpp b/rmw_iceoryx_cpp/src/rmw_init.cpp index 84f5806..f5c9b43 100644 --- a/rmw_iceoryx_cpp/src/rmw_init.cpp +++ b/rmw_iceoryx_cpp/src/rmw_init.cpp @@ -17,7 +17,7 @@ #include #include "iceoryx_posh/runtime/posh_runtime.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" #include "rcutils/error_handling.h" #include "rcutils/process.h" diff --git a/rmw_iceoryx_cpp/src/rmw_node.cpp b/rmw_iceoryx_cpp/src/rmw_node.cpp index d0c5835..a74c100 100644 --- a/rmw_iceoryx_cpp/src/rmw_node.cpp +++ b/rmw_iceoryx_cpp/src/rmw_node.cpp @@ -20,7 +20,7 @@ #include "rmw/allocators.h" #include "./types/iceoryx_node.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" extern "C" { diff --git a/rmw_iceoryx_cpp/src/rmw_publisher.cpp b/rmw_iceoryx_cpp/src/rmw_publisher.cpp index b371234..9eac5da 100644 --- a/rmw_iceoryx_cpp/src/rmw_publisher.cpp +++ b/rmw_iceoryx_cpp/src/rmw_publisher.cpp @@ -25,7 +25,7 @@ #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" #include "./types/iceoryx_publisher.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" extern "C" { diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index b8d9ba5..77cd5ea 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 96a8fc1..8d2dd50 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/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index d09a932..364f890 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -23,7 +23,7 @@ #include "types/iceoryx_server.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" extern "C" { diff --git a/rmw_iceoryx_cpp/src/rmw_subscription.cpp b/rmw_iceoryx_cpp/src/rmw_subscription.cpp index 14c4be0..86881e6 100644 --- a/rmw_iceoryx_cpp/src/rmw_subscription.cpp +++ b/rmw_iceoryx_cpp/src/rmw_subscription.cpp @@ -24,7 +24,7 @@ #include "rmw/rmw.h" #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" -#include "iceoryx_dust/cxx/std_string_support.hpp" +#include "iox/std_string_support.hpp" #include "./types/iceoryx_subscription.hpp" diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index b077098..0dd1ece 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_{false}; size_t response_size_{0}; - /// @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_