From b1865097892612e077d93eafbf5b290f8bb87478 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 11 Jan 2023 22:51:49 +0100 Subject: [PATCH] Move the code to inside the happy-path lambda for client server communication (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 84 +++++++++++++--------------- rmw_iceoryx_cpp/src/rmw_response.cpp | 40 ++++++------- 2 files changed, 58 insertions(+), 66 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 4758e76..573fae0 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -134,63 +134,59 @@ rmw_take_request( return RMW_RET_ERROR; } - const iox::mepoo::ChunkHeader * chunk_header = nullptr; - const iox::popo::RequestHeader * iceoryx_request_header = nullptr; - const void * user_payload = nullptr; - rmw_ret_t ret = RMW_RET_ERROR; iceoryx_server->take() .and_then( [&](const void * iceoryx_request_payload) { - user_payload = iceoryx_request_payload; - chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); - iceoryx_request_header = iox::popo::RequestHeader::fromPayload(user_payload); + const iox::mepoo::ChunkHeader * chunk_header = + iox::mepoo::ChunkHeader::fromUserPayload( + iceoryx_request_payload); + const iox::popo::RequestHeader * iceoryx_request_header = + iox::popo::RequestHeader::fromPayload( + iceoryx_request_payload); + + request_header->source_timestamp = 0; // Unsupported until needed + ret = rcutils_system_time_now(&request_header->received_timestamp); + if (ret != RMW_RET_OK) { + return; + } + request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); + auto typed_guid = iceoryx_server->getUid(); + iox::popo::UniquePortId::value_type guid = + static_cast(typed_guid); + size_t size = sizeof(guid); + auto max_rmw_storage = sizeof(request_header->request_id.writer_guid); + if (!typed_guid.isValid() || size > max_rmw_storage) { + RMW_SET_ERROR_MSG("Could not write server guid"); + ret = RMW_RET_ERROR; + return; + } + memcpy(request_header->request_id.writer_guid, &guid, size); + + // if fixed size, we fetch the data via memcpy + if (iceoryx_server_abstraction->is_fixed_size_) { + memcpy(ros_request, iceoryx_request_payload, chunk_header->userPayloadSize()); + } else { + rmw_iceoryx_cpp::deserializeRequest( + static_cast(iceoryx_request_payload), + &iceoryx_server_abstraction->type_supports_, + ros_request); + } + + // Hold the loaned request till we send the response in 'rmw_send_response' + iceoryx_server_abstraction->request_payload_ = iceoryx_request_payload; + + *taken = true; ret = RMW_RET_OK; }) .or_else( - [&](iox::popo::ServerRequestResult & error) { + [&](auto &) { *taken = false; RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; }); - if (ret == RMW_RET_ERROR) { - return ret; - } - - // if fixed size, we fetch the data via memcpy - if (iceoryx_server_abstraction->is_fixed_size_) { - memcpy(ros_request, user_payload, chunk_header->userPayloadSize()); - } else { - rmw_iceoryx_cpp::deserializeRequest( - static_cast(user_payload), - &iceoryx_server_abstraction->type_supports_, - ros_request); - } - request_header->source_timestamp = 0; // Unsupported until needed - ret = rcutils_system_time_now(&request_header->received_timestamp); - if (ret != RMW_RET_OK) { - return ret; - } - request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); - - auto typed_guid = iceoryx_server->getUid(); - iox::popo::UniquePortId::value_type guid = - static_cast(typed_guid); - size_t size = sizeof(guid); - auto max_rmw_storage = sizeof(request_header->request_id.writer_guid); - if (!typed_guid.isValid() || size > max_rmw_storage) { - RMW_SET_ERROR_MSG("Could not write server guid"); - ret = RMW_RET_ERROR; - return ret; - } - memcpy(request_header->request_id.writer_guid, &guid, size); - - // Hold the loaned request till we send the response in 'rmw_send_response' - iceoryx_server_abstraction->request_payload_ = user_payload; - - *taken = true; return ret; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 0b52fc9..52935a8 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -57,8 +57,6 @@ rmw_take_response( return RMW_RET_ERROR; } - const iox::mepoo::ChunkHeader * chunk_header = nullptr; - const void * user_payload = nullptr; rmw_ret_t ret = RMW_RET_ERROR; iceoryx_client->take() @@ -70,12 +68,12 @@ rmw_take_response( if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { - user_payload = iceoryx_response_payload; - chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + const iox::mepoo::ChunkHeader * chunk_header = + iox::mepoo::ChunkHeader::fromUserPayload(iceoryx_response_payload); auto typed_guid = chunk_header->originId(); iox::popo::UniquePortId::value_type guid = - static_cast(typed_guid); + static_cast(typed_guid); size_t size = sizeof(guid); auto max_rmw_storage = sizeof(request_header->request_id.writer_guid); if (!typed_guid.isValid() || size > max_rmw_storage) { @@ -90,6 +88,19 @@ rmw_take_response( if (ret != RMW_RET_OK) { return; } + + // if fixed size, we fetch the data via memcpy + if (iceoryx_client_abstraction->is_fixed_size_) { + memcpy(ros_response, iceoryx_response_payload, chunk_header->userPayloadSize()); + } else { + rmw_iceoryx_cpp::deserializeResponse( + static_cast(iceoryx_response_payload), + &iceoryx_client_abstraction->type_supports_, + ros_response); + } + iceoryx_client->releaseResponse(iceoryx_response_payload); + + *taken = true; ret = RMW_RET_OK; } else { RMW_SET_ERROR_MSG("Got response with outdated sequence number!"); @@ -98,27 +109,12 @@ rmw_take_response( } }) .or_else( - [&](iox::popo::ChunkReceiveResult) { + [&](auto &) { *taken = false; RMW_SET_ERROR_MSG("No chunk in iceoryx_client"); ret = RMW_RET_ERROR; }); - if (ret == RMW_RET_ERROR) { - return ret; - } - - // if fixed size, we fetch the data via memcpy - if (iceoryx_client_abstraction->is_fixed_size_) { - memcpy(ros_response, user_payload, chunk_header->userPayloadSize()); - } else { - rmw_iceoryx_cpp::deserializeResponse( - static_cast(user_payload), - &iceoryx_client_abstraction->type_supports_, - ros_response); - } - iceoryx_client->releaseResponse(user_payload); - *taken = true; return ret; } @@ -189,7 +185,7 @@ rmw_send_response( std::this_thread::sleep_for(std::chrono::milliseconds(100)); }) .or_else( - [&](auto & error) { + [&](auto &) { RMW_SET_ERROR_MSG("rmw_send_response loan error!"); ret = RMW_RET_ERROR; });