From c9006a886a971f106d256a14a7053b8f8e89bec3 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 31 Mar 2022 14:22:57 +0200 Subject: [PATCH 01/27] Implement create and destroy methods for service and client (#76) Signed-off-by: Simon Hoinkis --- .../iceoryx_name_conversion.hpp | 6 ++ .../src/internal/iceoryx_name_conversion.cpp | 30 +++++-- rmw_iceoryx_cpp/src/rmw_client.cpp | 81 ++++++++++++++++++- rmw_iceoryx_cpp/src/rmw_service.cpp | 49 ++++++++++- 4 files changed, 153 insertions(+), 13 deletions(-) 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 7abe16e..5796e53 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 @@ -23,6 +23,7 @@ #include "iceoryx_hoofs/cxx/string.hpp" struct rosidl_message_type_support_t; +struct rosidl_service_type_support_t; namespace rmw_iceoryx_cpp { @@ -71,5 +72,10 @@ get_iceoryx_service_description( const std::string & topic, const rosidl_message_type_support_t * type_supports); +iox::capro::ServiceDescription +get_iceoryx_service_description( + const std::string & topic, + const rosidl_service_type_support_t * type_supports); + } // namespace rmw_iceoryx_cpp #endif // RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_ diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp index f85112a..69afa3a 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp @@ -21,6 +21,7 @@ #include "rcpputils/split.hpp" #include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_cpp/service_type_support.hpp" #include "rosidl_typesupport_introspection_c/field_types.h" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -159,6 +160,16 @@ get_service_description_from_name_n_type( return std::make_tuple(service, instance, event); } +iox::capro::ServiceDescription make_service_description(const std::string & topic_name, const std::string & type_name) +{ + auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name); + + return iox::capro::ServiceDescription( + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)), + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)), + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple))); +} + iox::capro::ServiceDescription get_iceoryx_service_description( const std::string & topic_name, @@ -169,12 +180,21 @@ get_iceoryx_service_description( extract_type(type_supports, package_name, type_name); type_name = package_name + "/" + type_name; - auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name); + return make_service_description(topic_name, type_name); +} - return iox::capro::ServiceDescription( - iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)), - iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)), - iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple))); +iox::capro::ServiceDescription +get_iceoryx_service_description( + const std::string & topic_name, + const rosidl_service_type_support_t * type_supports) +{ + std::string package_name; + std::string type_name; + /// @todo Implement type extracts for ROS services + //extract_type(type_supports, package_name, type_name); + type_name = package_name + "/" + type_name; + + return make_service_description(topic_name, type_name); } } // namespace rmw_iceoryx_cpp diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 2796418..824a93e 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -20,6 +20,10 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" + +#include "iceoryx_posh/popo/untyped_client.hpp" + extern "C" { rmw_client_t * @@ -34,8 +38,55 @@ rmw_create_client( RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, NULL); RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, NULL); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support clients."); - return NULL; + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_create_client + : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); + + // create the iceoryx service description for a sender + auto service_description = + rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports); + + std::string node_full_name = std::string(node->namespace_) + std::string(node->name); + rmw_client_t * rmw_client = nullptr; + iox::popo::UntypedClient * iceoryx_client = nullptr; + + rmw_client = rmw_client_allocate(); + if (!rmw_client) { + RMW_SET_ERROR_MSG("failed to allocate memory for client"); + return nullptr; + } + + auto cleanupAfterError = [](){}; + + iceoryx_client = + static_cast(rmw_allocate( + sizeof(iox::popo::UntypedClient))); + if (!iceoryx_client) { + RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx client"); + cleanupAfterError(); + return nullptr; + } + + RMW_TRY_PLACEMENT_NEW( + iceoryx_client, iceoryx_client, + cleanupAfterError(), iox::popo::UntypedClient, service_description, + iox::popo::ClientOptions{ + 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + + rmw_client->implementation_identifier = rmw_get_implementation_identifier(); + rmw_client->data = iceoryx_client; + + rmw_client->service_name = + static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); + if (!rmw_client->service_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for service name"); + cleanupAfterError(); + return nullptr; + } else { + memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + } + + return rmw_client; } rmw_ret_t @@ -46,8 +97,30 @@ rmw_destroy_client( RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support clients."); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_destroy_client + : client, client->implementation_identifier, + rmw_get_implementation_identifier(), return RMW_RET_ERROR); + + rmw_ret_t result = RMW_RET_OK; + + iox::popo::UntypedClient * iceoryx_client = static_cast(client->data); + if (iceoryx_client) { + RMW_TRY_DESTRUCTOR( + iceoryx_client->~UntypedClientImpl(), + iceoryx_client, + result = RMW_RET_ERROR) + rmw_free(iceoryx_client); + } + + client->data = nullptr; + + rmw_free(const_cast(client->service_name)); + client->service_name = nullptr; + + rmw_client_free(client); + + return result; } rmw_ret_t rmw_client_request_publisher_get_actual_qos( diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index d075f1f..ec0ed3a 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -19,9 +19,12 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" + +#include "iceoryx_posh/popo/untyped_server.hpp" + extern "C" { -/// @todo Use the new request/response API of iceoryx v2.0 here instead of dummy services rmw_service_t * rmw_create_service( const rmw_node_t * node, @@ -34,7 +37,17 @@ rmw_create_service( RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_create_service + : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); + + // create the iceoryx service description for a sender + auto service_description = + rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports); + + std::string node_full_name = std::string(node->namespace_) + std::string(node->name); rmw_service_t * rmw_service = nullptr; + iox::popo::UntypedServer * iceoryx_server = nullptr; rmw_service = rmw_service_allocate(); if (!rmw_service) { @@ -42,15 +55,32 @@ rmw_create_service( return nullptr; } - void * info = nullptr; + auto cleanupAfterError = [](){}; + + iceoryx_server = + static_cast(rmw_allocate( + sizeof(iox::popo::UntypedServer))); + if (!iceoryx_server) { + RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx server"); + cleanupAfterError(); + return nullptr; + } + + RMW_TRY_PLACEMENT_NEW( + iceoryx_server, iceoryx_server, + cleanupAfterError(), iox::popo::UntypedServer, service_description, + iox::popo::ServerOptions{ + 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); rmw_service->implementation_identifier = rmw_get_implementation_identifier(); - rmw_service->data = info; + rmw_service->data = iceoryx_server; rmw_service->service_name = static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); if (!rmw_service->service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + RMW_SET_ERROR_MSG("failed to allocate memory for service name"); + cleanupAfterError(); + return nullptr; } else { memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); } @@ -71,6 +101,17 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) rmw_ret_t result = RMW_RET_OK; + iox::popo::UntypedServer * iceoryx_server = static_cast(service->data); + if (iceoryx_server) { + RMW_TRY_DESTRUCTOR( + iceoryx_server->~UntypedServerImpl(), + iceoryx_server, + result = RMW_RET_ERROR) + rmw_free(iceoryx_server); + } + + service->data = nullptr; + rmw_free(const_cast(service->service_name)); service->service_name = nullptr; From 57b5b7fe2e46e16eec7df43cbbfa68e936b001d1 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Tue, 5 Apr 2022 16:53:08 +0200 Subject: [PATCH 02/27] Implement wrapper classes for client and server and add type introspection methods accordingly (#76) Signed-off-by: Simon Hoinkis --- .../iceoryx_type_info_introspection.hpp | 10 ++ .../iceoryx_type_info_introspection.cpp | 73 ++++++++++++++ rmw_iceoryx_cpp/src/rmw_client.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_response.cpp | 97 ++++++++++++++++--- rmw_iceoryx_cpp/src/rmw_service.cpp | 2 +- rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 42 ++++++++ rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 42 ++++++++ 7 files changed, 252 insertions(+), 16 deletions(-) create mode 100644 rmw_iceoryx_cpp/src/types/iceoryx_client.hpp create mode 100644 rmw_iceoryx_cpp/src/types/iceoryx_server.hpp diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index f9b2fae..e77d457 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -20,6 +20,7 @@ #include struct rosidl_message_type_support_t; +struct rosidl_service_type_support_t; namespace rmw_iceoryx_cpp { @@ -35,12 +36,21 @@ enum class TypeSupportLanguage const std::pair get_type_support( const rosidl_message_type_support_t * type_supports); +/// @brief Wraps get_service_typesupport_handle() and does error handling +/// @return std::pair containing enum TypeSupportLanguage and handle to the type support +const std::pair get_type_support( + const rosidl_service_type_support_t * type_supports); + bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports); +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); 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); + std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_supports); std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index 0a9e623..78d53ab 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -20,6 +20,7 @@ #include #include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_cpp/service_type_support.hpp" #include "rosidl_typesupport_introspection_c/field_types.h" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -167,6 +168,44 @@ const std::pair get_type_support( + const rosidl_service_type_support_t * type_supports) +{ + rcutils_error_string_t cpp_error_string; + rcutils_error_string_t c_error_string; + + // first, try to extract cpp type support + auto ts_cpp = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (ts_cpp != nullptr) { + return std::make_pair(rmw_iceoryx_cpp::TypeSupportLanguage::CPP, ts_cpp); + } else { + /// @todo Reset error string since this is not yet an error + /// https://github.com/ros2/rosidl_typesupport/pull/102 + cpp_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + } + + // second, try to extract c type support + auto ts_c = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (ts_c != nullptr) { + return std::make_pair(rmw_iceoryx_cpp::TypeSupportLanguage::C, ts_c); + } else { + /// @todo https://github.com/ros2/rosidl_typesupport/pull/102 + c_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + } + + // still here? Then something's wrong + std::stringstream error_string; + error_string << "No suitable type support given! "; + error_string << "cpp error: " << cpp_error_string.str; + error_string << "c error: " << c_error_string.str; + throw std::runtime_error(error_string.str()); +} + bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports) { auto ts = get_type_support(type_supports); @@ -184,6 +223,23 @@ bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports) return false; } +bool iceoryx_is_fixed_size(const rosidl_service_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + return details_cpp::is_fixed_size(members); + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return details_c::is_fixed_size(members); + } + // Something went wrong + return false; +} + size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_supports) { auto ts = get_type_support(type_supports); @@ -201,6 +257,23 @@ size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_suppo return 0; } +size_t iceoryx_get_message_size(const rosidl_service_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + return members->size_of_; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return members->size_of_; + } + // Something went wrong + return 0; +} + std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_supports) { auto ts = get_type_support(type_supports); diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 824a93e..1df8025 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -22,7 +22,7 @@ #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" -#include "iceoryx_posh/popo/untyped_client.hpp" +#include "types/iceoryx_client.hpp" extern "C" { diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 3eb2ab2..7eedb17 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -17,38 +17,107 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "iceoryx_posh/popo/untyped_client.hpp" + extern "C" { rmw_ret_t - -/// @todo Use the new request/response API of iceoryx v2.0 here rmw_take_response( const rmw_client_t * client, rmw_service_info_t * request_header, void * ros_response, bool * taken) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_ERROR); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_ERROR); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_take_response + : client, + client->implementation_identifier, + rmw_get_implementation_identifier(), + return RMW_RET_ERROR); + + auto iceoryx_client = static_cast(client->data); + if (!iceoryx_client) { + RMW_SET_ERROR_MSG("client data is null"); + 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() + .and_then( + [&](const void * responsePayload) { + auto responseHeader = iox::popo::ResponseHeader::fromPayload(responsePayload); + /// @todo check writer guid + if (responseHeader->getSequenceId() == request_header->request_id.sequence_number) + { + user_payload = responseHeader; + chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + ret = RMW_RET_OK; + } + else + { + std::cout << "Got Response with outdated sequence number!" << std::endl; + ret = RMW_RET_ERROR; + + } + }) + .or_else( + [&](iox::popo::ChunkReceiveResult) { + RMW_SET_ERROR_MSG("No chunk in iceoryx_client"); + ret = RMW_RET_ERROR; + }); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support responses."); - return RMW_RET_UNSUPPORTED; + if (ret == RMW_RET_ERROR) { + return ret; + } + + /// @todo a wrapper class of client is needed to save the fixed size info + // if fixed size, we fetch the data via memcpy + //if (iceoryx_client->is_fixed_size_) { + memcpy(ros_response, user_payload, chunk_header->userPayloadSize()); + iceoryx_client->releaseResponse(user_payload); + *taken = true; + ret = RMW_RET_OK; + //} else { + // rmw_iceoryx_cpp::deserialize( + // static_cast(user_payload), + // &iceoryx_client->type_supports_, + // ros_message); + iceoryx_client->releaseResponse(user_payload); + *taken = true; + ret = RMW_RET_OK; + //} + + *taken = false; + return ret; } -/// @todo Use the new request/response API of iceoryx v2.0 here rmw_ret_t rmw_send_response( const rmw_service_t * service, rmw_request_id_t * request_header, void * ros_response) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_ERROR); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_ERROR); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_send_response + : service, service->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 responses."); - return RMW_RET_UNSUPPORTED; + return ret; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index ec0ed3a..5bae8ce 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -21,7 +21,7 @@ #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" -#include "iceoryx_posh/popo/untyped_server.hpp" +#include "types/iceoryx_server.hpp" extern "C" { diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp new file mode 100644 index 0000000..13c9689 --- /dev/null +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2022 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES__ICEORYX_CLIENT_HPP_ +#define TYPES__ICEORYX_CLIENT_HPP_ + +#include "iceoryx_posh/popo/untyped_client.hpp" + +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" + +struct IceoryxServer +{ + IceoryxServer( + const rosidl_service_type_support_t * type_supports, + iox::popo::UntypedClient * const iceoryx_client) + : type_supports_(*type_supports), + iceoryx_client_(iceoryx_client), + is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), + message_size_(rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)) + {} + + rosidl_service_type_support_t type_supports_; + iox::popo::UntypedClient * const iceoryx_client_; + bool is_fixed_size_; + size_t message_size_; +}; + +#endif // TYPES__ICEORYX_CLIENT_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp new file mode 100644 index 0000000..8fcf73a --- /dev/null +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2022 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES__ICEORYX_SERVER_HPP_ +#define TYPES__ICEORYX_SERVER_HPP_ + +#include "iceoryx_posh/popo/untyped_server.hpp" + +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" + +struct IceoryxServer +{ + IceoryxServer( + const rosidl_service_type_support_t * type_supports, + iox::popo::UntypedServer * const iceoryx_server) + : type_supports_(*type_supports), + iceoryx_server_(iceoryx_server), + is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), + message_size_(rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)) + {} + + rosidl_service_type_support_t type_supports_; + iox::popo::UntypedServer * const iceoryx_server_; + bool is_fixed_size_; + size_t message_size_; +}; + +#endif // TYPES__ICEORYX_SERVER_HPP_ From d73422a849924556e406f5430fcdb95492c30f92 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Mon, 2 Jan 2023 15:02:10 +0100 Subject: [PATCH 03/27] Update Readme (#76) Signed-off-by: Simon Hoinkis --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d8abaca..24eb631 100644 --- a/README.md +++ b/README.md @@ -154,8 +154,8 @@ Unfortunately, not all features are yet fully fleshed out. | `ros2 node list` | :heavy_check_mark: | | `ros2 node info` | :heavy_check_mark: | | `ros2 interface *` | :heavy_check_mark: | -| `ros2 service *` | :x: (coming with iceoryx v2.0) | -| `ros2 param list` | :x: (coming with iceoryx v2.0) | +| `ros2 service *` | :heavy_check_mark | +| `ros2 param list` | :heavy_check_mark | | `rqt_graph` | :heavy_check_mark: | | `rqt_top` | :heavy_check_mark: | | `rqt_console` | :heavy_check_mark: | From 287a211ea089ee5af2a12c5e8033c06e4a9801ac Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Tue, 3 Jan 2023 22:58:37 +0100 Subject: [PATCH 04/27] Implement 'rmw_send_request', 'rmw_take_request' and 'rmw_take_response' (#76) Signed-off-by: Simon Hoinkis --- .../rmw_iceoryx_cpp/iceoryx_deserialize.hpp | 7 + .../iceoryx_type_info_introspection.hpp | 2 + .../src/internal/iceoryx_deserialize.cpp | 19 +++ .../iceoryx_deserialize_typesupport_c.hpp | 16 +++ .../iceoryx_deserialize_typesupport_cpp.hpp | 18 ++- .../iceoryx_type_info_introspection.cpp | 11 ++ rmw_iceoryx_cpp/src/rmw_publish.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_request.cpp | 121 +++++++++++++++++- rmw_iceoryx_cpp/src/rmw_response.cpp | 57 +++++++-- rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 8 +- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 4 +- 11 files changed, 239 insertions(+), 26 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp index ae82201..82829f2 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp +++ b/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_ diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index e77d457..c1a7944 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/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); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp index 934d99f..e9806cb 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp +++ b/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(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(ts.second->data); + rmw_iceoryx_cpp::details_c::deserializeResponse(serialized_msg, members_c, ros_message); + } +} + } // namespace rmw_iceoryx_cpp diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp index e9e364b..208fc84 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp +++ b/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_ diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp index a0a6cce..6d80a2c 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp @@ -24,7 +24,7 @@ #include #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_ diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index 78d53ab..613ca0c 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/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) diff --git a/rmw_iceoryx_cpp/src/rmw_publish.cpp b/rmw_iceoryx_cpp/src/rmw_publish.cpp index 869b935..a7d8200 100644 --- a/rmw_iceoryx_cpp/src/rmw_publish.cpp +++ b/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. diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 0dbbb76..0f2cff9 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/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(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(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(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 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" diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 7eedb17..33b9b79 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2022 - 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. @@ -17,7 +18,10 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" -#include "iceoryx_posh/popo/untyped_client.hpp" +#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" + +#include "./types/iceoryx_client.hpp" +#include "./types/iceoryx_server.hpp" extern "C" { @@ -40,12 +44,18 @@ rmw_take_response( rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto iceoryx_client = static_cast(client->data); - if (!iceoryx_client) { + auto iceoryx_client_abstraction = static_cast(client->data); + if (!iceoryx_client_abstraction) { RMW_SET_ERROR_MSG("client data is null"); return RMW_RET_ERROR; } + auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; + if (!iceoryx_client) { + RMW_SET_ERROR_MSG("iceoryx_client is null"); + return RMW_RET_ERROR; + } + const iox::mepoo::ChunkHeader * chunk_header = nullptr; const void * user_payload = nullptr; rmw_ret_t ret = RMW_RET_ERROR; @@ -65,7 +75,6 @@ rmw_take_response( { std::cout << "Got Response with outdated sequence number!" << std::endl; ret = RMW_RET_ERROR; - } }) .or_else( @@ -78,22 +87,21 @@ rmw_take_response( return ret; } - /// @todo a wrapper class of client is needed to save the fixed size info // if fixed size, we fetch the data via memcpy - //if (iceoryx_client->is_fixed_size_) { + if (iceoryx_client_abstraction->is_fixed_size_) { memcpy(ros_response, user_payload, chunk_header->userPayloadSize()); iceoryx_client->releaseResponse(user_payload); *taken = true; ret = RMW_RET_OK; - //} else { - // rmw_iceoryx_cpp::deserialize( - // static_cast(user_payload), - // &iceoryx_client->type_supports_, - // ros_message); + } else { + rmw_iceoryx_cpp::deserialize( + static_cast(user_payload), + &iceoryx_client_abstraction->type_supports_, + ros_response); iceoryx_client->releaseResponse(user_payload); *taken = true; ret = RMW_RET_OK; - //} + } *taken = false; return ret; @@ -109,14 +117,37 @@ rmw_send_response( RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( rmw_send_response : service, service->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); + auto iceoryx_service_abstraction = static_cast(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; + } + rmw_ret_t ret = RMW_RET_ERROR; + auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_); + iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->message_size_, iceoryx_service_abstraction->message_alignment_) + .and_then([&](auto& responsePayload) { + /// @todo memcpy or serialize the response + void * response; + // auto response = static_cast(responsePayload); + // response->sum = request->augend + request->addend; + iceoryx_server->send(response).or_else( + [&](auto& error) { ret = RMW_RET_ERROR; }); + }) + .or_else( + [&](auto& error) { ret = RMW_RET_ERROR; }); return ret; } diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp index 13c9689..bd53217 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 by Apex.AI Inc. All rights reserved. +// Copyright (c) 2022 - 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. @@ -22,9 +22,9 @@ #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" -struct IceoryxServer +struct IceoryxClient { - IceoryxServer( + IceoryxClient( const rosidl_service_type_support_t * type_supports, iox::popo::UntypedClient * const iceoryx_client) : type_supports_(*type_supports), @@ -37,6 +37,8 @@ struct IceoryxServer iox::popo::UntypedClient * const iceoryx_client_; bool is_fixed_size_; size_t message_size_; + uint32_t message_alignment_; + /// @todo add sample here to take the the response later? nope im server! }; #endif // TYPES__ICEORYX_CLIENT_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index 8fcf73a..1c11852 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 by Apex.AI Inc. All rights reserved. +// Copyright (c) 2022 - 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. @@ -37,6 +37,8 @@ struct IceoryxServer iox::popo::UntypedServer * const iceoryx_server_; bool is_fixed_size_; size_t message_size_; + uint32_t message_alignment_; + void * request_payload_; }; #endif // TYPES__ICEORYX_SERVER_HPP_ From 64870a191c6623039c7dcb169c1305e7ac0fefe3 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 10:24:16 +0100 Subject: [PATCH 05/27] Add fixed-size path for 'rmw_send_response' and 'rmw_take_response' (#76) Signed-off-by: Simon Hoinkis --- .../iceoryx_type_info_introspection.cpp | 22 ++++ rmw_iceoryx_cpp/src/rmw_request.cpp | 101 ++++++++---------- rmw_iceoryx_cpp/src/rmw_response.cpp | 27 +++-- rmw_iceoryx_cpp/src/rmw_service.cpp | 2 + rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 2 +- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 3 +- 6 files changed, 92 insertions(+), 65 deletions(-) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index 613ca0c..bcdc274 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -25,10 +25,13 @@ #include "rosidl_typesupport_introspection_c/field_types.h" #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/field_types.hpp" #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + #include "rcutils/error_handling.h" @@ -349,6 +352,25 @@ void iceoryx_init_message( } } +void iceoryx_init_message( + const rosidl_service_type_support_t * type_supports, + void * message) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + members->request_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); + return; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + members->request_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); + return; + } +} + void iceoryx_fini_message( const rosidl_message_type_support_t * type_supports, void * message) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 0f2cff9..535f655 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -20,47 +20,15 @@ #include "./types/iceoryx_client.hpp" #include "./types/iceoryx_server.hpp" +#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" + extern "C" { -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, const void * ros_request, - int64_t * sequence_id) + int64_t * sequence_id) // out going param { RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR); @@ -90,19 +58,21 @@ rmw_send_request( } iceoryx_client->loan(iceoryx_client_abstraction->message_size_, iceoryx_client_abstraction->message_alignment_) - .and_then([&](auto& requestPayload) { + .and_then([&](void * requestPayload) { auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); - requestHeader->setSequenceId(*sequence_id); + requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_); + iceoryx_client_abstraction->sequence_id_ += 1; /// @todo memcpy or serialize the response - void * request; - // auto request = static_cast(requestPayload); - // request->augend = fibonacciLast; - // request->addend = fibonacciCurrent; - iceoryx_client->send(request).or_else( + // 1) init message + // 2) write ros_response to shared memory + memcpy(requestPayload, ros_request, iceoryx_client_abstraction->message_size_); + iceoryx_client->send(requestPayload).or_else( [&](auto& error) { ret = RMW_RET_ERROR; }); }) .or_else([&](auto& error) { ret = RMW_RET_ERROR; }); + *sequence_id = iceoryx_client_abstraction->sequence_id_; + return ret; } @@ -135,26 +105,49 @@ rmw_take_request( 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 payload_vector{}; + const iox::mepoo::ChunkHeader * chunk_header = nullptr; + const iox::popo::RequestHeader * iceoryx_request_header = nullptr; + const void * user_payload = nullptr; - // rmw_iceoryx_cpp::serialize(ros_message, &iceoryx_publisher->type_supports_, payload_vector); + rmw_ret_t ret = RMW_RET_ERROR; - // // send composed payload - // return details::send_payload(iceoryx_sender, payload_vector.data(), payload_vector.size()); + iceoryx_server->take() + .and_then( + [&](const void * requestPayload) { + user_payload = requestPayload; + chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + iceoryx_request_header = iox::popo::RequestHeader::fromPayload(user_payload); + ret = RMW_RET_OK; + }) + .or_else( + [&](auto&) { + RMW_SET_ERROR_MSG("rmw_take_request error!"); + ret = RMW_RET_ERROR; + }); + return ret; - *taken = true; - return RMW_RET_OK; + // if fixed size, we fetch the data via memcpy + if (iceoryx_service_abstraction->is_fixed_size_) { + memcpy(ros_request, user_payload, chunk_header->userPayloadSize()); + iceoryx_server->releaseRequest(user_payload); + *taken = true; + ret = RMW_RET_OK; + } else { + rmw_iceoryx_cpp::deserialize( + static_cast(user_payload), + &iceoryx_service_abstraction->type_supports_, + ros_request); + iceoryx_server->releaseRequest(user_payload); + *taken = true; + ret = RMW_RET_OK; + } + + return ret; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 33b9b79..6f242a2 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -89,13 +89,15 @@ rmw_take_response( // if fixed size, we fetch the data via memcpy if (iceoryx_client_abstraction->is_fixed_size_) { - memcpy(ros_response, user_payload, chunk_header->userPayloadSize()); + memcpy(request_header, user_payload, sizeof(*request_header)); + /// @todo cast to uint8_t before doing pointer arithmetic? + memcpy(ros_response, user_payload + sizeof(*request_header), chunk_header->userPayloadSize()); iceoryx_client->releaseResponse(user_payload); *taken = true; ret = RMW_RET_OK; } else { rmw_iceoryx_cpp::deserialize( - static_cast(user_payload), + static_cast(user_payload), /// @todo add fourth param for 'request_header' &iceoryx_client_abstraction->type_supports_, ros_response); iceoryx_client->releaseResponse(user_payload); @@ -138,16 +140,23 @@ rmw_send_response( auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_); iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->message_size_, iceoryx_service_abstraction->message_alignment_) - .and_then([&](auto& responsePayload) { + .and_then([&](void * responsePayload) { /// @todo memcpy or serialize the response - void * response; - // auto response = static_cast(responsePayload); - // response->sum = request->augend + request->addend; - iceoryx_server->send(response).or_else( - [&](auto& error) { ret = RMW_RET_ERROR; }); + // 1) init message like pub/sub? + // 2) write | request_header | ros_response | to shared memory + memcpy(responsePayload, request_header, sizeof(*request_header)); + memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_service_abstraction->message_size_); + iceoryx_server->send(responsePayload).or_else( + [&](auto&) { + RMW_SET_ERROR_MSG("rmw_send_response send error!"); + ret = RMW_RET_ERROR; + }); }) .or_else( - [&](auto& error) { ret = RMW_RET_ERROR; }); + [&](auto&) { + RMW_SET_ERROR_MSG("rmw_send_response loan error!"); + ret = RMW_RET_ERROR; + }); return ret; } diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index 5bae8ce..df723be 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -85,6 +85,8 @@ rmw_create_service( memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); } + /// @todo allocate IceoryxServer here and fill size_t message_size_ and message_alignment_; + return rmw_service; } diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp index bd53217..f7438dd 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -38,7 +38,7 @@ struct IceoryxClient bool is_fixed_size_; size_t message_size_; uint32_t message_alignment_; - /// @todo add sample here to take the the response later? nope im server! + uint64_t sequence_id_; }; #endif // TYPES__ICEORYX_CLIENT_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index 1c11852..e753050 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -37,7 +37,8 @@ struct IceoryxServer iox::popo::UntypedServer * const iceoryx_server_; bool is_fixed_size_; size_t message_size_; - uint32_t message_alignment_; + /// @todo Is there a way to get the aligment for a complete type via the rosidl? + uint32_t message_alignment_{8}; void * request_payload_; }; From 146ada8ba8abe60ef07b182baac7a780c8be8989 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 11:11:30 +0100 Subject: [PATCH 06/27] Create server and client abstraction (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_client.cpp | 50 +++++++++++++++++++++++--- rmw_iceoryx_cpp/src/rmw_service.cpp | 55 +++++++++++++++++++++++++---- 2 files changed, 94 insertions(+), 11 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 1df8025..6c93e27 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -49,15 +49,37 @@ rmw_create_client( std::string node_full_name = std::string(node->namespace_) + std::string(node->name); rmw_client_t * rmw_client = nullptr; iox::popo::UntypedClient * iceoryx_client = nullptr; + IceoryxClient * iceoryx_client_abstraction = nullptr; + + bool returnOnError = false; + + auto cleanupAfterError = [&](){ + if (rmw_client) { + if (iceoryx_client) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_client->~UntypedClient(), iox::popo::UntypedClient) + rmw_free(iceoryx_client); + } + if (iceoryx_client_abstraction) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_client_abstraction->~IceoryxClient(), IceoryxClient) + rmw_free(iceoryx_client_abstraction); + } + if (rmw_client->service_name) { + rmw_free(const_cast(rmw_client->service_name)); + } + rmw_client_free(rmw_client); + returnOnError = true; + } + }; rmw_client = rmw_client_allocate(); if (!rmw_client) { RMW_SET_ERROR_MSG("failed to allocate memory for client"); + cleanupAfterError(); return nullptr; } - auto cleanupAfterError = [](){}; - iceoryx_client = static_cast(rmw_allocate( sizeof(iox::popo::UntypedClient))); @@ -72,6 +94,27 @@ rmw_create_client( cleanupAfterError(), iox::popo::UntypedClient, service_description, iox::popo::ClientOptions{ 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + if(returnOnError) + { + return nullptr; + } + + iceoryx_client->connect(); + + iceoryx_client_abstraction = + static_cast(rmw_allocate(sizeof(IceoryxClient))); + if (!iceoryx_client_abstraction) { + RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher"); + cleanupAfterError(); + return nullptr; + } + RMW_TRY_PLACEMENT_NEW( + iceoryx_client_abstraction, iceoryx_client_abstraction, + cleanupAfterError(), IceoryxClient, type_supports, iceoryx_client); + if(returnOnError) + { + return nullptr; + } rmw_client->implementation_identifier = rmw_get_implementation_identifier(); rmw_client->data = iceoryx_client; @@ -82,9 +125,8 @@ rmw_create_client( RMW_SET_ERROR_MSG("failed to allocate memory for service name"); cleanupAfterError(); return nullptr; - } else { - memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); } + memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); return rmw_client; } diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index df723be..5b79a96 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -48,18 +48,41 @@ rmw_create_service( std::string node_full_name = std::string(node->namespace_) + std::string(node->name); rmw_service_t * rmw_service = nullptr; iox::popo::UntypedServer * iceoryx_server = nullptr; + IceoryxServer * iceoryx_server_abstraction = nullptr; + + bool returnOnError = false; + + auto cleanupAfterError = [&](){ + if (rmw_service) { + if (iceoryx_server) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_server->~UntypedServer(), iox::popo::UntypedServer) + rmw_free(iceoryx_server); + } + if (iceoryx_server_abstraction) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_server_abstraction->~IceoryxServer(), IceoryxServer) + rmw_free(iceoryx_server_abstraction); + } + if (rmw_service->service_name) { + rmw_free(const_cast(rmw_service->service_name)); + } + rmw_service_free(rmw_service); + returnOnError = true; + } + }; rmw_service = rmw_service_allocate(); if (!rmw_service) { RMW_SET_ERROR_MSG("failed to allocate memory for service"); + cleanupAfterError(); return nullptr; } - auto cleanupAfterError = [](){}; - iceoryx_server = static_cast(rmw_allocate( sizeof(iox::popo::UntypedServer))); + if (!iceoryx_server) { RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx server"); cleanupAfterError(); @@ -71,9 +94,30 @@ rmw_create_service( cleanupAfterError(), iox::popo::UntypedServer, service_description, iox::popo::ServerOptions{ 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + if(returnOnError) + { + return nullptr; + } + + iceoryx_server->offer(); + + iceoryx_server_abstraction = + static_cast(rmw_allocate(sizeof(IceoryxServer))); + if (!iceoryx_server_abstraction) { + RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher"); + cleanupAfterError(); + return nullptr; + } + RMW_TRY_PLACEMENT_NEW( + iceoryx_server_abstraction, iceoryx_server_abstraction, + cleanupAfterError(), IceoryxServer, type_supports, iceoryx_server); + if(returnOnError) + { + return nullptr; + } rmw_service->implementation_identifier = rmw_get_implementation_identifier(); - rmw_service->data = iceoryx_server; + rmw_service->data = iceoryx_server_abstraction; rmw_service->service_name = static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); @@ -81,11 +125,8 @@ rmw_create_service( RMW_SET_ERROR_MSG("failed to allocate memory for service name"); cleanupAfterError(); return nullptr; - } else { - memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); } - - /// @todo allocate IceoryxServer here and fill size_t message_size_ and message_alignment_; + memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); return rmw_service; } From 7db68cbaab327775780bb9d09e38a98216284d6a Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 11:46:35 +0100 Subject: [PATCH 07/27] Fix 'is_fixed_size' for ROS 2 services (#76) Signed-off-by: Simon Hoinkis --- .../src/internal/iceoryx_type_info_introspection.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index bcdc274..f034ca8 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -232,12 +232,12 @@ bool iceoryx_is_fixed_size(const rosidl_service_type_support_t * type_supports) if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts.second->data); - return details_cpp::is_fixed_size(members); + static_cast(ts.second->data); + return details_cpp::is_fixed_size(members->request_members_) && details_cpp::is_fixed_size(members->response_members_); } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts.second->data); - return details_c::is_fixed_size(members); + static_cast(ts.second->data); + return details_c::is_fixed_size(members->request_members_) && details_c::is_fixed_size(members->response_members_); } // Something went wrong return false; From 5f8499333619265533fe1ae2f1c527d5571ea31e Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 17:32:09 +0100 Subject: [PATCH 08/27] Properly destroy services and clients and add serialization for services (#76) Signed-off-by: Simon Hoinkis --- .../rmw_iceoryx_cpp/iceoryx_serialize.hpp | 11 ++++ .../iceoryx_type_info_introspection.hpp | 17 +++-- .../src/internal/iceoryx_name_conversion.cpp | 12 +++- .../src/internal/iceoryx_serialize.cpp | 38 +++++++++++ .../iceoryx_type_info_introspection.cpp | 63 +++++++++++++++++-- rmw_iceoryx_cpp/src/rmw_client.cpp | 23 ++++--- rmw_iceoryx_cpp/src/rmw_request.cpp | 43 ++++++++++--- rmw_iceoryx_cpp/src/rmw_response.cpp | 4 +- rmw_iceoryx_cpp/src/rmw_service.cpp | 19 ++++-- .../src/rmw_service_server_is_available.cpp | 41 +++++++++++- rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 7 ++- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 6 +- 12 files changed, 239 insertions(+), 45 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp index 8f37463..a8a0644 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp @@ -18,6 +18,7 @@ #include struct rosidl_message_type_support_t; +struct rosidl_service_type_support_t; namespace rmw_iceoryx_cpp { @@ -27,5 +28,15 @@ void serialize( const rosidl_message_type_support_t * type_supports, std::vector & payload_vector); +void serializeRequest( + const void * ros_message, + const rosidl_service_type_support_t * type_supports, + std::vector & payload_vector); + +void serializeResponse( + const void * ros_message, + const rosidl_service_type_support_t * type_supports, + std::vector & payload_vector); + } // namespace rmw_iceoryx_cpp #endif // RMW_ICEORYX_CPP__ICEORYX_SERIALIZE_HPP_ diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index c1a7944..4bf9ebc 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -42,28 +42,37 @@ const std::pair get_ const rosidl_service_type_support_t * type_supports); bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports); - 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); +size_t iceoryx_get_request_size(const rosidl_service_type_support_t * type_supports); +size_t iceoryx_get_response_size(const rosidl_service_type_support_t * type_supports); std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_supports); +std::string iceoryx_get_service_name(const rosidl_service_type_support_t * type_supports); std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports); +std::string iceoryx_get_service_namespace(const rosidl_service_type_support_t * type_supports); void iceoryx_init_message( const rosidl_message_type_support_t * type_supports, void * message); +void iceoryx_init_message( // init_response, init_request + const rosidl_service_type_support_t * type_supports, + void * message); + void iceoryx_fini_message( const rosidl_message_type_support_t * type_supports, void * message); +/// @todo +// void iceoryx_fini_message( +// const rosidl_service_type_support_t * type_supports, +// void * message); + } // namespace rmw_iceoryx_cpp #endif // RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp index 69afa3a..86b165d 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp @@ -63,6 +63,15 @@ inline void extract_type( type_name = rmw_iceoryx_cpp::iceoryx_get_message_name(type_support); } +inline void extract_type( + const rosidl_service_type_support_t * type_support, + std::string & package_name, + std::string & type_name) +{ + package_name = to_message_type(rmw_iceoryx_cpp::iceoryx_get_service_namespace(type_support)); + type_name = rmw_iceoryx_cpp::iceoryx_get_service_name(type_support); +} + namespace rmw_iceoryx_cpp { @@ -190,8 +199,7 @@ get_iceoryx_service_description( { std::string package_name; std::string type_name; - /// @todo Implement type extracts for ROS services - //extract_type(type_supports, package_name, type_name); + extract_type(type_supports, package_name, type_name); type_name = package_name + "/" + type_name; return make_service_description(topic_name, type_name); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp index c2fbc68..28388c0 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp @@ -19,9 +19,11 @@ #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" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" #include "rcutils/error_handling.h" @@ -52,4 +54,40 @@ void serialize( } } +void serializeRequest( + const void * ros_message, + const rosidl_service_type_support_t * type_supports, + std::vector & payload_vector) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members->request_members_, payload_vector); + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_c::serialize(ros_message, members->request_members_, payload_vector); + } +} + +void serializeResponse( + const void * ros_message, + const rosidl_service_type_support_t * type_supports, + std::vector & payload_vector) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members->response_members_, payload_vector); + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_c::serialize(ros_message, members->response_members_, payload_vector); + } +} + } // namespace rmw_iceoryx_cpp diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index f034ca8..0a2159c 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -260,18 +260,35 @@ size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_suppo return 0; } -size_t iceoryx_get_message_size(const rosidl_service_type_support_t * type_supports) +size_t iceoryx_get_request_size(const rosidl_service_type_support_t * type_supports) { auto ts = get_type_support(type_supports); if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts.second->data); - return members->size_of_; + static_cast(ts.second->data); + return members->request_members_->size_of_; } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts.second->data); - return members->size_of_; + static_cast(ts.second->data); + return members->request_members_->size_of_; + } + // Something went wrong + return 0; +} + +size_t iceoryx_get_response_size(const rosidl_service_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + return members->response_members_->size_of_; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return members->response_members_->size_of_; } // Something went wrong return 0; @@ -294,6 +311,23 @@ std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_ return ""; } +std::string iceoryx_get_service_name(const rosidl_service_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + return members->service_name_; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return members->service_name_; + } + // Something went wrong + return ""; +} + std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports) { auto ts = get_type_support(type_supports); @@ -311,6 +345,23 @@ std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * return ""; } +std::string iceoryx_get_service_namespace(const rosidl_service_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members = + static_cast(ts.second->data); + return members->service_namespace_; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return members->service_namespace_; + } + // Something went wrong + return ""; +} + bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports) { try { @@ -362,11 +413,13 @@ void iceoryx_init_message( auto members = static_cast(ts.second->data); members->request_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); + members->response_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); return; } else if (ts.first == TypeSupportLanguage::C) { auto members = static_cast(ts.second->data); members->request_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); + members->response_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); return; } } diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 6c93e27..f094da1 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -93,7 +93,7 @@ rmw_create_client( iceoryx_client, iceoryx_client, cleanupAfterError(), iox::popo::UntypedClient, service_description, iox::popo::ClientOptions{ - 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); if(returnOnError) { return nullptr; @@ -127,7 +127,7 @@ rmw_create_client( return nullptr; } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); - + std::cout << "RMW Client created!" << std::endl; return rmw_client; } @@ -146,13 +146,20 @@ rmw_destroy_client( rmw_ret_t result = RMW_RET_OK; - iox::popo::UntypedClient * iceoryx_client = static_cast(client->data); - if (iceoryx_client) { + IceoryxClient * iceoryx_client_abstraction = static_cast(client->data); + if (iceoryx_client_abstraction) { + if (iceoryx_client_abstraction->iceoryx_client_) { + RMW_TRY_DESTRUCTOR( + iceoryx_client_abstraction->iceoryx_client_->~UntypedClient(), + iceoryx_client_abstraction->iceoryx_client_, + result = RMW_RET_ERROR) + rmw_free(iceoryx_client_abstraction->iceoryx_client_); + } RMW_TRY_DESTRUCTOR( - iceoryx_client->~UntypedClientImpl(), - iceoryx_client, + iceoryx_client_abstraction->~IceoryxClient(), + iceoryx_client_abstraction, result = RMW_RET_ERROR) - rmw_free(iceoryx_client); + rmw_free(iceoryx_client_abstraction); } client->data = nullptr; @@ -161,7 +168,7 @@ rmw_destroy_client( client->service_name = nullptr; rmw_client_free(client); - + std::cout << "RMW Client destroyed!" << std::endl; return result; } diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 535f655..65ca29f 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -21,6 +21,7 @@ #include "./types/iceoryx_server.hpp" #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" +#include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" extern "C" { @@ -57,22 +58,43 @@ rmw_send_request( return ret; } - iceoryx_client->loan(iceoryx_client_abstraction->message_size_, iceoryx_client_abstraction->message_alignment_) + std::cout << "I'm alive!" << std::endl; + + iceoryx_client->loan(iceoryx_client_abstraction->request_size_, iceoryx_client_abstraction->request_alignment_) .and_then([&](void * requestPayload) { auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); + requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_); + *sequence_id = iceoryx_client_abstraction->sequence_id_; iceoryx_client_abstraction->sequence_id_ += 1; - /// @todo memcpy or serialize the response - // 1) init message - // 2) write ros_response to shared memory - memcpy(requestPayload, ros_request, iceoryx_client_abstraction->message_size_); + + if (iceoryx_client_abstraction->is_fixed_size_) + { + std::cout << "FIXED SIZE!" << std::endl; + memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_); + } + else + { + std::cout << "NOT FIXED SIZE!" << std::endl; + // message is not fixed size, so we have to serialize + std::vector payload_vector{}; + + rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector); + memcpy(requestPayload, payload_vector.data(), payload_vector.size()); + } + iceoryx_client->send(requestPayload).or_else( - [&](auto& error) { ret = RMW_RET_ERROR; }); + [&](auto&) { + RMW_SET_ERROR_MSG("rmw_send_request error!"); + ret = RMW_RET_ERROR; + }); }) - .or_else([&](auto& error) { ret = RMW_RET_ERROR; }); - - *sequence_id = iceoryx_client_abstraction->sequence_id_; + .or_else([&](auto&) { + RMW_SET_ERROR_MSG("rmw_send_request error!"); + ret = RMW_RET_ERROR; + }); + ret = RMW_RET_OK; return ret; } @@ -126,7 +148,8 @@ rmw_take_request( ret = RMW_RET_OK; }) .or_else( - [&](auto&) { + [&](iox::popo::ServerRequestResult& error) { + std::cout << "Could not send Response! Error: " << error << std::endl; RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; }); diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 6f242a2..0eda01f 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -139,13 +139,13 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_); - iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->message_size_, iceoryx_service_abstraction->message_alignment_) + iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->response_size_, iceoryx_service_abstraction->response_alignment_) .and_then([&](void * responsePayload) { /// @todo memcpy or serialize the response // 1) init message like pub/sub? // 2) write | request_header | ros_response | to shared memory memcpy(responsePayload, request_header, sizeof(*request_header)); - memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_service_abstraction->message_size_); + memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_service_abstraction->response_size_); iceoryx_server->send(responsePayload).or_else( [&](auto&) { RMW_SET_ERROR_MSG("rmw_send_response send error!"); diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index 5b79a96..4ef5c14 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -93,7 +93,7 @@ rmw_create_service( iceoryx_server, iceoryx_server, cleanupAfterError(), iox::popo::UntypedServer, service_description, iox::popo::ServerOptions{ - 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); if(returnOnError) { return nullptr; @@ -144,13 +144,20 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) rmw_ret_t result = RMW_RET_OK; - iox::popo::UntypedServer * iceoryx_server = static_cast(service->data); - if (iceoryx_server) { + IceoryxServer * iceoryx_server_abstraction = static_cast(service->data); + if (iceoryx_server_abstraction) { + if (iceoryx_server_abstraction->iceoryx_server_) { + RMW_TRY_DESTRUCTOR( + iceoryx_server_abstraction->iceoryx_server_->~UntypedServer(), + iceoryx_server_abstraction->iceoryx_server_, + result = RMW_RET_ERROR) + rmw_free(iceoryx_server_abstraction->iceoryx_server_); + } RMW_TRY_DESTRUCTOR( - iceoryx_server->~UntypedServerImpl(), - iceoryx_server, + iceoryx_server_abstraction->~IceoryxServer(), + iceoryx_server_abstraction, result = RMW_RET_ERROR) - rmw_free(iceoryx_server); + rmw_free(iceoryx_server_abstraction); } service->data = nullptr; diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index 74663ba..0b61655 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -17,6 +17,10 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "./types/iceoryx_client.hpp" + +#include "iceoryx_posh/runtime/service_discovery.hpp" + extern "C" { rmw_ret_t @@ -29,7 +33,40 @@ rmw_service_server_is_available( RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_ERROR); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support services."); - return RMW_RET_UNSUPPORTED; + rmw_ret_t ret = RMW_RET_ERROR; + + auto iceoryx_client_abstraction = static_cast(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; + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + *is_available = true; + // if(iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) + // { + // *is_available = true; + // } + + /// @todo better to go through service discovery? + // iox::runtime::ServiceDiscovery serviceDiscovery; + // auto& searchItem = iceoryx_client->getServiceDescription(); + // serviceDiscovery.findService( searchItem.getServiceIDString(), + // searchItem.getInstanceIDString(), + // searchItem.getEventIDString(), + // [&](auto&){ *is_available = true; }, + // iox::popo::MessagingPattern::REQ_RES); + + ret = RMW_RET_OK; + return ret; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp index f7438dd..7f6b887 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -30,14 +30,15 @@ struct IceoryxClient : type_supports_(*type_supports), iceoryx_client_(iceoryx_client), is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), - message_size_(rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)) + request_size_(rmw_iceoryx_cpp::iceoryx_get_request_size(type_supports)) {} rosidl_service_type_support_t type_supports_; iox::popo::UntypedClient * const iceoryx_client_; bool is_fixed_size_; - size_t message_size_; - uint32_t message_alignment_; + size_t request_size_; + /// @todo How can the full type be aquired via rosidl to do an 'alignof()'? + uint32_t request_alignment_{8}; uint64_t sequence_id_; }; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index e753050..c68c7b5 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -30,15 +30,15 @@ struct IceoryxServer : type_supports_(*type_supports), iceoryx_server_(iceoryx_server), is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), - message_size_(rmw_iceoryx_cpp::iceoryx_get_message_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 message_size_; + size_t response_size_; /// @todo Is there a way to get the aligment for a complete type via the rosidl? - uint32_t message_alignment_{8}; + uint32_t response_alignment_{8}; void * request_payload_; }; From 60ea893284e57f029d0755f36a76d3a7dd3314e4 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 21:57:05 +0100 Subject: [PATCH 09/27] Fix typo leading to a segfault (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_client.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_request.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index f094da1..ca488e4 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -117,7 +117,7 @@ rmw_create_client( } rmw_client->implementation_identifier = rmw_get_implementation_identifier(); - rmw_client->data = iceoryx_client; + rmw_client->data = iceoryx_client_abstraction; rmw_client->service_name = static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 65ca29f..0e2c806 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -149,7 +149,7 @@ rmw_take_request( }) .or_else( [&](iox::popo::ServerRequestResult& error) { - std::cout << "Could not send Response! Error: " << error << std::endl; + std::cout << "Could not take request! Error: " << error << std::endl; RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; }); From 998edd133fb4687eb038f2517701dda5d88896b8 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 23:18:37 +0100 Subject: [PATCH 10/27] Attach servers and clients to waitset (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_wait.cpp | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp index 68cb369..d270679 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait.cpp @@ -25,6 +25,8 @@ #include "rmw/rmw.h" #include "./types/iceoryx_subscription.hpp" +#include "./types/iceoryx_client.hpp" +#include "./types/iceoryx_server.hpp" extern "C" { @@ -70,6 +72,31 @@ rmw_wait( }); } + // attach all iceoryx servers to WaitSet + for (size_t i = 0; i < services->service_count; ++i) { + auto iceoryx_server_abstraction = + static_cast(subscriptions->subscribers[i]); + auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; + + waitset->attachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST).or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("failed to attach subscriber"); + skip_wait = true; + }); + } + + // attach all iceoryx client to WaitSet + for (size_t i = 0; i < clients->client_count; ++i) { + auto iceoryx_client_abstraction = + static_cast(subscriptions->subscribers[i]); + auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; + + waitset->attachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE).or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("failed to attach subscriber"); + skip_wait = true; + }); + } // attach all guard conditions to WaitSet for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { From 1d4b4697867c233a38b8ecb02e376e1ffd0cbdcf Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 23:19:45 +0100 Subject: [PATCH 11/27] Remove debug prints and add de-serialization to 'rmw_send_response' (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_client.cpp | 2 -- rmw_iceoryx_cpp/src/rmw_request.cpp | 23 +++++++--------- rmw_iceoryx_cpp/src/rmw_response.cpp | 40 +++++++++++++++++----------- 3 files changed, 33 insertions(+), 32 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index ca488e4..6c49c6d 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -127,7 +127,6 @@ rmw_create_client( return nullptr; } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); - std::cout << "RMW Client created!" << std::endl; return rmw_client; } @@ -168,7 +167,6 @@ rmw_destroy_client( client->service_name = nullptr; rmw_client_free(client); - std::cout << "RMW Client destroyed!" << std::endl; return result; } diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 0e2c806..7ec0a9a 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -20,8 +20,8 @@ #include "./types/iceoryx_client.hpp" #include "./types/iceoryx_server.hpp" -#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" +#include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" extern "C" { @@ -29,7 +29,7 @@ rmw_ret_t rmw_send_request( const rmw_client_t * client, const void * ros_request, - int64_t * sequence_id) // out going param + int64_t * sequence_id) { RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR); @@ -58,8 +58,6 @@ rmw_send_request( return ret; } - std::cout << "I'm alive!" << std::endl; - iceoryx_client->loan(iceoryx_client_abstraction->request_size_, iceoryx_client_abstraction->request_alignment_) .and_then([&](void * requestPayload) { auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); @@ -70,15 +68,12 @@ rmw_send_request( if (iceoryx_client_abstraction->is_fixed_size_) { - std::cout << "FIXED SIZE!" << std::endl; memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_); } else { - std::cout << "NOT FIXED SIZE!" << std::endl; // message is not fixed size, so we have to serialize std::vector payload_vector{}; - rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector); memcpy(requestPayload, payload_vector.data(), payload_vector.size()); } @@ -153,23 +148,23 @@ rmw_take_request( RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; }); - return ret; + + if (ret == RMW_RET_ERROR) { + return ret; + } // if fixed size, we fetch the data via memcpy if (iceoryx_service_abstraction->is_fixed_size_) { memcpy(ros_request, user_payload, chunk_header->userPayloadSize()); - iceoryx_server->releaseRequest(user_payload); - *taken = true; - ret = RMW_RET_OK; } else { rmw_iceoryx_cpp::deserialize( static_cast(user_payload), &iceoryx_service_abstraction->type_supports_, ros_request); - iceoryx_server->releaseRequest(user_payload); - *taken = true; - ret = RMW_RET_OK; } + iceoryx_server->releaseRequest(user_payload); + *taken = true; + ret = RMW_RET_OK; return ret; } diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 0eda01f..4f98b6e 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -18,6 +18,7 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" #include "./types/iceoryx_client.hpp" @@ -92,18 +93,15 @@ rmw_take_response( memcpy(request_header, user_payload, sizeof(*request_header)); /// @todo cast to uint8_t before doing pointer arithmetic? memcpy(ros_response, user_payload + sizeof(*request_header), chunk_header->userPayloadSize()); - iceoryx_client->releaseResponse(user_payload); - *taken = true; - ret = RMW_RET_OK; } else { rmw_iceoryx_cpp::deserialize( - static_cast(user_payload), /// @todo add fourth param for 'request_header' + static_cast(user_payload), /// @todo add fourth param for 'request_header', but how to find out when header ends, separator char? &iceoryx_client_abstraction->type_supports_, ros_response); - iceoryx_client->releaseResponse(user_payload); - *taken = true; - ret = RMW_RET_OK; } + iceoryx_client->releaseResponse(user_payload); + *taken = true; + ret = RMW_RET_OK; *taken = false; return ret; @@ -124,13 +122,13 @@ rmw_send_response( : service, service->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto iceoryx_service_abstraction = static_cast(service->data); - if (!iceoryx_service_abstraction) { + auto iceoryx_server_abstraction = static_cast(service->data); + if (!iceoryx_server_abstraction) { RMW_SET_ERROR_MSG("service data is null"); return RMW_RET_ERROR; } - auto iceoryx_server = iceoryx_service_abstraction->iceoryx_server_; + auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; if (!iceoryx_server) { RMW_SET_ERROR_MSG("iceoryx_server is null"); return RMW_RET_ERROR; @@ -138,14 +136,24 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; - auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_service_abstraction->request_payload_); - iceoryx_server->loan(requestHeader, iceoryx_service_abstraction->response_size_, iceoryx_service_abstraction->response_alignment_) + auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_); + iceoryx_server->loan(requestHeader, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_) .and_then([&](void * responsePayload) { /// @todo memcpy or serialize the response - // 1) init message like pub/sub? - // 2) write | request_header | ros_response | to shared memory - memcpy(responsePayload, request_header, sizeof(*request_header)); - memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_service_abstraction->response_size_); + // write |-request_header-|-ros_response-| to shared memory + if (iceoryx_server_abstraction->is_fixed_size_) + { + memcpy(responsePayload, request_header, sizeof(*request_header)); + memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_server_abstraction->response_size_); + } + else + { + // message is not fixed size, so we have to serialize + std::vector payload_vector{}; + rmw_iceoryx_cpp::serializeRequest(request_header, &iceoryx_server_abstraction->type_supports_, payload_vector); + rmw_iceoryx_cpp::serializeRequest(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); + memcpy(responsePayload, payload_vector.data(), payload_vector.size()); + } iceoryx_server->send(responsePayload).or_else( [&](auto&) { RMW_SET_ERROR_MSG("rmw_send_response send error!"); From 1911d21f3ef5443ce8362778f0e0009a094a28a1 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 4 Jan 2023 23:44:10 +0100 Subject: [PATCH 12/27] Add missing remove-from-waitset part (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 3 ++- rmw_iceoryx_cpp/src/rmw_response.cpp | 4 +++- rmw_iceoryx_cpp/src/rmw_wait.cpp | 36 ++++++++++++++++++++++++---- 3 files changed, 37 insertions(+), 6 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 7ec0a9a..a213b76 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -77,7 +77,7 @@ rmw_send_request( rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector); memcpy(requestPayload, payload_vector.data(), payload_vector.size()); } - + std::cout << "Client sent request!" << std::endl; iceoryx_client->send(requestPayload).or_else( [&](auto&) { RMW_SET_ERROR_MSG("rmw_send_request error!"); @@ -165,6 +165,7 @@ rmw_take_request( iceoryx_server->releaseRequest(user_payload); *taken = true; ret = RMW_RET_OK; + std::cout << "Server took request!" << std::endl; return ret; } diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 4f98b6e..7b5dad4 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -74,7 +74,7 @@ rmw_take_response( } else { - std::cout << "Got Response with outdated sequence number!" << std::endl; + std::cout << "Got Response with outdated sequence number!" << std::endl; ret = RMW_RET_ERROR; } }) @@ -102,6 +102,7 @@ rmw_take_response( iceoryx_client->releaseResponse(user_payload); *taken = true; ret = RMW_RET_OK; + std::cout << "Client took response!" << std::endl; *taken = false; return ret; @@ -154,6 +155,7 @@ rmw_send_response( rmw_iceoryx_cpp::serializeRequest(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } + std::cout << "Server sent response!" << std::endl; iceoryx_server->send(responsePayload).or_else( [&](auto&) { RMW_SET_ERROR_MSG("rmw_send_response send error!"); diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp index d270679..2ea4377 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait.cpp @@ -75,12 +75,12 @@ rmw_wait( // attach all iceoryx servers to WaitSet for (size_t i = 0; i < services->service_count; ++i) { auto iceoryx_server_abstraction = - static_cast(subscriptions->subscribers[i]); + static_cast(services->services[i]); auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; waitset->attachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST).or_else( [&](auto &) { - RMW_SET_ERROR_MSG("failed to attach subscriber"); + RMW_SET_ERROR_MSG("failed to attach service"); skip_wait = true; }); } @@ -88,12 +88,12 @@ rmw_wait( // attach all iceoryx client to WaitSet for (size_t i = 0; i < clients->client_count; ++i) { auto iceoryx_client_abstraction = - static_cast(subscriptions->subscribers[i]); + static_cast(clients->clients[i]); auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; waitset->attachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE).or_else( [&](auto &) { - RMW_SET_ERROR_MSG("failed to attach subscriber"); + RMW_SET_ERROR_MSG("failed to attach client"); skip_wait = true; }); } @@ -141,6 +141,34 @@ rmw_wait( } } + // reset all the servers that don't have new data + for (size_t i = 0; i < services->service_count; ++i) { + auto iceoryx_server_abstraction = + static_cast(services->services[i]); + iox::popo::UntypedServer * iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; + + // remove waitset from all receivers because next call a new waitset could be provided + waitset->detachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST); + + if (!iceoryx_server->hasRequests()) { + services->services[i] = nullptr; + } + } + + // reset all the clients that don't have new data + for (size_t i = 0; i < clients->client_count; ++i) { + auto iceoryx_client_abstraction = + static_cast(clients->clients[i]); + iox::popo::UntypedClient * iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; + + // remove waitset from all receivers because next call a new waitset could be provided + waitset->detachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE); + + if (!iceoryx_client->hasResponses()) { + clients->clients[i] = nullptr; + } + } + // reset all the guard_conditions that have not triggered for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { auto iceoryx_guard_condition = From 917da67f67fe04c57c988577049bc02109917bad Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 11:47:04 +0100 Subject: [PATCH 13/27] Do not send 'request_header' over user payload (#76) Signed-off-by: Simon Hoinkis --- .../rmw_iceoryx_cpp/iceoryx_deserialize.hpp | 8 +++- .../src/internal/iceoryx_deserialize.cpp | 20 ++++++++- rmw_iceoryx_cpp/src/rmw_request.cpp | 13 ++++-- rmw_iceoryx_cpp/src/rmw_response.cpp | 42 ++++++++++--------- 4 files changed, 57 insertions(+), 26 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp index 82829f2..4d5ad86 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp @@ -27,8 +27,12 @@ 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( +void deserializeRequest( + const char * serialized_msg, + const rosidl_service_type_support_t * type_supports, + void * ros_message); + +void deserializeResponse( const char * serialized_msg, const rosidl_service_type_support_t * type_supports, void * ros_message); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp index e9806cb..bf3ea69 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp @@ -51,7 +51,25 @@ void deserialize( } } -void deserialize( +void deserializeRequest( + 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(ts.second->data); + rmw_iceoryx_cpp::details_cpp::deserializeRequest(serialized_msg, members_cpp, ros_message); + } else if (ts.first == TypeSupportLanguage::C) { + auto members_c = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_c::deserializeRequest(serialized_msg, members_c, ros_message); + } +} + +void deserializeResponse( const char * serialized_msg, const rosidl_service_type_support_t * type_supports, void * ros_message) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index a213b76..23b3244 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -136,8 +136,8 @@ rmw_take_request( iceoryx_server->take() .and_then( - [&](const void * requestPayload) { - user_payload = requestPayload; + [&](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); ret = RMW_RET_OK; @@ -145,6 +145,7 @@ rmw_take_request( .or_else( [&](iox::popo::ServerRequestResult& error) { std::cout << "Could not take request! Error: " << error << std::endl; + *taken = false; RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; }); @@ -157,12 +158,16 @@ rmw_take_request( if (iceoryx_service_abstraction->is_fixed_size_) { memcpy(ros_request, user_payload, chunk_header->userPayloadSize()); } else { - rmw_iceoryx_cpp::deserialize( + rmw_iceoryx_cpp::deserializeRequest( static_cast(user_payload), &iceoryx_service_abstraction->type_supports_, ros_request); } - iceoryx_server->releaseRequest(user_payload); + request_header->source_timestamp = 0; // Unsupported until needed + rcutils_system_time_now(&request_header->received_timestamp); + request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); + request_header->request_id.writer_guid[0] = 42; /// @todo + *taken = true; ret = RMW_RET_OK; std::cout << "Server took request!" << std::endl; diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 7b5dad4..c0f2201 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -63,23 +63,29 @@ rmw_take_response( iceoryx_client->take() .and_then( - [&](const void * responsePayload) { - auto responseHeader = iox::popo::ResponseHeader::fromPayload(responsePayload); - /// @todo check writer guid - if (responseHeader->getSequenceId() == request_header->request_id.sequence_number) + [&](const void * iceoryx_response_payload) { + auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload(iceoryx_response_payload); + /// @todo check writer guid? + request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); + request_header->source_timestamp = 0; // Unsupported until needed + rcutils_system_time_now(&request_header->received_timestamp); + + if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { - user_payload = responseHeader; + user_payload = iceoryx_response_payload; chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); ret = RMW_RET_OK; } else { - std::cout << "Got Response with outdated sequence number!" << std::endl; + RMW_SET_ERROR_MSG("Got response with outdated sequence number!"); + *taken = false; ret = RMW_RET_ERROR; } }) .or_else( [&](iox::popo::ChunkReceiveResult) { + *taken = false; RMW_SET_ERROR_MSG("No chunk in iceoryx_client"); ret = RMW_RET_ERROR; }); @@ -90,12 +96,10 @@ rmw_take_response( // if fixed size, we fetch the data via memcpy if (iceoryx_client_abstraction->is_fixed_size_) { - memcpy(request_header, user_payload, sizeof(*request_header)); - /// @todo cast to uint8_t before doing pointer arithmetic? - memcpy(ros_response, user_payload + sizeof(*request_header), chunk_header->userPayloadSize()); + memcpy(ros_response, user_payload, chunk_header->userPayloadSize()); } else { - rmw_iceoryx_cpp::deserialize( - static_cast(user_payload), /// @todo add fourth param for 'request_header', but how to find out when header ends, separator char? + rmw_iceoryx_cpp::deserializeResponse( + static_cast(user_payload), &iceoryx_client_abstraction->type_supports_, ros_response); } @@ -104,7 +108,6 @@ rmw_take_response( ret = RMW_RET_OK; std::cout << "Client took response!" << std::endl; - *taken = false; return ret; } @@ -138,21 +141,19 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_); + requestHeader->setSequenceId(request_header->sequence_number); + iceoryx_server->loan(requestHeader, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_) .and_then([&](void * responsePayload) { - /// @todo memcpy or serialize the response - // write |-request_header-|-ros_response-| to shared memory if (iceoryx_server_abstraction->is_fixed_size_) { - memcpy(responsePayload, request_header, sizeof(*request_header)); - memcpy(responsePayload + sizeof(*request_header), ros_response, iceoryx_server_abstraction->response_size_); + memcpy(responsePayload, ros_response, iceoryx_server_abstraction->response_size_); } else { // message is not fixed size, so we have to serialize std::vector payload_vector{}; - rmw_iceoryx_cpp::serializeRequest(request_header, &iceoryx_server_abstraction->type_supports_, payload_vector); - rmw_iceoryx_cpp::serializeRequest(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); + rmw_iceoryx_cpp::serializeResponse(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } std::cout << "Server sent response!" << std::endl; @@ -163,11 +164,14 @@ rmw_send_response( }); }) .or_else( - [&](auto&) { + [&](auto& error) { + std::cout << "Could not allocate Response! Error: " << error << std::endl; RMW_SET_ERROR_MSG("rmw_send_response loan error!"); ret = RMW_RET_ERROR; }); + iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_); + return ret; } } // extern "C" From 56cc84073caaedd29196e8a0473e9d5ced34a56c Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 12:28:00 +0100 Subject: [PATCH 14/27] Properly save the sample in 'rwm_take_request' (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 31 +++++++++++--------- rmw_iceoryx_cpp/src/rmw_response.cpp | 16 ++++++---- rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 2 +- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 2 +- 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 23b3244..633d575 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -77,19 +77,20 @@ rmw_send_request( rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector); memcpy(requestPayload, payload_vector.data(), payload_vector.size()); } - std::cout << "Client sent request!" << std::endl; - iceoryx_client->send(requestPayload).or_else( - [&](auto&) { - RMW_SET_ERROR_MSG("rmw_send_request error!"); - ret = RMW_RET_ERROR; - }); + iceoryx_client->send(requestPayload).and_then([&]{ + std::cout << "Client sent request!" << std::endl; + ret = RMW_RET_OK; + }).or_else( + [&](auto&) { + RMW_SET_ERROR_MSG("rmw_send_request error!"); + ret = RMW_RET_ERROR; + }); }) .or_else([&](auto&) { RMW_SET_ERROR_MSG("rmw_send_request error!"); ret = RMW_RET_ERROR; }); - ret = RMW_RET_OK; return ret; } @@ -110,20 +111,20 @@ rmw_take_request( : service, service->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto iceoryx_service_abstraction = static_cast(service->data); - if (!iceoryx_service_abstraction) { + auto iceoryx_server_abstraction = static_cast(service->data); + if (!iceoryx_server_abstraction) { RMW_SET_ERROR_MSG("service data is null"); return RMW_RET_ERROR; } - auto iceoryx_server = iceoryx_service_abstraction->iceoryx_server_; + auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; if (!iceoryx_server) { RMW_SET_ERROR_MSG("iceoryx_server is null"); return RMW_RET_ERROR; } // 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_)) { + if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_server_abstraction->type_supports_)) { RMW_SET_ERROR_MSG("Use either C typesupport or CPP typesupport"); return RMW_RET_ERROR; } @@ -155,12 +156,12 @@ rmw_take_request( } // if fixed size, we fetch the data via memcpy - if (iceoryx_service_abstraction->is_fixed_size_) { + 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_service_abstraction->type_supports_, + &iceoryx_server_abstraction->type_supports_, ros_request); } request_header->source_timestamp = 0; // Unsupported until needed @@ -168,8 +169,10 @@ rmw_take_request( request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); request_header->request_id.writer_guid[0] = 42; /// @todo + // Hold the loaned request till we send the response in 'rmw_send_response' + iceoryx_server_abstraction->request_payload_ = user_payload; + *taken = true; - ret = RMW_RET_OK; std::cout << "Server took request!" << std::endl; return ret; diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index c0f2201..36f34a7 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -105,7 +105,6 @@ rmw_take_response( } iceoryx_client->releaseResponse(user_payload); *taken = true; - ret = RMW_RET_OK; std::cout << "Client took response!" << std::endl; return ret; @@ -140,10 +139,12 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; - auto requestHeader = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_); - requestHeader->setSequenceId(request_header->sequence_number); + auto* iceoryx_request_header = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_); + /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, we need to compare + /// the user-provided sequence id with the one from the 'iceoryx_request_header' + //iceoryx_request_header->setSequenceId(request_header->sequence_number); - iceoryx_server->loan(requestHeader, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_) + iceoryx_server->loan(iceoryx_request_header, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_) .and_then([&](void * responsePayload) { if (iceoryx_server_abstraction->is_fixed_size_) { @@ -156,8 +157,10 @@ rmw_send_response( rmw_iceoryx_cpp::serializeResponse(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } - std::cout << "Server sent response!" << std::endl; - iceoryx_server->send(responsePayload).or_else( + iceoryx_server->send(responsePayload).and_then([&]{ + std::cout << "Server sent response!" << std::endl; + ret = RMW_RET_OK; + }).or_else( [&](auto&) { RMW_SET_ERROR_MSG("rmw_send_response send error!"); ret = RMW_RET_ERROR; @@ -170,6 +173,7 @@ rmw_send_response( ret = RMW_RET_ERROR; }); + // Release the hold request iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_); return ret; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp index 7f6b887..703b622 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -39,7 +39,7 @@ struct IceoryxClient size_t request_size_; /// @todo How can the full type be aquired via rosidl to do an 'alignof()'? uint32_t request_alignment_{8}; - uint64_t sequence_id_; + int64_t sequence_id_; }; #endif // TYPES__ICEORYX_CLIENT_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index c68c7b5..c6c57e7 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -39,7 +39,7 @@ struct IceoryxServer size_t response_size_; /// @todo Is there a way to get the aligment for a complete type via the rosidl? uint32_t response_alignment_{8}; - void * request_payload_; + const void * request_payload_; }; #endif // TYPES__ICEORYX_SERVER_HPP_ From 57cc79ac8e07763b3b3aff0963ac772c188fead8 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 15:37:30 +0100 Subject: [PATCH 15/27] Fix lost responses issue (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_response.cpp | 3 +++ .../src/rmw_service_server_is_available.cpp | 11 +++++------ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 36f34a7..9fe0c4c 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -157,6 +157,8 @@ rmw_send_response( rmw_iceoryx_cpp::serializeResponse(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } + /// @todo Why are the sleep before and after 'send()' needed? rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); iceoryx_server->send(responsePayload).and_then([&]{ std::cout << "Server sent response!" << std::endl; ret = RMW_RET_OK; @@ -165,6 +167,7 @@ rmw_send_response( RMW_SET_ERROR_MSG("rmw_send_response send error!"); ret = RMW_RET_ERROR; }); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); }) .or_else( [&](auto& error) { diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index 0b61655..bc6e0ff 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -51,13 +51,12 @@ rmw_service_server_is_available( std::this_thread::sleep_for(std::chrono::seconds(1)); - *is_available = true; - // if(iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) - // { - // *is_available = true; - // } + if(iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) + { + *is_available = true; + } - /// @todo better to go through service discovery? + /// @todo Better to go through service discovery? // iox::runtime::ServiceDiscovery serviceDiscovery; // auto& searchItem = iceoryx_client->getServiceDescription(); // serviceDiscovery.findService( searchItem.getServiceIDString(), From ce02908d959347a3b88696761584bef10c62e8b3 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 16:35:02 +0100 Subject: [PATCH 16/27] Implement 'rmw_get_service_names_and_types' (#76) Signed-off-by: Simon Hoinkis --- .../iceoryx_topic_names_and_types.hpp | 1 + .../iceoryx_get_topic_endpoint_info.cpp | 4 +-- .../iceoryx_topic_names_and_types.cpp | 36 +++++++++++++++++++ rmw_iceoryx_cpp/src/rmw_request.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_response.cpp | 2 +- .../src/rmw_service_names_and_types.cpp | 20 +++++++++-- .../src/rmw_service_server_is_available.cpp | 2 +- 7 files changed, 60 insertions(+), 7 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp index bc70e9b..dae8503 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp @@ -35,6 +35,7 @@ void fill_topic_containers( std::map> & topic_publishers_); std::map get_topic_names_and_types(); +std::map get_service_names_and_types(); std::map> get_nodes_and_publishers(); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp index f59968a..7f1a0ae 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp @@ -117,7 +117,7 @@ fill_rmw_publisher_end_info( int i = 0; // store all data in rmw_topic_endpoint_info_array_t - for (const auto node_full_name : full_name_array) { + for (const auto& node_full_name : full_name_array) { auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); // duplicate and store the topic_name @@ -204,7 +204,7 @@ fill_rmw_subscriber_end_info( int i = 0; // store all data in rmw_topic_endpoint_info_array_t - for (const auto node_full_name : full_name_array) { + for (const auto& node_full_name : full_name_array) { auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); // duplicate and store the topic_name diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp index 198bb08..d1e0166 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp @@ -28,6 +28,8 @@ #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" +#include "iceoryx_posh/runtime/service_discovery.hpp" + namespace rmw_iceoryx_cpp { @@ -140,6 +142,40 @@ std::map get_topic_names_and_types() return names_n_types; } +std::map get_service_names_and_types() +{ + std::map names_n_types; + std::map> clients_topics; // Currently, no possibility to query for clients.. + std::map> servers_topics; // Can be discovered via 'ServiceDiscovery' + std::map> topic_clients; // Currently, no possibility to query for clients.. + std::map> topic_servers; // Can be discovered, but missing associated node + + std::vector available_servers; + + iox::runtime::ServiceDiscovery service_discovery; + service_discovery.findService(iox::cxx::nullopt, + iox::cxx::nullopt, + iox::cxx::nullopt, + [&](auto& service_description){ available_servers.push_back(service_description); }, + iox::popo::MessagingPattern::REQ_RES); + + for (auto & server : available_servers) { + auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( + std::string(server.getServiceIDString().c_str()), + std::string(server.getInstanceIDString().c_str()), + std::string(server.getEventIDString().c_str())); + + names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type); + /// @todo There is no API to find out which 'ServiceDescription' is offered by which node, for now we use 'NodeFoo'.. + servers_topics[std::string("NodeFoo")].push_back( + std::get<0>( + name_and_type)); + topic_servers[std::get<0>(name_and_type)].push_back(std::string("NodeFoo")); + } + + return names_n_types; +} + std::map> get_nodes_and_publishers() { std::map names_n_types; diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 633d575..57b8ce1 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -165,7 +165,7 @@ rmw_take_request( ros_request); } request_header->source_timestamp = 0; // Unsupported until needed - rcutils_system_time_now(&request_header->received_timestamp); + ret = rcutils_system_time_now(&request_header->received_timestamp); request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); request_header->request_id.writer_guid[0] = 42; /// @todo diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 9fe0c4c..b7cf62e 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -68,7 +68,7 @@ rmw_take_response( /// @todo check writer guid? request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); request_header->source_timestamp = 0; // Unsupported until needed - rcutils_system_time_now(&request_header->received_timestamp); + ret = rcutils_system_time_now(&request_header->received_timestamp); if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { diff --git a/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp b/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp index 8463144..19ef64d 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp @@ -19,6 +19,8 @@ #include "rmw/names_and_types.h" #include "rmw/rmw.h" +#include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" + extern "C" { rmw_ret_t @@ -31,7 +33,21 @@ rmw_get_service_names_and_types( RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support services."); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_get_service_names_and_types + : node, node->implementation_identifier, + rmw_get_implementation_identifier(), return RMW_RET_ERROR); + + rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; // error already set + } + + auto iceoryx_service_names_and_types = rmw_iceoryx_cpp::get_service_names_and_types(); + + return rmw_iceoryx_cpp::fill_rmw_names_and_types( + service_names_and_types, iceoryx_service_names_and_types, allocator); + + return RMW_RET_OK; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index bc6e0ff..f661558 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -19,7 +19,7 @@ #include "./types/iceoryx_client.hpp" -#include "iceoryx_posh/runtime/service_discovery.hpp" +//#include "iceoryx_posh/runtime/service_discovery.hpp" extern "C" { From eebf11b751fa7a033d0d563a861d470c900051d8 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 16:42:26 +0100 Subject: [PATCH 17/27] Format all the things (#76) Signed-off-by: Simon Hoinkis --- .../iceoryx_type_info_introspection.hpp | 9 --- .../iceoryx_deserialize_typesupport_c.hpp | 4 +- .../iceoryx_deserialize_typesupport_cpp.hpp | 4 +- .../iceoryx_get_topic_endpoint_info.cpp | 4 +- .../src/internal/iceoryx_name_conversion.cpp | 4 +- .../src/internal/iceoryx_serialize.cpp | 4 +- .../iceoryx_topic_names_and_types.cpp | 13 ++-- .../iceoryx_type_info_introspection.cpp | 14 +++- rmw_iceoryx_cpp/src/rmw_client.cpp | 68 ++++++++-------- rmw_iceoryx_cpp/src/rmw_request.cpp | 73 +++++++++-------- rmw_iceoryx_cpp/src/rmw_response.cpp | 78 ++++++++++--------- rmw_iceoryx_cpp/src/rmw_service.cpp | 68 ++++++++-------- .../src/rmw_service_server_is_available.cpp | 3 +- 13 files changed, 175 insertions(+), 171 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index 4bf9ebc..68ad109 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -61,18 +61,9 @@ void iceoryx_init_message( const rosidl_message_type_support_t * type_supports, void * message); -void iceoryx_init_message( // init_response, init_request - const rosidl_service_type_support_t * type_supports, - void * message); - void iceoryx_fini_message( const rosidl_message_type_support_t * type_supports, void * message); -/// @todo -// void iceoryx_fini_message( -// const rosidl_service_type_support_t * type_supports, -// void * message); - } // namespace rmw_iceoryx_cpp #endif // RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp index 208fc84..2d8dc16 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp @@ -239,7 +239,7 @@ const char * deserialize( return serialized_msg; } -const char* deserializeResponse( +const char * deserializeResponse( const char * serialized_msg, const rosidl_typesupport_introspection_c__ServiceMembers * service_members, void * ros_message) @@ -247,7 +247,7 @@ const char* deserializeResponse( return deserialize(serialized_msg, service_members->response_members_, ros_message); } -const char* deserializeRequest( +const char * deserializeRequest( const char * serialized_msg, const rosidl_typesupport_introspection_c__ServiceMembers * service_members, void * ros_message) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp index 6d80a2c..6623969 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp @@ -309,7 +309,7 @@ const char * deserialize( return serialized_msg; } -const char* deserializeResponse( +const char * deserializeResponse( const char * serialized_msg, const rosidl_typesupport_introspection_cpp::ServiceMembers * service_members, void * ros_message) @@ -317,7 +317,7 @@ const char* deserializeResponse( return deserialize(serialized_msg, service_members->response_members_, ros_message); } -const char* deserializeRequest( +const char * deserializeRequest( const char * serialized_msg, const rosidl_typesupport_introspection_cpp::ServiceMembers * service_members, void * ros_message) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp index 7f1a0ae..2b82e4c 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp @@ -117,7 +117,7 @@ fill_rmw_publisher_end_info( int i = 0; // store all data in rmw_topic_endpoint_info_array_t - for (const auto& node_full_name : full_name_array) { + for (const auto & node_full_name : full_name_array) { auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); // duplicate and store the topic_name @@ -204,7 +204,7 @@ fill_rmw_subscriber_end_info( int i = 0; // store all data in rmw_topic_endpoint_info_array_t - for (const auto& node_full_name : full_name_array) { + for (const auto & node_full_name : full_name_array) { auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); // duplicate and store the topic_name diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp index 86b165d..632eddc 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp @@ -169,7 +169,9 @@ get_service_description_from_name_n_type( return std::make_tuple(service, instance, event); } -iox::capro::ServiceDescription make_service_description(const std::string & topic_name, const std::string & type_name) +iox::capro::ServiceDescription make_service_description( + const std::string & topic_name, + const std::string & type_name) { auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp index 28388c0..5fc1453 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp @@ -82,7 +82,9 @@ void serializeResponse( if (ts.first == TypeSupportLanguage::CPP) { auto members = static_cast(ts.second->data); - rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members->response_members_, payload_vector); + rmw_iceoryx_cpp::details_cpp::serialize( + ros_message, members->response_members_, + payload_vector); } else if (ts.first == TypeSupportLanguage::C) { auto members = static_cast(ts.second->data); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp index d1e0166..18c946f 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp @@ -148,16 +148,17 @@ std::map get_service_names_and_types() std::map> clients_topics; // Currently, no possibility to query for clients.. std::map> servers_topics; // Can be discovered via 'ServiceDiscovery' std::map> topic_clients; // Currently, no possibility to query for clients.. - std::map> topic_servers; // Can be discovered, but missing associated node + std::map> topic_servers; // Can be discovered, but missing associated node std::vector available_servers; iox::runtime::ServiceDiscovery service_discovery; - service_discovery.findService(iox::cxx::nullopt, - iox::cxx::nullopt, - iox::cxx::nullopt, - [&](auto& service_description){ available_servers.push_back(service_description); }, - iox::popo::MessagingPattern::REQ_RES); + service_discovery.findService( + iox::cxx::nullopt, + iox::cxx::nullopt, + iox::cxx::nullopt, + [&](auto & service_description) {available_servers.push_back(service_description);}, + iox::popo::MessagingPattern::REQ_RES); for (auto & server : available_servers) { auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index 0a2159c..fe6cba0 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -233,11 +233,13 @@ bool iceoryx_is_fixed_size(const rosidl_service_type_support_t * type_supports) if (ts.first == TypeSupportLanguage::CPP) { auto members = static_cast(ts.second->data); - return details_cpp::is_fixed_size(members->request_members_) && details_cpp::is_fixed_size(members->response_members_); + return details_cpp::is_fixed_size(members->request_members_) && details_cpp::is_fixed_size( + members->response_members_); } else if (ts.first == TypeSupportLanguage::C) { auto members = static_cast(ts.second->data); - return details_c::is_fixed_size(members->request_members_) && details_c::is_fixed_size(members->response_members_); + return details_c::is_fixed_size(members->request_members_) && details_c::is_fixed_size( + members->response_members_); } // Something went wrong return false; @@ -412,8 +414,12 @@ void iceoryx_init_message( if (ts.first == TypeSupportLanguage::CPP) { auto members = static_cast(ts.second->data); - members->request_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); - members->response_members_->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); + members->request_members_->init_function( + message, + rosidl_runtime_cpp::MessageInitialization::ALL); + members->response_members_->init_function( + message, + rosidl_runtime_cpp::MessageInitialization::ALL); return; } else if (ts.first == TypeSupportLanguage::C) { auto members = diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 6c49c6d..70ed2ec 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -38,9 +38,9 @@ rmw_create_client( RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, NULL); RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, NULL); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - rmw_create_client - : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_create_client + : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); // create the iceoryx service description for a sender auto service_description = @@ -53,25 +53,25 @@ rmw_create_client( bool returnOnError = false; - auto cleanupAfterError = [&](){ - if (rmw_client) { - if (iceoryx_client) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_client->~UntypedClient(), iox::popo::UntypedClient) - rmw_free(iceoryx_client); - } - if (iceoryx_client_abstraction) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_client_abstraction->~IceoryxClient(), IceoryxClient) - rmw_free(iceoryx_client_abstraction); - } - if (rmw_client->service_name) { - rmw_free(const_cast(rmw_client->service_name)); + auto cleanupAfterError = [&]() { + if (rmw_client) { + if (iceoryx_client) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_client->~UntypedClient(), iox::popo::UntypedClient) + rmw_free(iceoryx_client); + } + if (iceoryx_client_abstraction) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_client_abstraction->~IceoryxClient(), IceoryxClient) + rmw_free(iceoryx_client_abstraction); + } + if (rmw_client->service_name) { + rmw_free(const_cast(rmw_client->service_name)); + } + rmw_client_free(rmw_client); + returnOnError = true; } - rmw_client_free(rmw_client); - returnOnError = true; - } - }; + }; rmw_client = rmw_client_allocate(); if (!rmw_client) { @@ -94,8 +94,7 @@ rmw_create_client( cleanupAfterError(), iox::popo::UntypedClient, service_description, iox::popo::ClientOptions{ 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); - if(returnOnError) - { + if (returnOnError) { return nullptr; } @@ -111,8 +110,7 @@ rmw_create_client( RMW_TRY_PLACEMENT_NEW( iceoryx_client_abstraction, iceoryx_client_abstraction, cleanupAfterError(), IceoryxClient, type_supports, iceoryx_client); - if(returnOnError) - { + if (returnOnError) { return nullptr; } @@ -147,18 +145,18 @@ rmw_destroy_client( IceoryxClient * iceoryx_client_abstraction = static_cast(client->data); if (iceoryx_client_abstraction) { - if (iceoryx_client_abstraction->iceoryx_client_) { - RMW_TRY_DESTRUCTOR( - iceoryx_client_abstraction->iceoryx_client_->~UntypedClient(), - iceoryx_client_abstraction->iceoryx_client_, - result = RMW_RET_ERROR) - rmw_free(iceoryx_client_abstraction->iceoryx_client_); - } + if (iceoryx_client_abstraction->iceoryx_client_) { RMW_TRY_DESTRUCTOR( - iceoryx_client_abstraction->~IceoryxClient(), - iceoryx_client_abstraction, + iceoryx_client_abstraction->iceoryx_client_->~UntypedClient(), + iceoryx_client_abstraction->iceoryx_client_, result = RMW_RET_ERROR) - rmw_free(iceoryx_client_abstraction); + rmw_free(iceoryx_client_abstraction->iceoryx_client_); + } + RMW_TRY_DESTRUCTOR( + iceoryx_client_abstraction->~IceoryxClient(), + iceoryx_client_abstraction, + result = RMW_RET_ERROR) + rmw_free(iceoryx_client_abstraction); } client->data = nullptr; diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 57b8ce1..47f55c3 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -58,38 +58,42 @@ rmw_send_request( return ret; } - iceoryx_client->loan(iceoryx_client_abstraction->request_size_, iceoryx_client_abstraction->request_alignment_) - .and_then([&](void * requestPayload) { - auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); - - requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_); - *sequence_id = iceoryx_client_abstraction->sequence_id_; - iceoryx_client_abstraction->sequence_id_ += 1; - - if (iceoryx_client_abstraction->is_fixed_size_) - { - memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_); - } - else - { - // message is not fixed size, so we have to serialize - std::vector payload_vector{}; - rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector); - memcpy(requestPayload, payload_vector.data(), payload_vector.size()); - } - iceoryx_client->send(requestPayload).and_then([&]{ - std::cout << "Client sent request!" << std::endl; - ret = RMW_RET_OK; - }).or_else( - [&](auto&) { - RMW_SET_ERROR_MSG("rmw_send_request error!"); - ret = RMW_RET_ERROR; - }); - }) - .or_else([&](auto&) { - RMW_SET_ERROR_MSG("rmw_send_request error!"); - ret = RMW_RET_ERROR; - }); + iceoryx_client->loan( + iceoryx_client_abstraction->request_size_, + iceoryx_client_abstraction->request_alignment_) + .and_then( + [&](void * requestPayload) { + auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); + + requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_); + *sequence_id = iceoryx_client_abstraction->sequence_id_; + iceoryx_client_abstraction->sequence_id_ += 1; + + if (iceoryx_client_abstraction->is_fixed_size_) { + memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_); + } else { + // message is not fixed size, so we have to serialize + std::vector payload_vector{}; + rmw_iceoryx_cpp::serializeRequest( + ros_request, &iceoryx_client_abstraction->type_supports_, + payload_vector); + memcpy(requestPayload, payload_vector.data(), payload_vector.size()); + } + iceoryx_client->send(requestPayload).and_then( + [&] { + std::cout << "Client sent request!" << std::endl; + ret = RMW_RET_OK; + }).or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("rmw_send_request error!"); + ret = RMW_RET_ERROR; + }); + }) + .or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("rmw_send_request error!"); + ret = RMW_RET_ERROR; + }); return ret; } @@ -124,7 +128,8 @@ rmw_take_request( } // this should never happen if checked already at rmw_create_service - if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_server_abstraction->type_supports_)) { + if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_server_abstraction->type_supports_)) + { RMW_SET_ERROR_MSG("Use either C typesupport or CPP typesupport"); return RMW_RET_ERROR; } @@ -144,7 +149,7 @@ rmw_take_request( ret = RMW_RET_OK; }) .or_else( - [&](iox::popo::ServerRequestResult& error) { + [&](iox::popo::ServerRequestResult & error) { std::cout << "Could not take request! Error: " << error << std::endl; *taken = false; RMW_SET_ERROR_MSG("rmw_take_request error!"); diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index b7cf62e..162ed46 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -64,20 +64,18 @@ rmw_take_response( iceoryx_client->take() .and_then( [&](const void * iceoryx_response_payload) { - auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload(iceoryx_response_payload); + auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload( + iceoryx_response_payload); /// @todo check writer guid? request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); request_header->source_timestamp = 0; // Unsupported until needed ret = rcutils_system_time_now(&request_header->received_timestamp); - if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) - { + if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { user_payload = iceoryx_response_payload; chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); ret = RMW_RET_OK; - } - else - { + } else { RMW_SET_ERROR_MSG("Got response with outdated sequence number!"); *taken = false; ret = RMW_RET_ERROR; @@ -139,42 +137,46 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; - auto* iceoryx_request_header = iox::popo::RequestHeader::fromPayload(iceoryx_server_abstraction->request_payload_); + auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload( + iceoryx_server_abstraction->request_payload_); /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, we need to compare /// the user-provided sequence id with the one from the 'iceoryx_request_header' //iceoryx_request_header->setSequenceId(request_header->sequence_number); - iceoryx_server->loan(iceoryx_request_header, iceoryx_server_abstraction->response_size_, iceoryx_server_abstraction->response_alignment_) - .and_then([&](void * responsePayload) { - if (iceoryx_server_abstraction->is_fixed_size_) - { - memcpy(responsePayload, ros_response, iceoryx_server_abstraction->response_size_); - } - else - { - // message is not fixed size, so we have to serialize - std::vector payload_vector{}; - rmw_iceoryx_cpp::serializeResponse(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector); - memcpy(responsePayload, payload_vector.data(), payload_vector.size()); - } - /// @todo Why are the sleep before and after 'send()' needed? rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - iceoryx_server->send(responsePayload).and_then([&]{ - std::cout << "Server sent response!" << std::endl; - ret = RMW_RET_OK; - }).or_else( - [&](auto&) { - RMW_SET_ERROR_MSG("rmw_send_response send error!"); - ret = RMW_RET_ERROR; - }); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - }) - .or_else( - [&](auto& error) { - std::cout << "Could not allocate Response! Error: " << error << std::endl; - RMW_SET_ERROR_MSG("rmw_send_response loan error!"); - ret = RMW_RET_ERROR; - }); + iceoryx_server->loan( + iceoryx_request_header, iceoryx_server_abstraction->response_size_, + iceoryx_server_abstraction->response_alignment_) + .and_then( + [&](void * responsePayload) { + if (iceoryx_server_abstraction->is_fixed_size_) { + memcpy(responsePayload, ros_response, iceoryx_server_abstraction->response_size_); + } else { + // message is not fixed size, so we have to serialize + std::vector payload_vector{}; + rmw_iceoryx_cpp::serializeResponse( + ros_response, + &iceoryx_server_abstraction->type_supports_, payload_vector); + memcpy(responsePayload, payload_vector.data(), payload_vector.size()); + } + /// @todo Why are the sleep before and after 'send()' needed? rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + iceoryx_server->send(responsePayload).and_then( + [&] { + std::cout << "Server sent response!" << std::endl; + ret = RMW_RET_OK; + }).or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("rmw_send_response send error!"); + ret = RMW_RET_ERROR; + }); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + }) + .or_else( + [&](auto & error) { + std::cout << "Could not allocate Response! Error: " << error << std::endl; + RMW_SET_ERROR_MSG("rmw_send_response loan error!"); + ret = RMW_RET_ERROR; + }); // Release the hold request iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_); diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index 4ef5c14..522aef0 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -38,8 +38,8 @@ rmw_create_service( RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - rmw_create_service - : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); + rmw_create_service + : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); // create the iceoryx service description for a sender auto service_description = @@ -52,25 +52,25 @@ rmw_create_service( bool returnOnError = false; - auto cleanupAfterError = [&](){ - if (rmw_service) { - if (iceoryx_server) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_server->~UntypedServer(), iox::popo::UntypedServer) - rmw_free(iceoryx_server); + auto cleanupAfterError = [&]() { + if (rmw_service) { + if (iceoryx_server) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_server->~UntypedServer(), iox::popo::UntypedServer) + rmw_free(iceoryx_server); + } + if (iceoryx_server_abstraction) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + iceoryx_server_abstraction->~IceoryxServer(), IceoryxServer) + rmw_free(iceoryx_server_abstraction); + } + if (rmw_service->service_name) { + rmw_free(const_cast(rmw_service->service_name)); + } + rmw_service_free(rmw_service); + returnOnError = true; } - if (iceoryx_server_abstraction) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_server_abstraction->~IceoryxServer(), IceoryxServer) - rmw_free(iceoryx_server_abstraction); - } - if (rmw_service->service_name) { - rmw_free(const_cast(rmw_service->service_name)); - } - rmw_service_free(rmw_service); - returnOnError = true; - } - }; + }; rmw_service = rmw_service_allocate(); if (!rmw_service) { @@ -94,11 +94,10 @@ rmw_create_service( cleanupAfterError(), iox::popo::UntypedServer, service_description, iox::popo::ServerOptions{ 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); - if(returnOnError) - { + if (returnOnError) { return nullptr; } - + iceoryx_server->offer(); iceoryx_server_abstraction = @@ -111,8 +110,7 @@ rmw_create_service( RMW_TRY_PLACEMENT_NEW( iceoryx_server_abstraction, iceoryx_server_abstraction, cleanupAfterError(), IceoryxServer, type_supports, iceoryx_server); - if(returnOnError) - { + if (returnOnError) { return nullptr; } @@ -146,18 +144,18 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) IceoryxServer * iceoryx_server_abstraction = static_cast(service->data); if (iceoryx_server_abstraction) { - if (iceoryx_server_abstraction->iceoryx_server_) { - RMW_TRY_DESTRUCTOR( - iceoryx_server_abstraction->iceoryx_server_->~UntypedServer(), - iceoryx_server_abstraction->iceoryx_server_, - result = RMW_RET_ERROR) - rmw_free(iceoryx_server_abstraction->iceoryx_server_); - } + if (iceoryx_server_abstraction->iceoryx_server_) { RMW_TRY_DESTRUCTOR( - iceoryx_server_abstraction->~IceoryxServer(), - iceoryx_server_abstraction, + iceoryx_server_abstraction->iceoryx_server_->~UntypedServer(), + iceoryx_server_abstraction->iceoryx_server_, result = RMW_RET_ERROR) - rmw_free(iceoryx_server_abstraction); + rmw_free(iceoryx_server_abstraction->iceoryx_server_); + } + RMW_TRY_DESTRUCTOR( + iceoryx_server_abstraction->~IceoryxServer(), + iceoryx_server_abstraction, + result = RMW_RET_ERROR) + rmw_free(iceoryx_server_abstraction); } service->data = nullptr; diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index f661558..d896bdb 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -51,8 +51,7 @@ rmw_service_server_is_available( std::this_thread::sleep_for(std::chrono::seconds(1)); - if(iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) - { + if (iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) { *is_available = true; } From 577ddce4a94bb8ebea4f646b0c699646ca752434 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 16:48:49 +0100 Subject: [PATCH 18/27] Update CI and docs (#76) Signed-off-by: Simon Hoinkis --- .github/workflows/custom_repos.repos | 5 ----- .github/workflows/main.yml | 9 ++++----- README.md | 21 ++++----------------- 3 files changed, 8 insertions(+), 27 deletions(-) delete mode 100644 .github/workflows/custom_repos.repos diff --git a/.github/workflows/custom_repos.repos b/.github/workflows/custom_repos.repos deleted file mode 100644 index 7cb785a..0000000 --- a/.github/workflows/custom_repos.repos +++ /dev/null @@ -1,5 +0,0 @@ -repositories: - eclipse-iceoryx/iceoryx: - type: git - url: https://github.com/eclipse-iceoryx/iceoryx.git - version: master diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 0f91e0d..619cf16 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -3,7 +3,7 @@ name: Integration build rmw_iceoryx on: push: branches: - - master + - humble pull_request: jobs: @@ -17,13 +17,12 @@ jobs: - uses: actions/checkout@v2 - name: Setup ROS uses: ros-tooling/setup-ros@master + with: + required-ros-distributions: humble - name: Install Iceoryx Dependencies run: sudo apt-get update && sudo apt-get install -y cmake libacl1-dev libncurses5-dev pkg-config - name: Build & Test uses: ros-tooling/action-ros-ci@master with: package-name: rmw_iceoryx_cpp - target-ros2-distro: rolling - vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos - ${{ github.workspace }}/.github/workflows/custom_repos.repos + target-ros2-distro: humble diff --git a/README.md b/README.md index 24eb631..0c14e70 100644 --- a/README.md +++ b/README.md @@ -15,26 +15,13 @@ To install rmw_iceoryx in a ROS 2 workspace with the latest ROS version, just ex ```bash mkdir -p ~/iceoryx_ws/src cd $_ -# LATEST_ROS_VERSION could be e.g. galactic +# LATEST_ROS_VERSION could be e.g. humble git clone --branch LATEST_ROS_VERSION https://github.com/ros2/rmw_iceoryx.git ``` For alternative installation instructions and more details about iceoryx's internals, please see [iceoryx's GitHub repo](https://github.com/eclipse/iceoryx). rmw_iceoryx is compatible with ROS 2 starting with Eloquent release. - -If you want to use ROS 2 rolling, you need to checkout the `master` branch of both rmw_iceoryx and iceoryx: - -```bash -cd ~/iceoryx_ws/ -wget https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos -# Change the line of Eclipse iceoryx to "version: master" -vcs import src < ros2.repos - -cd ~/iceoryx_ws/src -git clone --branch master https://github.com/ros2/rmw_iceoryx.git -``` - Assuming you have ROS 2 installed correctly, you can compile the iceoryx workspace with colcon: ```bash @@ -47,7 +34,7 @@ colcon build colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF ``` -That's it! You've installed rmw_iceoryx and are ready to rumble. +That's it! You've installed iceoryx and are ready to rumble. Working with rmw_iceoryx_cpp ============================ @@ -154,8 +141,8 @@ Unfortunately, not all features are yet fully fleshed out. | `ros2 node list` | :heavy_check_mark: | | `ros2 node info` | :heavy_check_mark: | | `ros2 interface *` | :heavy_check_mark: | -| `ros2 service *` | :heavy_check_mark | -| `ros2 param list` | :heavy_check_mark | +| `ros2 service *` | :heavy_check_mark: | +| `ros2 param list` | :x: | | `rqt_graph` | :heavy_check_mark: | | `rqt_top` | :heavy_check_mark: | | `rqt_console` | :heavy_check_mark: | From 3c278f16b86016302213ad630f441feccf0309ab Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 17:27:20 +0100 Subject: [PATCH 19/27] Add copyright notice to all touched files (#76) Signed-off-by: Simon Hoinkis --- .../rmw_iceoryx_cpp/iceoryx_deserialize.hpp | 1 + .../iceoryx_name_conversion.hpp | 1 + .../rmw_iceoryx_cpp/iceoryx_serialize.hpp | 1 + .../iceoryx_topic_names_and_types.hpp | 1 + .../iceoryx_type_info_introspection.hpp | 2 +- .../src/internal/iceoryx_deserialize.cpp | 2 +- .../iceoryx_deserialize_typesupport_c.hpp | 2 ++ .../iceoryx_deserialize_typesupport_cpp.hpp | 1 + .../iceoryx_get_topic_endpoint_info.cpp | 1 + .../src/internal/iceoryx_name_conversion.cpp | 1 + .../src/internal/iceoryx_serialize.cpp | 2 +- .../iceoryx_topic_names_and_types.cpp | 2 +- .../iceoryx_type_info_introspection.cpp | 27 +------------------ rmw_iceoryx_cpp/src/rmw_client.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_publish.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_request.cpp | 1 + rmw_iceoryx_cpp/src/rmw_service.cpp | 2 +- .../src/rmw_service_names_and_types.cpp | 1 + .../src/rmw_service_server_is_available.cpp | 1 + rmw_iceoryx_cpp/src/rmw_wait.cpp | 2 +- 20 files changed, 21 insertions(+), 34 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp index 4d5ad86..c2fca35 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. 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 5796e53..6b445c7 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 @@ -1,5 +1,6 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. // Copyright (c) 2021 by ZhenshengLee. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp index a8a0644..61ccb63 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp index dae8503..39b2a59 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index 68ad109..c69d633 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp index bf3ea69..4dcf434 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp index 2d8dc16..c049d46 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. @@ -27,6 +28,7 @@ #include "rosidl_typesupport_introspection_c/field_types.h" #include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" #include "./iceoryx_serialization_common.hpp" diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp index 6623969..80fb0d1 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_cpp.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp index 2b82e4c..5cad1ab 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2021 by ZhenshengLee. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp index 632eddc..8ece43a 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. // Copyright (c) 2021 by ZhenshengLee. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp index 5fc1453..67a98fa 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp index 18c946f..9f48cc2 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.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. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index fe6cba0..9112032 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.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. @@ -405,31 +405,6 @@ void iceoryx_init_message( } } -void iceoryx_init_message( - const rosidl_service_type_support_t * type_supports, - void * message) -{ - auto ts = get_type_support(type_supports); - - if (ts.first == TypeSupportLanguage::CPP) { - auto members = - static_cast(ts.second->data); - members->request_members_->init_function( - message, - rosidl_runtime_cpp::MessageInitialization::ALL); - members->response_members_->init_function( - message, - rosidl_runtime_cpp::MessageInitialization::ALL); - return; - } else if (ts.first == TypeSupportLanguage::C) { - auto members = - static_cast(ts.second->data); - members->request_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); - members->response_members_->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); - return; - } -} - void iceoryx_fini_message( const rosidl_message_type_support_t * type_supports, void * message) diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 70ed2ec..9c367af 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. -// Copyright (c) 2022 by Apex.AI Inc. All rights reserved. +// Copyright (c) 2022 - 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. diff --git a/rmw_iceoryx_cpp/src/rmw_publish.cpp b/rmw_iceoryx_cpp/src/rmw_publish.cpp index a7d8200..869b935 100644 --- a/rmw_iceoryx_cpp/src/rmw_publish.cpp +++ b/rmw_iceoryx_cpp/src/rmw_publish.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. -// Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved. +// Copyright (c) 2021 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. diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 47f55c3..df09096 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index 522aef0..14c6f24 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. -// Copyright (c) 2022 by Apex.AI Inc. All rights reserved. +// Copyright (c) 2022 - 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. diff --git a/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp b/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp index 19ef64d..a81f5c9 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index d896bdb..fb1ae7f 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 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. diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp index 2ea4377..d532120 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait.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. From 8cd3111491c825e15c0bc76bb8c239bc12365152 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 17:56:56 +0100 Subject: [PATCH 20/27] Fix cpplint warnings (#76) Signed-off-by: Simon Hoinkis --- .../internal/iceoryx_topic_names_and_types.cpp | 15 ++++++++++----- rmw_iceoryx_cpp/src/rmw_request.cpp | 4 ++-- rmw_iceoryx_cpp/src/rmw_response.cpp | 16 ++++++++++------ .../src/rmw_service_server_is_available.cpp | 2 +- 4 files changed, 23 insertions(+), 14 deletions(-) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp index 9f48cc2..54b822f 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp @@ -145,10 +145,14 @@ std::map get_topic_names_and_types() std::map get_service_names_and_types() { std::map names_n_types; - std::map> clients_topics; // Currently, no possibility to query for clients.. - std::map> servers_topics; // Can be discovered via 'ServiceDiscovery' - std::map> topic_clients; // Currently, no possibility to query for clients.. - std::map> topic_servers; // Can be discovered, but missing associated node + // Currently, no possibility to query for clients.. + std::map> clients_topics; + // Can be discovered via 'ServiceDiscovery' + std::map> servers_topics; + // Currently, no possibility to query for clients.. + std::map> topic_clients; + // Can be discovered, but missing associated node + std::map> topic_servers; std::vector available_servers; @@ -167,7 +171,8 @@ std::map get_service_names_and_types() std::string(server.getEventIDString().c_str())); names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type); - /// @todo There is no API to find out which 'ServiceDescription' is offered by which node, for now we use 'NodeFoo'.. + /// @todo There is no API to find out which 'ServiceDescription' is offered by which node, + /// for now we use 'NodeFoo'.. servers_topics[std::string("NodeFoo")].push_back( std::get<0>( name_and_type)); diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index df09096..d7c672f 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -170,10 +170,10 @@ rmw_take_request( &iceoryx_server_abstraction->type_supports_, ros_request); } - request_header->source_timestamp = 0; // Unsupported until needed + request_header->source_timestamp = 0; // Unsupported until needed ret = rcutils_system_time_now(&request_header->received_timestamp); request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); - request_header->request_id.writer_guid[0] = 42; /// @todo + request_header->request_id.writer_guid[0] = 42; /// @todo // Hold the loaned request till we send the response in 'rmw_send_response' iceoryx_server_abstraction->request_payload_ = user_payload; diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 162ed46..f3e3a95 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -68,10 +68,12 @@ rmw_take_response( iceoryx_response_payload); /// @todo check writer guid? request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); - request_header->source_timestamp = 0; // Unsupported until needed + request_header->source_timestamp = 0; // Unsupported until needed ret = rcutils_system_time_now(&request_header->received_timestamp); - if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { + if (iceoryx_response_header->getSequenceId() == + iceoryx_client_abstraction->sequence_id_ - 1) + { user_payload = iceoryx_response_payload; chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); ret = RMW_RET_OK; @@ -139,9 +141,10 @@ rmw_send_response( auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload( iceoryx_server_abstraction->request_payload_); - /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, we need to compare - /// the user-provided sequence id with the one from the 'iceoryx_request_header' - //iceoryx_request_header->setSequenceId(request_header->sequence_number); + /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, + /// we need to compare the user-provided sequence id with the one from the + /// 'iceoryx_request_header' + // iceoryx_request_header->setSequenceId(request_header->sequence_number); iceoryx_server->loan( iceoryx_request_header, iceoryx_server_abstraction->response_size_, @@ -158,7 +161,8 @@ rmw_send_response( &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } - /// @todo Why are the sleep before and after 'send()' needed? rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. + /// @todo Why are the sleeps before and after 'send()' needed? + /// rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. std::this_thread::sleep_for(std::chrono::milliseconds(100)); iceoryx_server->send(responsePayload).and_then( [&] { diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index fb1ae7f..f5d0dd7 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -20,7 +20,7 @@ #include "./types/iceoryx_client.hpp" -//#include "iceoryx_posh/runtime/service_discovery.hpp" +// #include "iceoryx_posh/runtime/service_discovery.hpp" extern "C" { From 18ab525934c3299dde9b2aa5b1e5a9c5642c78ed Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 5 Jan 2023 18:01:03 +0100 Subject: [PATCH 21/27] Update Ubuntu version to be able to build for humble (#76) Signed-off-by: Simon Hoinkis --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 619cf16..c850f5e 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -12,7 +12,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-20.04] + os: [ubuntu-22.04] steps: - uses: actions/checkout@v2 - name: Setup ROS From 8b67b384cad2c4f58722d602a0f50535f4b74b3b Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Mon, 9 Jan 2023 09:55:39 +0100 Subject: [PATCH 22/27] Check request pointer for 'nullptr' (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 4 ---- rmw_iceoryx_cpp/src/rmw_response.cpp | 11 +++++++---- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 2 +- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index d7c672f..5d69784 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -82,7 +82,6 @@ rmw_send_request( } iceoryx_client->send(requestPayload).and_then( [&] { - std::cout << "Client sent request!" << std::endl; ret = RMW_RET_OK; }).or_else( [&](auto &) { @@ -151,7 +150,6 @@ rmw_take_request( }) .or_else( [&](iox::popo::ServerRequestResult & error) { - std::cout << "Could not take request! Error: " << error << std::endl; *taken = false; RMW_SET_ERROR_MSG("rmw_take_request error!"); ret = RMW_RET_ERROR; @@ -179,8 +177,6 @@ rmw_take_request( iceoryx_server_abstraction->request_payload_ = user_payload; *taken = true; - std::cout << "Server took request!" << std::endl; - return ret; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index f3e3a95..318f008 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -105,8 +105,6 @@ rmw_take_response( } iceoryx_client->releaseResponse(user_payload); *taken = true; - std::cout << "Client took response!" << std::endl; - return ret; } @@ -139,6 +137,12 @@ rmw_send_response( rmw_ret_t ret = RMW_RET_ERROR; + if (!iceoryx_server_abstraction->request_payload_) { + 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_); /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, @@ -166,7 +170,6 @@ rmw_send_response( std::this_thread::sleep_for(std::chrono::milliseconds(100)); iceoryx_server->send(responsePayload).and_then( [&] { - std::cout << "Server sent response!" << std::endl; ret = RMW_RET_OK; }).or_else( [&](auto &) { @@ -177,13 +180,13 @@ rmw_send_response( }) .or_else( [&](auto & error) { - std::cout << "Could not allocate Response! Error: " << error << std::endl; RMW_SET_ERROR_MSG("rmw_send_response loan error!"); ret = RMW_RET_ERROR; }); // Release the hold request iceoryx_server->releaseRequest(iceoryx_server_abstraction->request_payload_); + iceoryx_server_abstraction->request_payload_ = nullptr; return ret; } diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index c6c57e7..515600a 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -39,7 +39,7 @@ struct IceoryxServer size_t response_size_; /// @todo Is there a way to get the aligment for a complete type via the rosidl? uint32_t response_alignment_{8}; - const void * request_payload_; + const void * request_payload_{nullptr}; }; #endif // TYPES__ICEORYX_SERVER_HPP_ From bb005ce3f3690dc58295dcf46cd10abd3a26c20f Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 11 Jan 2023 22:05:16 +0100 Subject: [PATCH 23/27] Remove out-commented code and implement setting the GUID (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 16 +++++++++++- rmw_iceoryx_cpp/src/rmw_response.cpp | 26 +++++++++++++------ .../src/rmw_service_server_is_available.cpp | 11 -------- 3 files changed, 33 insertions(+), 20 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 5d69784..4758e76 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -170,8 +170,22 @@ rmw_take_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(); - request_header->request_id.writer_guid[0] = 42; /// @todo + + 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; diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 318f008..0b52fc9 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -66,16 +66,30 @@ rmw_take_response( [&](const void * iceoryx_response_payload) { auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload( iceoryx_response_payload); - /// @todo check writer guid? - request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); - request_header->source_timestamp = 0; // Unsupported until needed - ret = rcutils_system_time_now(&request_header->received_timestamp); if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1) { user_payload = iceoryx_response_payload; chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + + auto typed_guid = chunk_header->originId(); + 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); + request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); + request_header->source_timestamp = 0; // Unsupported until needed + ret = rcutils_system_time_now(&request_header->received_timestamp); + if (ret != RMW_RET_OK) { + return; + } ret = RMW_RET_OK; } else { RMW_SET_ERROR_MSG("Got response with outdated sequence number!"); @@ -145,10 +159,6 @@ rmw_send_response( auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload( iceoryx_server_abstraction->request_payload_); - /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so, - /// we need to compare the user-provided sequence id with the one from the - /// 'iceoryx_request_header' - // iceoryx_request_header->setSequenceId(request_header->sequence_number); iceoryx_server->loan( iceoryx_request_header, iceoryx_server_abstraction->response_size_, diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp index f5d0dd7..d3d32df 100644 --- a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp @@ -20,8 +20,6 @@ #include "./types/iceoryx_client.hpp" -// #include "iceoryx_posh/runtime/service_discovery.hpp" - extern "C" { rmw_ret_t @@ -56,15 +54,6 @@ rmw_service_server_is_available( *is_available = true; } - /// @todo Better to go through service discovery? - // iox::runtime::ServiceDiscovery serviceDiscovery; - // auto& searchItem = iceoryx_client->getServiceDescription(); - // serviceDiscovery.findService( searchItem.getServiceIDString(), - // searchItem.getInstanceIDString(), - // searchItem.getEventIDString(), - // [&](auto&){ *is_available = true; }, - // iox::popo::MessagingPattern::REQ_RES); - ret = RMW_RET_OK; return ret; } From b1865097892612e077d93eafbf5b290f8bb87478 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 11 Jan 2023 22:51:49 +0100 Subject: [PATCH 24/27] 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; }); From 6546111d8eba6aacf3954c813f2a895914c79c0e Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 12 Jan 2023 10:51:59 +0100 Subject: [PATCH 25/27] Use default alignment when loaning for request/response (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_request.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_response.cpp | 2 +- rmw_iceoryx_cpp/src/types/iceoryx_client.hpp | 2 -- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 2 -- 4 files changed, 2 insertions(+), 6 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 573fae0..01032e0 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -61,7 +61,7 @@ rmw_send_request( iceoryx_client->loan( iceoryx_client_abstraction->request_size_, - iceoryx_client_abstraction->request_alignment_) + iox::CHUNK_DEFAULT_USER_PAYLOAD_ALIGNMENT) .and_then( [&](void * requestPayload) { auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 52935a8..3ad89aa 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -158,7 +158,7 @@ rmw_send_response( iceoryx_server->loan( iceoryx_request_header, iceoryx_server_abstraction->response_size_, - iceoryx_server_abstraction->response_alignment_) + iox::CHUNK_DEFAULT_USER_PAYLOAD_ALIGNMENT) .and_then( [&](void * responsePayload) { if (iceoryx_server_abstraction->is_fixed_size_) { diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp index 703b622..517cc20 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp @@ -37,8 +37,6 @@ struct IceoryxClient iox::popo::UntypedClient * const iceoryx_client_; bool is_fixed_size_; size_t request_size_; - /// @todo How can the full type be aquired via rosidl to do an 'alignof()'? - uint32_t request_alignment_{8}; int64_t sequence_id_; }; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index 515600a..d576601 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -37,8 +37,6 @@ struct IceoryxServer iox::popo::UntypedServer * const iceoryx_server_; bool is_fixed_size_; size_t response_size_; - /// @todo Is there a way to get the aligment for a complete type via the rosidl? - uint32_t response_alignment_{8}; const void * request_payload_{nullptr}; }; From cbb145d41608238ba7953761f00bf2e40e1be26b Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Tue, 24 Jan 2023 16:37:00 +0100 Subject: [PATCH 26/27] Remove sleeps in 'rmw_send_response' as fix will be in 'v2.0.3' release (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/rmw_client.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_response.cpp | 4 ---- rmw_iceoryx_cpp/src/rmw_service.cpp | 2 +- rmw_iceoryx_cpp/src/rmw_wait.cpp | 8 ++++---- rmw_iceoryx_cpp/src/types/iceoryx_server.hpp | 4 ++++ 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp index 9c367af..064310c 100644 --- a/rmw_iceoryx_cpp/src/rmw_client.cpp +++ b/rmw_iceoryx_cpp/src/rmw_client.cpp @@ -93,7 +93,7 @@ rmw_create_client( iceoryx_client, iceoryx_client, cleanupAfterError(), iox::popo::UntypedClient, service_description, iox::popo::ClientOptions{ - 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + qos_policies->depth, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); if (returnOnError) { return nullptr; } diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index 3ad89aa..ae817fc 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -171,9 +171,6 @@ rmw_send_response( &iceoryx_server_abstraction->type_supports_, payload_vector); memcpy(responsePayload, payload_vector.data(), payload_vector.size()); } - /// @todo Why are the sleeps before and after 'send()' needed? - /// rmw_cyclonedds and rmw_fastrtps seem to do something similar in 'rmw_send_response'.. - std::this_thread::sleep_for(std::chrono::milliseconds(100)); iceoryx_server->send(responsePayload).and_then( [&] { ret = RMW_RET_OK; @@ -182,7 +179,6 @@ rmw_send_response( RMW_SET_ERROR_MSG("rmw_send_response send error!"); ret = RMW_RET_ERROR; }); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); }) .or_else( [&](auto &) { diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index 14c6f24..c06069a 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -93,7 +93,7 @@ rmw_create_service( iceoryx_server, iceoryx_server, cleanupAfterError(), iox::popo::UntypedServer, service_description, iox::popo::ServerOptions{ - 1U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + qos_policies->depth, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); if (returnOnError) { return nullptr; } diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp index d532120..ac6a7a6 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait.cpp @@ -114,16 +114,16 @@ rmw_wait( goto after_wait; } + // The triggered entities are checked below individually, this could be refactored by looping + // over the vector returned by 'wait()' and using 'vectorEntry->doesOriginateFrom()' if (!wait_timeout) { - /// @todo Check triggered subscribers in vector? Is that relevant for rmw? - auto notificationVector = waitset->wait(); + waitset->wait(); } else { auto sec = iox::units::Duration::fromSeconds(wait_timeout->sec); auto nsec = iox::units::Duration::fromNanoseconds(wait_timeout->nsec); auto timeout = sec + nsec; - /// @todo Check triggered subscribers in vector? Is that relevant for rmw? - auto notificationVector = waitset->timedWait(iox::units::Duration(timeout)); + waitset->timedWait(iox::units::Duration(timeout)); } after_wait: diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp index d576601..d44a34d 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp @@ -37,6 +37,10 @@ struct IceoryxServer 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}; }; From 78201e10676f6bd272dace8b9047b9a7d62a6bed Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Wed, 25 Jan 2023 12:19:51 +0100 Subject: [PATCH 27/27] Update version number in package.xml (#76) Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_iceoryx_cpp/package.xml b/rmw_iceoryx_cpp/package.xml index e4f256e..065a614 100644 --- a/rmw_iceoryx_cpp/package.xml +++ b/rmw_iceoryx_cpp/package.xml @@ -2,7 +2,7 @@ rmw_iceoryx_cpp - 1.0.1 + 2.0.3 rmw implementation for Bosch's zero copy middleware iceoryx Karsten Knese Apache License 2.0