From 360fb1c0cc156c9fa8dbf99440ded3d280a97992 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 19:39:56 +0200 Subject: [PATCH 01/60] Revert "Unique network flows (#502)" This reverts commit ae4485e080132f8acac9b2a7e1cc7d5042e7d6d0. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/CMakeLists.txt | 1 - rmw_fastrtps_cpp/src/publisher.cpp | 7 - .../src/rmw_get_endpoint_network_flow.cpp | 46 ---- rmw_fastrtps_cpp/src/subscription.cpp | 36 +--- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 1 - rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 12 +- .../src/rmw_get_endpoint_network_flow.cpp | 46 ---- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 36 +--- rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 - .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 15 -- .../src/rmw_get_endpoint_network_flow.cpp | 204 ------------------ 11 files changed, 3 insertions(+), 402 deletions(-) delete mode 100644 rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp delete mode 100644 rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp delete mode 100644 rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 35ef66d0e..3b376b9cc 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -89,7 +89,6 @@ add_library(rmw_fastrtps_cpp src/serialization_format.cpp src/subscription.cpp src/type_support_common.cpp - src/rmw_get_endpoint_network_flow.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 41df66259..845cd825d 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -81,13 +81,6 @@ rmw_fastrtps_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); - if (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED == - publisher_options->require_unique_network_flow_endpoints) - { - RMW_SET_ERROR_MSG("Unique network flow endpoints not supported on publishers"); - return nullptr; - } - Participant * participant = participant_info->participant; RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); diff --git a/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp deleted file mode 100644 index 371c43d88..000000000 --- a/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// 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. - -#include "rmw/get_network_flow_endpoints.h" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" -#include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -extern "C" -{ -rmw_ret_t -rmw_publisher_get_network_flow_endpoints( - const rmw_publisher_t * publisher, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - return rmw_fastrtps_shared_cpp::__rmw_publisher_get_network_flow_endpoints( - publisher, - allocator, - network_flow_endpoint_array); -} - -rmw_ret_t -rmw_subscription_get_network_flow_endpoints( - const rmw_subscription_t * subscription, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - return rmw_fastrtps_shared_cpp::__rmw_subscription_get_network_flow_endpoints( - subscription, - allocator, - network_flow_endpoint_array); -} -} // extern "C" diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 30cbf6fb0..c8f69488e 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -46,7 +46,6 @@ using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; -using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; namespace rmw_fastrtps_cpp @@ -170,40 +169,7 @@ create_subscription( } } - eprosima::fastrtps::SubscriberAttributes originalParam = subscriberParam; - switch (subscription_options->require_unique_network_flow_endpoints) { - default: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: - // Unique network flow endpoints not required. We leave the decission to the XML profile. - break; - - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: - // Ensure we request unique network flow endpoints - if (nullptr == - PropertyPolicyHelper::find_property( - subscriberParam.properties, - "fastdds.unique_network_flows")) - { - subscriberParam.properties.properties().emplace_back("fastdds.unique_network_flows", ""); - } - break; - } - - info->subscriber_ = Domain::createSubscriber( - participant, - subscriberParam, - info->listener_); - if (!info->subscriber_ && - (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == - subscription_options->require_unique_network_flow_endpoints)) - { - info->subscriber_ = Domain::createSubscriber( - participant, - originalParam, - info->listener_); - } + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); if (!info->subscriber_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index f91079b4d..ec43fe20f 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -94,7 +94,6 @@ add_library(rmw_fastrtps_dynamic_cpp src/type_support_common.cpp src/type_support_proxy.cpp src/type_support_registry.cpp - src/rmw_get_endpoint_network_flow.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 899b05a9e..f6cfde1a1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -81,13 +81,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); - if (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED == - publisher_options->require_unique_network_flow_endpoints) - { - RMW_SET_ERROR_MSG("Unique network flow endpoints not supported on publishers"); - return nullptr; - } - Participant * participant = participant_info->participant; RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); @@ -200,10 +193,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - info->publisher_ = Domain::createPublisher( - participant, - publisherParam, - info->listener_); + info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); if (!info->publisher_) { RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp deleted file mode 100644 index 7740818e7..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// 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. - -#include "rmw/error_handling.h" -#include "rmw/rmw.h" -#include "rmw/types.h" -#include "rmw/get_network_flow_endpoints.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -extern "C" -{ -rmw_ret_t -rmw_publisher_get_network_flow_endpoints( - const rmw_publisher_t * publisher, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - return rmw_fastrtps_shared_cpp::__rmw_publisher_get_network_flow_endpoints( - publisher, - allocator, - network_flow_endpoint_array); -} - -rmw_ret_t -rmw_subscription_get_network_flow_endpoints( - const rmw_subscription_t * subscription, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - return rmw_fastrtps_shared_cpp::__rmw_subscription_get_network_flow_endpoints( - subscription, - allocator, - network_flow_endpoint_array); -} -} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 41091471e..156fc6ece 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -44,7 +44,6 @@ using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; -using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; using TopicDataType = eprosima::fastrtps::TopicDataType; using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; @@ -181,40 +180,7 @@ create_subscription( return nullptr; } - eprosima::fastrtps::SubscriberAttributes originalParam = subscriberParam; - switch (subscription_options->require_unique_network_flow_endpoints) { - default: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: - // Unique network flow endpoints not required. We leave the decission to the XML profile. - break; - - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: - // Ensure we request unique network flow endpoints - if (nullptr == - PropertyPolicyHelper::find_property( - subscriberParam.properties, - "fastdds.unique_network_flows")) - { - subscriberParam.properties.properties().emplace_back("fastdds.unique_network_flows", ""); - } - break; - } - - info->subscriber_ = Domain::createSubscriber( - participant, - subscriberParam, - info->listener_); - if (!info->subscriber_ && - (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == - subscription_options->require_unique_network_flow_endpoints)) - { - info->subscriber_ = Domain::createSubscriber( - participant, - originalParam, - info->listener_); - } + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); if (!info->subscriber_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); return nullptr; diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 5bf283fc9..71c39d2e6 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -86,7 +86,6 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_wait_set.cpp src/subscription.cpp src/TypeSupport_impl.cpp - src/rmw_get_endpoint_network_flow.cpp ) target_include_directories(rmw_fastrtps_shared_cpp PUBLIC diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 082e5629a..1a9a0148b 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -23,7 +23,6 @@ #include "rmw/topic_endpoint_info_array.h" #include "rmw/types.h" #include "rmw/names_and_types.h" -#include "rmw/network_flow_endpoint_array.h" namespace rmw_fastrtps_shared_cpp { @@ -402,20 +401,6 @@ __rmw_qos_profile_check_compatible( char * reason, size_t reason_size); -RMW_FASTRTPS_SHARED_CPP_PUBLIC -rmw_ret_t -__rmw_publisher_get_network_flow_endpoints( - const rmw_publisher_t * publisher, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array); - -RMW_FASTRTPS_SHARED_CPP_PUBLIC -rmw_ret_t -__rmw_subscription_get_network_flow_endpoints( - const rmw_subscription_t * subscription, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array); - } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp deleted file mode 100644 index f227eaf7d..000000000 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// 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. - -#include - -#include "rmw/get_network_flow_endpoints.h" -#include "rmw/error_handling.h" -#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "fastrtps/utils/IPLocator.h" - - -namespace rmw_fastrtps_shared_cpp -{ - -using Locator_t = eprosima::fastrtps::rtps::Locator_t; -using LocatorList_t = eprosima::fastrtps::rtps::LocatorList_t; -using IPLocator = eprosima::fastrtps::rtps::IPLocator; - -rmw_ret_t fill_network_flow_endpoint(rmw_network_flow_endpoint_t *, const Locator_t &); - -rmw_ret_t -__rmw_publisher_get_network_flow_endpoints( - const rmw_publisher_t * publisher, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - rmw_ret_t res = RMW_RET_OK; - - // Retrieve the sender locators - CustomPublisherInfo * data = - static_cast(publisher->data); - LocatorList_t locators; - data->publisher_->get_sending_locators(locators); - - if (locators.empty()) { - return res; - } - - // It must be a non-initialized array - if (RMW_RET_OK != - (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) - { - return res; - } - - // Allocate - if (RMW_RET_OK != - (res = rmw_network_flow_endpoint_array_init( - network_flow_endpoint_array, - locators.size(), - allocator))) - { - return res; - } - - // Translate the locators, on error reset the array - try { - auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; - for (const Locator_t & loc : locators) { - if (RMW_RET_OK != - (res = fill_network_flow_endpoint(rmw_nf++, loc))) - { - throw res; - } - } - } catch (rmw_ret_t) { - // clear the array - rmw_network_flow_endpoint_array_fini( - network_flow_endpoint_array); - - // set error message - RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); - } - - return res; -} - -rmw_ret_t -__rmw_subscription_get_network_flow_endpoints( - const rmw_subscription_t * subscription, - rcutils_allocator_t * allocator, - rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) -{ - rmw_ret_t res = RMW_RET_OK; - - // Retrieve the listener locators - CustomSubscriberInfo * data = - static_cast(subscription->data); - LocatorList_t locators; - data->subscriber_->get_listening_locators(locators); - - if (locators.empty()) { - return res; - } - - // It must be a non-initialized array - if (RMW_RET_OK != - (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) - { - return res; - } - - // Allocate - if (RMW_RET_OK != - (res = rmw_network_flow_endpoint_array_init( - network_flow_endpoint_array, - locators.size(), - allocator))) - { - return res; - } - - // Translate the locators, on error reset the array - try { - auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; - for (const Locator_t & loc : locators) { - if (RMW_RET_OK != - (res = fill_network_flow_endpoint(rmw_nf++, loc))) - { - throw res; - } - } - } catch (rmw_ret_t) { - // clear the array - rmw_network_flow_endpoint_array_fini( - network_flow_endpoint_array); - - // set error message - RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); - } - - return res; -} - -// Ancillary translation methods -rmw_transport_protocol_t -get_transport_protocol(const Locator_t & loc) -{ - if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_UDPv6)) { - return RMW_TRANSPORT_PROTOCOL_UDP; - } else if (loc.kind & (LOCATOR_KIND_TCPv4 | LOCATOR_KIND_TCPv6)) { - return RMW_TRANSPORT_PROTOCOL_TCP; - } - - return RMW_TRANSPORT_PROTOCOL_UNKNOWN; -} - -rmw_internet_protocol_t -get_internet_protocol(const Locator_t & loc) -{ - if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_TCPv4)) { - return RMW_INTERNET_PROTOCOL_IPV4; - } else if (loc.kind & (LOCATOR_KIND_TCPv6 | LOCATOR_KIND_UDPv6)) { - return RMW_INTERNET_PROTOCOL_IPV6; - } - - return RMW_INTERNET_PROTOCOL_UNKNOWN; -} - -rmw_ret_t -fill_network_flow_endpoint( - rmw_network_flow_endpoint_t * network_flow_endpoint, - const Locator_t & locator) -{ - rmw_ret_t res = RMW_RET_OK; - - // Translate transport protocol - network_flow_endpoint->transport_protocol = get_transport_protocol(locator); - - // Translate internet protocol - network_flow_endpoint->internet_protocol = get_internet_protocol(locator); - - // Set the port - network_flow_endpoint->transport_port = IPLocator::getPhysicalPort(locator); - - // Set the address - std::string address = IPLocator::ip_to_string(locator); - - if (RMW_RET_OK != - (res = rmw_network_flow_endpoint_set_internet_address( - network_flow_endpoint, - address.c_str(), - address.length()))) - { - return res; - } - - return res; -} - -} // namespace rmw_fastrtps_shared_cpp From 7e8640517f1db2a30a99e256fdd66a7dc40edf71 Mon Sep 17 00:00:00 2001 From: jparisu Date: Thu, 28 Jan 2021 16:26:37 +0100 Subject: [PATCH 02/60] rebase first steps Signed-off-by: jparisu Signed-off-by: Miguel Company --- .../rmw_fastrtps_cpp/ServiceTypeSupport.hpp | 4 +- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 10 +- .../include/rmw_fastrtps_cpp/get_client.hpp | 16 +- .../rmw_fastrtps_cpp/get_participant.hpp | 8 +- .../rmw_fastrtps_cpp/get_publisher.hpp | 8 +- .../include/rmw_fastrtps_cpp/get_service.hpp | 16 +- .../rmw_fastrtps_cpp/get_subscriber.hpp | 8 +- .../include/rmw_fastrtps_cpp/publisher.hpp | 3 +- .../include/rmw_fastrtps_cpp/subscription.hpp | 3 +- rmw_fastrtps_cpp/src/get_client.cpp | 7 +- rmw_fastrtps_cpp/src/get_participant.cpp | 2 +- rmw_fastrtps_cpp/src/get_publisher.cpp | 2 +- rmw_fastrtps_cpp/src/get_service.cpp | 4 +- rmw_fastrtps_cpp/src/get_subscriber.cpp | 2 +- .../src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 216 ++++++++++++------ rmw_fastrtps_cpp/src/subscription.cpp | 206 ++++++++++++----- rmw_fastrtps_cpp/src/type_support_common.hpp | 9 +- .../test/test_get_native_entities.cpp | 2 +- rmw_fastrtps_cpp/test/test_logging.cpp | 2 +- .../MessageTypeSupport.hpp | 4 +- .../MessageTypeSupport_impl.hpp | 4 +- .../ServiceTypeSupport.hpp | 4 +- .../ServiceTypeSupport_impl.hpp | 4 +- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 10 +- .../TypeSupport_impl.hpp | 6 +- .../rmw_fastrtps_dynamic_cpp/get_client.hpp | 17 +- .../get_participant.hpp | 9 +- .../get_publisher.hpp | 9 +- .../rmw_fastrtps_dynamic_cpp/get_service.hpp | 17 +- .../get_subscriber.hpp | 5 +- .../rmw_fastrtps_dynamic_cpp/publisher.hpp | 2 +- .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 2 +- rmw_fastrtps_dynamic_cpp/src/get_client.cpp | 4 +- .../src/get_participant.cpp | 6 +- .../src/get_publisher.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/get_service.cpp | 4 +- .../src/get_subscriber.cpp | 2 +- .../src/init_rmw_context_impl.cpp | 2 +- .../src/type_support_common.cpp | 14 +- .../src/type_support_common.hpp | 11 +- .../test/test_get_native_entities.cpp | 2 +- .../test/test_logging.cpp | 2 +- rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 29 +-- .../create_rmw_gid.hpp | 2 +- .../custom_client_info.hpp | 25 +- .../custom_event_info.hpp | 7 - .../custom_participant_info.hpp | 34 +-- .../custom_publisher_info.hpp | 8 +- .../custom_service_info.hpp | 23 +- .../custom_subscriber_info.hpp | 7 +- .../rmw_fastrtps_shared_cpp/guid_utils.hpp | 2 +- .../rmw_fastrtps_shared_cpp/participant.hpp | 2 + .../include/rmw_fastrtps_shared_cpp/qos.hpp | 60 +++-- .../rmw_security_logging.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 30 +++ .../src/TypeSupport_impl.cpp | 4 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 133 +++++++---- rmw_fastrtps_shared_cpp/src/qos.cpp | 118 +++++++--- .../src/rmw_compare_gids_equal.cpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_logging.cpp | 6 +- .../src/rmw_node_names.cpp | 4 - rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 9 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 2 - .../src/rmw_service_server_is_available.cpp | 4 +- .../src/rmw_subscription.cpp | 9 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 45 ++++ rmw_fastrtps_shared_cpp/test/test_logging.cpp | 2 +- 69 files changed, 782 insertions(+), 460 deletions(-) create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp create mode 100644 rmw_fastrtps_shared_cpp/src/utils.cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp index 49ec62900..fb38863c1 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp @@ -15,8 +15,8 @@ #ifndef RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 5c9964a5a..b465a5f68 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -15,13 +15,11 @@ #ifndef RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ -#include -#include +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" -#include - -#include -#include +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" #include #include diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp index 6d6c1377a..ab429826d 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp @@ -15,34 +15,34 @@ #ifndef RMW_FASTRTPS_CPP__GET_CLIENT_HPP_ #define RMW_FASTRTPS_CPP__GET_CLIENT_HPP_ -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS publisher handle for the request. +/// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or * when the client handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_request_publisher(rmw_client_t * client); -/// Return a native FastRTPS subscriber handle for the response. +/// Return a native Fast DDS DataReader handle for the response. /** * The function returns `NULL` when either the client handle is `NULL` or * when the client handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_response_subscriber(rmw_client_t * client); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp index 6b287ce98..2de0a844b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp @@ -15,22 +15,22 @@ #ifndef RMW_FASTRTPS_CPP__GET_PARTICIPANT_HPP_ #define RMW_FASTRTPS_CPP__GET_PARTICIPANT_HPP_ -#include "fastrtps/participant/Participant.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS participant handle. +/// Return a native Fast DDS DomainParticipant handle. /** * The function returns `NULL` when either the node handle is `NULL` or when the * node handle is from a different rmw implementation. * - * \return native FastRTPS participant handle if successful, otherwise `NULL` + * \return native Fast DDS DomainParticipant handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Participant * +eprosima::fastdds::dds::DomainParticipant * get_participant(rmw_node_t * node); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp index 0f8c94c98..cee30beae 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp @@ -15,22 +15,22 @@ #ifndef RMW_FASTRTPS_CPP__GET_PUBLISHER_HPP_ #define RMW_FASTRTPS_CPP__GET_PUBLISHER_HPP_ -#include "fastrtps/publisher/Publisher.h" +#include "fastdds/dds/publisher/DataWriter.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS publisher handle. +/// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or * when the publisher handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_publisher(rmw_publisher_t * publisher); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp index 645da26e4..26360875a 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp @@ -15,34 +15,34 @@ #ifndef RMW_FASTRTPS_CPP__GET_SERVICE_HPP_ #define RMW_FASTRTPS_CPP__GET_SERVICE_HPP_ -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS subscriber handle for the request. +/// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or * when the service handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_request_subscriber(rmw_service_t * service); -/// Return a native FastRTPS publisher handle for the response. +/// Return a native Fast DDS DataWriter handle for the response. /** * The function returns `NULL` when either the service handle is `NULL` or * when the service handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_response_publisher(rmw_service_t * service); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp index 403a176f4..c2ffac1ff 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -15,22 +15,22 @@ #ifndef RMW_FASTRTPS_CPP__GET_SUBSCRIBER_HPP_ #define RMW_FASTRTPS_CPP__GET_SUBSCRIBER_HPP_ -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/subscriber/DataReader.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS subscriber handle. +/// Return a native Fast DDS subscriber handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or * when the subscription handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS subscriber handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_subscriber(rmw_subscription_t * subscription); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp index e59b3ccd1..dae92d2af 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -28,9 +28,8 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, + bool keyed, // unused bool create_publisher_listener); - } // namespace rmw_fastrtps_cpp #endif // RMW_FASTRTPS_CPP__PUBLISHER_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp index a35d03f15..23c2b09b7 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -30,9 +30,8 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, + bool keyed, // unused bool create_subscription_listener); - } // namespace rmw_fastrtps_cpp #endif // RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_cpp/src/get_client.cpp b/rmw_fastrtps_cpp/src/get_client.cpp index b76cd9f02..c4076b2f6 100644 --- a/rmw_fastrtps_cpp/src/get_client.cpp +++ b/rmw_fastrtps_cpp/src/get_client.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" + #include "rmw_fastrtps_cpp/get_client.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" @@ -20,7 +23,7 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_request_publisher(rmw_client_t * client) { if (!client) { @@ -33,7 +36,7 @@ get_request_publisher(rmw_client_t * client) return impl->request_publisher_; } -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_response_subscriber(rmw_client_t * client) { if (!client) { diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index 120086460..de4542088 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -22,7 +22,7 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Participant * +eprosima::fastdds::dds::DomainParticipant * get_participant(rmw_node_t * node) { if (!node) { diff --git a/rmw_fastrtps_cpp/src/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index bac263c6c..18af0ef77 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_publisher(rmw_publisher_t * publisher) { if (!publisher) { diff --git a/rmw_fastrtps_cpp/src/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index a7245a83b..51b88341b 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_request_subscriber(rmw_service_t * service) { if (!service) { @@ -33,7 +33,7 @@ get_request_subscriber(rmw_service_t * service) return impl->request_subscriber_; } -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_response_publisher(rmw_service_t * service) { if (!service) { diff --git a/rmw_fastrtps_cpp/src/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 9eb7eae8b..5aae463b7 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_subscriber(rmw_subscription_t * subscription) { if (!subscription) { diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index f32f783ed..fa3420d19 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -152,7 +152,7 @@ init_context_impl(rmw_context_t * context) } common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + eprosima_fastrtps_identifier, participant_info->participant->guid()); common_context->pub = publisher.get(); common_context->sub = subscription.get(); common_context->graph_guard_condition = graph_guard_condition.get(); diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 845cd825d..b0be5129f 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -15,9 +15,12 @@ #include -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/xmlparser/XMLProfileManager.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -42,11 +45,6 @@ #include "type_support_common.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; - rmw_publisher_t * rmw_fastrtps_cpp::create_publisher( const CustomParticipantInfo * participant_info, @@ -54,9 +52,11 @@ rmw_fastrtps_cpp::create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, + bool keyed, // always false bool create_publisher_listener) { + ///// + // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); @@ -81,9 +81,16 @@ rmw_fastrtps_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); - Participant * participant = participant_info->participant; - RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant; + RMW_CHECK_ARGUMENT_FOR_NULL(domainParticipant, nullptr); + + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher; + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, nullptr); + ///// + // Get Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -104,98 +111,164 @@ rmw_fastrtps_cpp::create_publisher( } } + ///// + // Create the RMW Publisher struct (info) CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; - - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // publisher which profile name matches with topic_name. If such profile does not exist, - // then use the default attributes. - eprosima::fastrtps::PublisherAttributes publisherParam; - Domain::getDefaultPublisherAttributes(publisherParam); // Loads the XML file if not loaded - XMLProfileManager::fillPublisherAttributes(topic_name, publisherParam, false); info = new (std::nothrow) CustomPublisherInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate CustomPublisherInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->type_support_) { - _unregister_type(participant, info->type_support_); - } - delete info->listener_; + [info]() { delete info; }); + info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support->data; + ///// + // Create the Type Support struct auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) - { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport"); - return nullptr; - } - _register_type(participant, info->type_support_); - } - if (!participant_info->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; - } + // This struct is not used in the future, as there is no need to unregister the types + info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); + return nullptr; } + auto cleanup_type_support = rcpputils::make_scope_exit( + [info]() { + delete info->type_support_; + }); - publisherParam.topic.topicKind = - keyed ? eprosima::fastrtps::rtps::WITH_KEY : eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); + info->type_support_->createData(); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { return nullptr; } + ///// + // Create Listener info->listener_ = nullptr; if (create_publisher_listener) { info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - return nullptr; + } + + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + return nullptr; + } + + auto cleanup_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } + + // Create Topic name + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + // Create topic + // it looks for the topic in case it is already created + // info->topic_ = domainParticipant->find_topic(topic_name_mangled, 0); // TODO eprosima + if (info->topic_ == nullptr){ + // the topic does not exist yet it creates a new one + info->topic_ = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); + } + + if (info->topic_ == nullptr) { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; + } + + // lambda to unregister topic + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, info]() { + domainParticipant->delete_topic(info->topic_); + info->topic_ = nullptr; + }); + + ///// + // Create DataWriter + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datawriter which profile name matches with topic_name. If such profile does not exist, + // then use the default QoS. + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } + + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + // Get QoS from ROS + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + return nullptr; } - info->publisher_ = Domain::createPublisher( - participant, - publisherParam, + // Creates DataWriter (with publisher name to not change name policy) + info->publisher_ = publisher->create_datawriter( + info->topic_, + dataWriterQos, info->listener_); + if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } - auto cleanup_publisher = rcpputils::make_scope_exit( - [info]() { - if (!Domain::removePublisher(info->publisher_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove publisher after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->publisher_); }); + ///// + // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->getGuid()); + eprosima_fastrtps_identifier, info->publisher_->guid()); + + ///// + // Allocate publisher + rmw_publisher_t * rmw_publisher = nullptr; rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { @@ -221,7 +294,10 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher->options = *publisher_options; cleanup_rmw_publisher.cancel(); - cleanup_publisher.cancel(); + cleanup_datawriter.cancel(); + cleanup_topic.cancel(); + cleanup_listener.cancel(); + cleanup_type_support.cancel(); cleanup_info.cancel(); return rmw_publisher; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index c8f69488e..b0642647a 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -15,6 +15,14 @@ #include #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDataType.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "rcutils/allocator.h" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -34,20 +42,11 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/xmlparser/XMLProfileManager.h" - #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_cpp/subscription.hpp" #include "type_support_common.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; - namespace rmw_fastrtps_cpp { @@ -61,6 +60,8 @@ create_subscription( bool keyed, bool create_subscription_listener) { + ///// + // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); @@ -84,8 +85,17 @@ create_subscription( } } RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - Participant * participant = participant_info->participant; - RMW_CHECK_FOR_NULL_WITH_MSG(participant, "participant handle is null", return nullptr); + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); + + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + RMW_CHECK_ARGUMENT_FOR_NULL(subscriber, nullptr); + + ///// + // Get Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); @@ -106,86 +116,157 @@ create_subscription( return nullptr; } } - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // subscriber which profile name matches with topic_name. If such profile does not exist, then use - // the default attributes. - eprosima::fastrtps::SubscriberAttributes subscriberParam; - Domain::getDefaultSubscriberAttributes(subscriberParam); // Loads the XML file if not loaded - XMLProfileManager::fillSubscriberAttributes(topic_name, subscriberParam, false); + ///// + // Create the RMW Subscriber struct (info) + CustomSubscriberInfo * info = nullptr; - CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); + info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->type_support_) { - _unregister_type(participant, info->type_support_); - } - delete info->listener_; + [info]() { delete info; }); + info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support->data; + ///// + // Create the Type Support struct auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) - { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport_cpp"); - return nullptr; - } - _register_type(participant, info->type_support_); - } - if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + // This struct is not used in the future, as there is no need to unregister the types + info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; } + auto cleanup_type_support = rcpputils::make_scope_exit( + [info]() { + delete info->type_support_; + }); - subscriberParam.topic.topicKind = - keyed ? eprosima::fastrtps::rtps::WITH_KEY : eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); + info->type_support_->createData(); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { return nullptr; } + ///// + // Create Listener + info->listener_ = nullptr; if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - return nullptr; - } } - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); return nullptr; } - auto cleanup_subscription = rcpputils::make_scope_exit( + + auto cleanup_listener = rcpputils::make_scope_exit( [info]() { - if (!Domain::removeSubscriber(info->subscriber_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove subscriber after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } + delete info->listener_; + }); + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } + + // Create Topic name + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + // Create topic + // it looks for the topic in case it is already created + // info->topic_ = domainParticipant->find_topic(topic_name_mangled, 0); // TODO eprosima + if (info->topic_ == nullptr){ + // the topic does not exist yet it creates a new one + info->topic_ = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); + } + + if (info->topic_ == nullptr) { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + // lambda to unregister topic + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, info]() { + domainParticipant->delete_topic(info->topic_); + info->topic_ = nullptr; + }); + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default QoS. + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); + + + if (!participant_info->leave_middleware_default_qos) { + dataReaderQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + return nullptr; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->subscriber_ = subscriber->create_datareader( + info->topic_, + dataReaderQos, + info->listener_); + + if (!info->subscriber_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->subscriber_); }); + ///// + // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->subscriber_->guid()); + ///// + // Allocate subscription rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { RMW_SET_ERROR_MSG("failed to allocate subscription"); @@ -209,7 +290,10 @@ create_subscription( rmw_subscription->can_loan_messages = false; cleanup_rmw_subscription.cancel(); - cleanup_subscription.cancel(); + cleanup_datareader.cancel(); + cleanup_topic.cancel(); + cleanup_listener.cancel(); + cleanup_type_support.cancel(); cleanup_info.cancel(); return rmw_subscription; } diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 7e3874e60..148c94acc 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -18,8 +18,8 @@ #include #include -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/topic/TopicDataType.hpp" #include "rmw/error_handling.h" @@ -63,10 +63,11 @@ _create_type_name( inline void _register_type( - eprosima::fastrtps::Participant * participant, + eprosima::fastdds::dds::DomainParticipant * participant, rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) { - eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); + // TODO eprosima + // eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); } #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp index 328201d97..78ed3008c 100644 --- a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp index 980888915..8414b903b 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -15,8 +15,8 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp index 607bf86ef..1c7f42d69 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -15,8 +15,8 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include "TypeSupport.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp index 61237fdb9..63e7060c0 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -15,8 +15,8 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include #include diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp index 0c09766fd..0842b168c 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -15,13 +15,11 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ -#include -#include +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" -#include - -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index 52b045c35..892fc1cbb 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -15,9 +15,9 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ -#include -#include -#include +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" +#include "fastcdr/exceptions/Exception.h" #include #include diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp index 9bf08a1f7..c86087b84 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp @@ -15,34 +15,35 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS publisher handle for the request. +/// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or * when the client handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_request_publisher(rmw_client_t * client); -/// Return a native FastRTPS subscriber handle for the response. +/// Return a native Fast DDS DataReader handle for the response. /** * The function returns `NULL` when either the client handle is `NULL` or * when the client handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_response_subscriber(rmw_client_t * client); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp index d32e07fe1..017605269 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp @@ -15,22 +15,23 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ -#include "fastrtps/participant/Participant.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS participant handle. +/// Return a native Fast DDS DomainParticipant handle. /** * The function returns `NULL` when either the node handle is `NULL` or when the * node handle is from a different rmw implementation. * - * \return native FastRTPS participant handle if successful, otherwise `NULL` + * \return native Fast DDS DomainParticipant handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Participant * +eprosima::fastdds::dds::DomainParticipant * get_participant(rmw_node_t * node); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp index 727bb4846..f563c3606 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp @@ -15,22 +15,23 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ -#include "fastrtps/publisher/Publisher.h" +#include "fastdds/dds/publisher/DataWriter.hpp" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS publisher handle. +/// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or * when the publisher handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_publisher(rmw_publisher_t * publisher); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp index 6d9e3d74a..59974dc49 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp @@ -15,34 +15,35 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS subscriber handle for the request. +/// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or * when the service handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_request_subscriber(rmw_service_t * service); -/// Return a native FastRTPS publisher handle for the response. +/// Return a native Fast DDS DataWriter handle for the response. /** * The function returns `NULL` when either the service handle is `NULL` or * when the service handle is from a different rmw implementation. * - * \return native FastRTPS publisher handle if successful, otherwise `NULL` + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_response_publisher(rmw_service_t * service); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp index f6614e7bf..24dfe6acd 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -15,7 +15,8 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/subscriber/DataReader.hpp" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" @@ -30,7 +31,7 @@ namespace rmw_fastrtps_dynamic_cpp * \return native FastRTPS subscriber handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_subscriber(rmw_subscription_t * subscription); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp index 4f98b6c14..8b54e1423 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, + bool keyed, //unused bool create_publisher_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp index 0db879040..4f3b30ce1 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, + // bool keyed, TODO eprosima bool create_subscription_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp index 017fa3a94..09f31e073 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_request_publisher(rmw_client_t * client) { if (!client) { @@ -33,7 +33,7 @@ get_request_publisher(rmw_client_t * client) return impl->request_publisher_; } -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_response_subscriber(rmw_client_t * client) { if (!client) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index a1c2107de..a699460a6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" +#include "fastdds/dds/domain/DomainParticipant.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" @@ -21,7 +21,7 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Participant * +eprosima::fastdds::dds::DomainParticipant * get_participant(rmw_node_t * node) { if (!node) { @@ -31,7 +31,7 @@ get_participant(rmw_node_t * node) return nullptr; } auto impl = static_cast(node->context->impl->participant_info); - return impl->participant; + return impl->participant_; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp index a60638c99..ac08515e7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_publisher(rmw_publisher_t * publisher) { if (!publisher) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp index 4eb691bc5..96de84cce 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_request_subscriber(rmw_service_t * service) { if (!service) { @@ -33,7 +33,7 @@ get_request_subscriber(rmw_service_t * service) return impl->request_subscriber_; } -eprosima::fastrtps::Publisher * +eprosima::fastdds::dds::DataWriter * get_response_publisher(rmw_service_t * service) { if (!service) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp index 001249f2e..daaecad13 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -20,7 +20,7 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * +eprosima::fastdds::dds::DataReader * get_subscriber(rmw_subscription_t * subscription) { if (!subscription) { diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 0d4a398a5..aaa658fda 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -152,7 +152,7 @@ init_context_impl(rmw_context_t * context) } common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + eprosima_fastrtps_identifier, participant_info->participant_->guid()); common_context->pub = publisher.get(); common_context->sub = subscription.get(); common_context->graph_guard_condition = graph_guard_condition.get(); diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index fc5e3a23b..88109944e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -35,10 +35,10 @@ using_introspection_cpp_typesupport(const char * typesupport_identifier) rosidl_typesupport_introspection_cpp::typesupport_identifier; } -void -_register_type( - eprosima::fastrtps::Participant * participant, - rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) -{ - eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); -} +// void +// _register_type( +// eprosima::fastrtps::Participant * participant, +// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) +// { +// eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); +// } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp index a396d68f3..f0756fec8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -18,9 +18,6 @@ #include #include -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" - #include "rcpputils/find_and_replace.hpp" #include "rmw/error_handling.h" @@ -112,9 +109,9 @@ _create_type_name( return ""; } -void -_register_type( - eprosima::fastrtps::Participant * participant, - rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport); +// void +// _register_type( +// eprosima::fastrtps::Participant * participant, +// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport); #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp index 19341f2bf..da1c9b9ab 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include -#include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/topic/TopicDataType.hpp" -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include @@ -38,7 +38,7 @@ struct SerializedData const void * impl; // RMW implementation specific data }; -class TypeSupport : public eprosima::fastrtps::TopicDataType +class TypeSupport : public eprosima::fastdds::dds::TopicDataType { public: virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const = 0; @@ -84,15 +84,16 @@ class TypeSupport : public eprosima::fastrtps::TopicDataType bool max_size_bound_; }; -inline void -_unregister_type( - eprosima::fastrtps::Participant * participant, - TypeSupport * typed_typesupport) -{ - if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { - delete typed_typesupport; - } -} +// TODO eprosima +// inline void +// _unregister_type( +// eprosima::fastrtps::Participant * participant, +// TypeSupport * typed_typesupport) +// { +// if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { +// delete typed_typesupport; +// } +// } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp index 0435bc4ab..aaecad510 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp @@ -15,7 +15,7 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 0ba4f9342..0a8bf0eed 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -25,12 +25,12 @@ #include "fastcdr/FastBuffer.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/SampleInfo.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -45,12 +45,17 @@ typedef struct CustomClientInfo const void * request_type_support_impl_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_{nullptr}; const void * response_type_support_impl_{nullptr}; - eprosima::fastrtps::Subscriber * response_subscriber_{nullptr}; - eprosima::fastrtps::Publisher * request_publisher_{nullptr}; + eprosima::fastdds::dds::DataReader * response_subscriber_{nullptr}; + eprosima::fastdds::dds::DataWriter * request_publisher_{nullptr}; + eprosima::fastdds::dds::Topic * response_topic_{nullptr}; + eprosima::fastdds::dds::Topic * request_topic_{nullptr}; + ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; - eprosima::fastrtps::Participant * participant_{nullptr}; + eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + eprosima::fastdds::dds::Publisher * publisher_{nullptr}; const char * typesupport_identifier_{nullptr}; ClientPubListener * pub_listener_{nullptr}; std::atomic_size_t response_subscriber_matched_count_; @@ -61,7 +66,7 @@ typedef struct CustomClientResponse { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; std::unique_ptr buffer_; - eprosima::fastrtps::SampleInfo_t sample_info_ {}; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastrtps::SubscriberListener diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index d3139afe2..edd2794ad 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -24,13 +24,6 @@ #include "fastcdr/FastBuffer.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" - #include "rmw/event.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 8b2106b37..6fdafe628 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -20,10 +20,11 @@ #include #include -#include "fastrtps/rtps/common/InstanceHandle.h" -#include "fastrtps/attributes/ParticipantAttributes.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/participant/ParticipantListener.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/domain/DomainParticipantListener.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/rtps/common/Types.h" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" @@ -51,8 +52,11 @@ enum class publishing_mode_t typedef struct CustomParticipantInfo { - eprosima::fastrtps::Participant * participant; - ::ParticipantListener * listener; + eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; + ::DomainParticipantListener * listener_{nullptr}; + + eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; // Flag to establish if the QoS of the participant, // its publishers and its subscribers are going @@ -63,18 +67,18 @@ typedef struct CustomParticipantInfo publishing_mode_t publishing_mode; } CustomParticipantInfo; -class ParticipantListener : public eprosima::fastrtps::ParticipantListener +class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantListener { public: - explicit ParticipantListener( + explicit DomainParticipantListener( const char * identifier, rmw_dds_common::Context * context) : context(context), identifier_(identifier) {} - void onParticipantDiscovery( - eprosima::fastrtps::Participant *, + void on_participant_discovery( + eprosima::fastdds::dds::DomainParticipant *, eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info) override { switch (info.status) { @@ -107,8 +111,8 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener } } - void onSubscriberDiscovery( - eprosima::fastrtps::Participant *, + void on_subscriber_discovery( + eprosima::fastdds::dds::DomainParticipant *, eprosima::fastrtps::rtps::ReaderDiscoveryInfo && info) override { if (eprosima::fastrtps::rtps::ReaderDiscoveryInfo::CHANGED_QOS_READER != info.status) { @@ -118,8 +122,8 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener } } - void onPublisherDiscovery( - eprosima::fastrtps::Participant *, + void on_publisher_discovery( + eprosima::fastdds::dds::DomainParticipant *, eprosima::fastrtps::rtps::WriterDiscoveryInfo && info) override { if (eprosima::fastrtps::rtps::WriterDiscoveryInfo::CHANGED_QOS_WRITER != info.status) { @@ -137,7 +141,7 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener { if (is_alive) { rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; - dds_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); + dds_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); // TODO eprosima : is this a real QoS or is it Attributes context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 5d626a2ee..2c43d9399 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -20,8 +20,9 @@ #include #include -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/topic/Topic.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" @@ -36,8 +37,9 @@ typedef struct CustomPublisherInfo : public CustomEventInfo { virtual ~CustomPublisherInfo() = default; - eprosima::fastrtps::Publisher * publisher_{nullptr}; + eprosima::fastdds::dds::DataWriter * publisher_{nullptr}; PubListener * listener_{nullptr}; + eprosima::fastdds::dds::Topic * topic_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; const void * type_support_impl_{nullptr}; rmw_gid_t publisher_gid{}; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 2b30ab929..94b5de839 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -24,12 +24,12 @@ #include "fastcdr/FastBuffer.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/subscriber/SampleInfo.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/SampleInfo.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -53,11 +53,16 @@ typedef struct CustomServiceInfo const void * request_type_support_impl_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_{nullptr}; const void * response_type_support_impl_{nullptr}; - eprosima::fastrtps::Subscriber * request_subscriber_{nullptr}; - eprosima::fastrtps::Publisher * response_publisher_{nullptr}; + eprosima::fastdds::dds::DataReader * request_subscriber_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_publisher_{nullptr}; + eprosima::fastdds::dds::Topic * response_topic_{nullptr}; + eprosima::fastdds::dds::Topic * request_topic_{nullptr}; + ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; - eprosima::fastrtps::Participant * participant_{nullptr}; + eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + eprosima::fastdds::dds::Publisher * publisher_{nullptr}; const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index bb52cbe4d..d97512117 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -21,8 +21,8 @@ #include #include -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -38,8 +38,9 @@ struct CustomSubscriberInfo : public CustomEventInfo { virtual ~CustomSubscriberInfo() = default; - eprosima::fastrtps::Subscriber * subscriber_{nullptr}; + eprosima::fastdds::dds::DataReader * subscriber_ {nullptr}; SubListener * listener_{nullptr}; + eprosima::fastdds::dds::Topic * topic_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; const void * type_support_impl_{nullptr}; rmw_gid_t subscription_gid_{}; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp index 21cd07fee..7162a5749 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp @@ -20,7 +20,7 @@ #include #include -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" namespace rmw_fastrtps_shared_cpp { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index f10303cf8..4cc4438b6 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -25,6 +25,8 @@ namespace rmw_fastrtps_shared_cpp { +// The Entities Publisher and Subscriber are created with the Participant +// Endpoint SubEntities as DataWriter and DataReader are created with create_publisher and create_subscription RMW_FASTRTPS_SHARED_CPP_PUBLIC CustomParticipantInfo * create_participant( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index 0cdbca24f..befa39a31 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -16,25 +16,16 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" -#include "fastrtps/qos/QosPolicies.h" -#include "fastrtps/qos/ReaderQos.h" -#include "fastrtps/qos/WriterQos.h" +#include +#include +#include +#include +#include #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/visibility_control.h" -namespace eprosima -{ -namespace fastrtps -{ -class SubscriberAttributes; -class PublisherAttributes; -} // namespace fastrtps -} // namespace eprosima - RMW_FASTRTPS_SHARED_CPP_PUBLIC bool is_valid_qos(const rmw_qos_profile_t & qos_policies); @@ -43,25 +34,31 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr); + eprosima::fastdds::dds::DataReaderQos & dataReaderQos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::PublisherAttributes & pattr); + eprosima::fastdds::dds::DataWriterQos & dataWriterQos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +get_topic_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::TopicQos & topicQos); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_time_t dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); /* - * Converts the low-level QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. - * Since WriterQos or ReaderQos does not have information about history and depth, these values are not set + * Converts the low-level QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. + * Since DataWriterQos or DataReaderQos does not have information about history and depth, these values are not set * by this function. * - * \param[in] dds_qos of type WriterQos or ReaderQos - * \param[out] qos the equivalent of the data in WriterQos or ReaderQos in rmw_qos_profile_t + * \param[in] dds_qos of type DataWriterQos or DataReaderQos + * \param[out] qos the equivalent of the data in DataWriterQos or DataReaderQos in rmw_qos_profile_t */ template void @@ -69,7 +66,7 @@ dds_qos_to_rmw_qos( const DDSQoSPolicyT & dds_qos, rmw_qos_profile_t * qos) { - switch (dds_qos.m_reliability.kind) { + switch (dds_qos.reliability().kind) { case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; @@ -81,7 +78,7 @@ dds_qos_to_rmw_qos( break; } - switch (dds_qos.m_durability.kind) { + switch (dds_qos.durability().kind) { case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; break; @@ -93,10 +90,10 @@ dds_qos_to_rmw_qos( break; } - qos->deadline = dds_duration_to_rmw(dds_qos.m_deadline.period); - qos->lifespan = dds_duration_to_rmw(dds_qos.m_lifespan.duration); + qos->deadline = dds_duration_to_rmw(dds_qos.deadline().period); + qos->lifespan = dds_duration_to_rmw(dds_qos.lifespan().duration); - switch (dds_qos.m_liveliness.kind) { + switch (dds_qos.liveliness().kind) { case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; break; @@ -107,7 +104,8 @@ dds_qos_to_rmw_qos( qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; break; } - qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.m_liveliness.lease_duration); + + qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.liveliness().lease_duration); } template @@ -117,15 +115,13 @@ dds_attributes_to_rmw_qos( rmw_qos_profile_t * qos); extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void -dds_attributes_to_rmw_qos( - const eprosima::fastrtps::PublisherAttributes & dds_qos, +void dds_attributes_to_rmw_qos( + const eprosima::fastdds::dds::DataWriterQos & dds_qos, rmw_qos_profile_t * qos); extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void -dds_attributes_to_rmw_qos( - const eprosima::fastrtps::SubscriberAttributes & dds_qos, +void dds_attributes_to_rmw_qos( + const eprosima::fastdds::dds::DataReaderQos & dds_qos, rmw_qos_profile_t * qos); #endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp index 84f88c223..611fa926e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp @@ -15,7 +15,7 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ #define RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ -#include "fastrtps/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" #include "rmw_fastrtps_shared_cpp/visibility_control.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp new file mode 100644 index 000000000..77955d9e3 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -0,0 +1,30 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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 RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ + +#include "fastrtps/types/TypesBase.h" + +#include "rmw/rmw.h" + + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); + +} + +#endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index c860e8ea2..c5a3c24d9 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" #include #include #include diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 608ac658e..40f6d6462 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -13,23 +13,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include -#include "fastrtps/config.h" -#include "fastrtps/Domain.h" -#include "fastrtps/attributes/ParticipantAttributes.h" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" -#include "fastrtps/rtps/common/Locator.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/transport/UDPv4TransportDescriptor.h" +#include "fastdds/dds/domain/DomainParticipantFactory.hpp" +#include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" +#include "fastdds/dds/publisher/qos/PublisherQos.hpp" +#include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" #include "rcutils/filesystem.h" #include "rcutils/get_env.h" @@ -41,13 +32,6 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" -using Domain = eprosima::fastrtps::Domain; -using IPLocator = eprosima::fastrtps::rtps::IPLocator; -using Locator_t = eprosima::fastrtps::rtps::Locator_t; -using Participant = eprosima::fastrtps::Participant; -using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; -using UDPv4TransportDescriptor = eprosima::fastrtps::rtps::UDPv4TransportDescriptor; - #if FASTRTPS_VERSION_MAJOR >= 2 #include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; @@ -90,53 +74,101 @@ get_security_file_paths( } #endif -static +// Private function to create Participant with QoS CustomParticipantInfo * __create_participant( const char * identifier, - const ParticipantAttributes & participantAttrs, + DomainParticipantQos domainParticipantQos, bool leave_middleware_default_qos, publishing_mode_t publishing_mode, - rmw_dds_common::Context * common_context) + rmw_dds_common::Context * common_context, + size_t domain_id) { + ///// // Declare everything before beginning to create things. - ::ParticipantListener * listener = nullptr; - Participant * participant = nullptr; CustomParticipantInfo * participant_info = nullptr; + ///// + // Create Custom Participant + try { + participant_info = new CustomParticipantInfo(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("__create_participant failed to allocate CustomParticipantInfo struct"); + } + // lambda to delete participant info + auto cleanup_participant_info = rcpputils::make_scope_exit( + [participant_info]() { + delete participant_info; + }); + + ///// + // Create listener try { - listener = new ::ParticipantListener( + participant_info->listener_ = new ::ParticipantListener( identifier, common_context); } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate participant listener"); - goto fail; + RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); + } + // lambda to delete listener + auto cleanup_listener = rcpputils::make_scope_exit( + [participant_info]() { + delete participant_info->listener_; + }); + + ///// + // Create Participant + participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( + domain_id, domainParticipantQos, listener) + ) + if (!participant_info->participant_) { + RMW_SET_ERROR_MSG("__create_participant failed to create participant"); + return nullptr; } + // lambda to delete participant + auto cleanup_participant = rcpputils::make_scope_exit( + [participant_info]() { + delete participant_info->participant_; + }); + + ///// + // Set participant info parameters + participant_info->leave_middleware_default_qos = leave_middleware_default_qos; + participant_info->publishing_mode = publishing_mode; + + ///// + // Create Publisher + PublisherQos publisherQos = participant_info->participant_->get_default_publisher_qos(); + publisherQos.entity_factory(domainParticipantQos.entity_factory()); - participant = Domain::createParticipant(participantAttrs, listener); - if (!participant) { - RMW_SET_ERROR_MSG("create_node() could not create participant"); + participant_info->publisher_ = participant_info->participant_->create_publisher(publisherQos); + if (!participant_info->publisher_) { + RMW_SET_ERROR_MSG("__create_participant could not create publisher"); return nullptr; } - try { - participant_info = new CustomParticipantInfo(); - } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate node impl struct"); - goto fail; + // lambda to delete publisher + auto cleanup_publisher = rcpputils::make_scope_exit( + [participant_info]() { + participant_info->participant->delete_publisher(participant_info->publisher); + }); + + ///// + // Create Subscriber + SubscriberQos subscriberQos = participant_info->participant_->get_default_subscriber_qos(); + subscriberQos.entity_factory(domainParticipantQos.entity_factory()); + + participant_info->subscriber_ = participant_info->participant_->create_subscriber(subscriberQos); + if (!participant_info->subscriber_) { + RMW_SET_ERROR_MSG("__create_participant could not create subscriber"); + return nullptr; } - participant_info->leave_middleware_default_qos = leave_middleware_default_qos; - participant_info->publishing_mode = publishing_mode; - participant_info->participant = participant; - participant_info->listener = listener; + cleanup_publisher.cancel(); + cleanup_participant.cancel(); + cleanup_listener.cancel(); + cleanup_participant_info.cancel(); return participant_info; -fail: - rmw_free(listener); - if (participant) { - Domain::removeParticipant(participant); - } - return nullptr; } CustomParticipantInfo * @@ -156,8 +188,13 @@ rmw_fastrtps_shared_cpp::create_participant( } ParticipantAttributes participantAttrs; + + DomainParticipantQos domainParticipantQos; + // Load default XML profile. - Domain::getDefaultParticipantAttributes(participantAttrs); + DomainParticipantFactory::get_instance()->load_profiles(); + domainParticipantQos = DomainParticipantFactory::get_instance()->get_default_participant_qos(); + if (localhost_only) { // In order to use the interface white list, we need to disable the default transport config diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index c5aad2e94..fcd49b4ae 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -16,8 +16,11 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/rtps/common/Time_t.h" #include "rmw/error_handling.h" #include "rmw_dds_common/time_utils.hpp" @@ -53,18 +56,18 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) return result; } +// Private function to encapsulate DataReader and DataWriter together with TopicQos fill entities template bool fill_entity_qos_from_profile( const rmw_qos_profile_t & qos_policies, - DDSEntityQos & entity_qos, - eprosima::fastrtps::HistoryQosPolicy & history_qos) + DDSEntityQos & entity_qos) { switch (qos_policies.history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - history_qos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - history_qos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: break; @@ -75,10 +78,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.durability) { case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - entity_qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_VOLATILE: - entity_qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: break; @@ -89,10 +92,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.reliability) { case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - entity_qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - entity_qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: break; @@ -102,33 +105,33 @@ bool fill_entity_qos_from_profile( } // ensure the history depth is at least the requested queue size - assert(history_qos.depth >= 0); + assert(entity_qos.depth >= 0); // TODO eprosima should not be after initialization? if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(history_qos.depth) < qos_policies.depth) + static_cast(entity_qos.depth) < qos_policies.depth) { if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { RMW_SET_ERROR_MSG( "failed to set history depth since the requested queue size exceeds the DDS type"); return false; } - history_qos.depth = static_cast(qos_policies.depth); + entity_qos.history().depth = static_cast(qos_policies.depth); } if (!is_rmw_duration_unspecified(qos_policies.lifespan)) { - entity_qos.m_lifespan.duration = rmw_time_to_fastrtps(qos_policies.lifespan); + entity_qos.lifespan().duration = rmw_time_to_fastrtps(qos_policies.lifespan); } if (!is_rmw_duration_unspecified(qos_policies.deadline)) { - entity_qos.m_deadline.period = rmw_time_to_fastrtps(qos_policies.deadline); + entity_qos.deadline().period = rmw_time_to_fastrtps(qos_policies.deadline); } switch (qos_policies.liveliness) { case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: - entity_qos.m_liveliness.kind = eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: - entity_qos.m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: break; @@ -137,15 +140,15 @@ bool fill_entity_qos_from_profile( return false; } if (!is_rmw_duration_unspecified(qos_policies.liveliness_lease_duration)) { - entity_qos.m_liveliness.lease_duration = + entity_qos.liveliness().lease_duration = rmw_time_to_fastrtps(qos_policies.liveliness_lease_duration); // Docs suggest setting no higher than 0.7 * lease_duration, choosing 2/3 to give safe buffer. // See doc at https://github.com/eProsima/Fast-RTPS/blob/ // a8691a40be6b8460b01edde36ad8563170a3a35a/include/fastrtps/qos/QosPolicies.h#L223-L232 - double period_in_ns = entity_qos.m_liveliness.lease_duration.to_ns() * 2.0 / 3.0; + double period_in_ns = entity_qos.liveliness().lease_duration.to_ns() * 2.0 / 3.0; double period_in_s = RCUTILS_NS_TO_S(period_in_ns); - entity_qos.m_liveliness.announcement_period = eprosima::fastrtps::Duration_t(period_in_s); + entity_qos.liveliness().announcement_period = eprosima::fastrtps::Duration_t(period_in_s); } return true; @@ -154,16 +157,53 @@ bool fill_entity_qos_from_profile( bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr) + eprosima::fastdds::dds::DataReaderQos & datareader_qos) { - return fill_entity_qos_from_profile(qos_policies, sattr.qos, sattr.topic.historyQos); + return fill_entity_qos_from_profile(qos_policies, datareader_qos); } bool get_datawriter_qos( - const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr) + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::DataWriterQos & datawriter_qos) { - return fill_entity_qos_from_profile(qos_policies, pattr.qos, pattr.topic.historyQos); + return fill_entity_qos_from_profile(qos_policies, datawriter_qos); +} + +bool +get_topic_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::TopicQos & topicQos) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + entity_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + topic_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + entity_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + topic_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + // ensure the history depth is at least the requested queue size + assert(topic_qos.depth >= 0); // TODO eprosima should not be after initialization? + if ( + qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && + static_cast(topic_qos.depth) < qos_policies.depth) + { + if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + topic_qos.history().depth = static_cast(qos_policies.depth); + } } bool @@ -172,13 +212,14 @@ is_valid_qos(const rmw_qos_profile_t & /* qos_policies */) return true; } -template + +template void -dds_attributes_to_rmw_qos( - const AttributeT & dds_qos, +dds_qos_to_rmw_qos + const DDSQoSPolicyT & dds_qos, rmw_qos_profile_t * qos) { - switch (dds_qos.topic.historyQos.kind) { + switch (dds_qos.history().kind) { case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; break; @@ -189,16 +230,17 @@ dds_attributes_to_rmw_qos( qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; break; } - qos->depth = static_cast(dds_qos.topic.historyQos.depth); - dds_qos_to_rmw_qos(dds_qos.qos, qos); + qos->depth = static_cast(dds_qos.history().depth); + dds_qos_to_rmw_qos(dds_qos, qos); } -template -void dds_attributes_to_rmw_qos( - const eprosima::fastrtps::PublisherAttributes & dds_qos, - rmw_qos_profile_t * qos); +// TODO eprosima +// template +// void dds_attributes_to_rmw_qos( +// const eprosima::fastdds::dds::DataWriterQos & dds_qos, +// rmw_qos_profile_t * qos); -template -void dds_attributes_to_rmw_qos( - const eprosima::fastrtps::SubscriberAttributes & dds_qos, - rmw_qos_profile_t * qos); +// template +// void dds_attributes_to_rmw_qos( +// const eprosima::fastdds::dds::DataReaderQos & dds_qos, +// rmw_qos_profile_t * qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp index d395bfa0f..2a902a60f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp index 80710b1de..60e974894 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp @@ -17,13 +17,13 @@ #include "rcutils/logging_macros.h" -#include "fastrtps/log/Log.h" +#include "fastdds/dds/log/Log.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" namespace rmw_fastrtps_shared_cpp { -using eprosima::fastrtps::Log; +using eprosima::fastdds::dds::Log; rmw_ret_t __rmw_set_log_severity(rmw_log_severity_t severity) @@ -49,7 +49,7 @@ __rmw_set_log_severity(rmw_log_severity_t severity) return RMW_RET_ERROR; } - eprosima::fastrtps::Log::SetVerbosity(log_kind); + eprosima::fastdds::dds::Log::SetVerbosity(log_kind); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp index 3a4f9bffb..415070e04 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp @@ -14,8 +14,6 @@ #include -#include "fastrtps/Domain.h" - #include "rcutils/allocator.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" @@ -34,8 +32,6 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -using Participant = eprosima::fastrtps::Participant; - namespace rmw_fastrtps_shared_cpp { rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index fb8cd2a49..76129bf73 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -20,6 +20,9 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" @@ -122,9 +125,9 @@ __rmw_publisher_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(publisher->data); - eprosima::fastrtps::Publisher * fastrtps_pub = info->publisher_; - const eprosima::fastrtps::PublisherAttributes & attributes = - fastrtps_pub->getAttributes(); + eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->publisher_; + const eprosima::fastdds::dds::DataWriterQos & attributes = + fastrtps_pub->get_qos(); dds_attributes_to_rmw_qos(attributes, qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 3d21032f4..9b4a40d55 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -17,8 +17,6 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/impl/cpp/macros.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp index ba17e788c..b20855da9 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp @@ -73,10 +73,10 @@ __rmw_service_server_is_available( } auto pub_topic_name = - client_info->request_publisher_->getAttributes().topic.getTopicName().to_string(); + client_info->request_topic_->get_name(); auto sub_topic_name = - client_info->response_subscriber_->getAttributes().topic.getTopicName().to_string(); + client_info->response_topic_->get_name(); *is_available = false; auto common_context = static_cast(node->context->impl->common); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 4ce18ac5e..c61fe1d39 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -20,7 +20,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -98,9 +99,9 @@ __rmw_subscription_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(subscription->data); - eprosima::fastrtps::Subscriber * fastrtps_sub = info->subscriber_; - const eprosima::fastrtps::SubscriberAttributes & attributes = - fastrtps_sub->getAttributes(); + eprosima::fastdds::dds::DataReader * fastrtps_sub = info->subscriber_; + const eprosima::fastdds::dds::DataReaderQos & attributes = + fastrtps_sub->get_qos(); dds_attributes_to_rmw_qos(attributes, qos); diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp new file mode 100644 index 000000000..cef8c40d5 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -0,0 +1,45 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "rmw/rmw.h" +#include "fastrtps/types/TypesBase.h" + +using ReturnCode_t = eprosima::fastrtps::types::ReturnCode_t; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) +{ + // not switch because it is not an enum class + if (code == ReturnCode_t::RETCODE_OK) { + return RMW_RET_OK; + } else if (code == ReturnCode_t::RETCODE_ERROR) { + // repeats the error to avoid too much if comparations + return RMW_RET_ERROR; + } else if (code == ReturnCode_t::RETCODE_TIMEOUT) { + return RMW_RET_TIMEOUT; + } else if (code == ReturnCode_t::RETCODE_UNSUPPORTED) { + return RMW_RET_UNSUPPORTED; + } else if (code == ReturnCode_t::RETCODE_BAD_PARAMETER) { + return RMW_RET_INVALID_ARGUMENT; + } else if (code == ReturnCode_t::RETCODE_OUT_OF_RESOURCES) { + // Could be that out of resources comes from a different source than a bad allocation + return RMW_RET_BAD_ALLOC; + } else { + return RMW_RET_ERROR; + } +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/test/test_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_logging.cpp index 4401d290b..ea3f036ec 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include Date: Wed, 3 Feb 2021 07:52:59 +0100 Subject: [PATCH 03/60] rebase second steps Signed-off-by: jparisu --- rmw_fastrtps_cpp/src/publisher.cpp | 59 +-- rmw_fastrtps_cpp/src/rmw_client.cpp | 386 +++++++++++------- rmw_fastrtps_cpp/src/rmw_service.cpp | 386 +++++++++++------- rmw_fastrtps_cpp/src/subscription.cpp | 60 ++- .../custom_client_info.hpp | 52 ++- .../custom_participant_info.hpp | 7 +- .../custom_publisher_info.hpp | 23 +- .../custom_service_info.hpp | 6 +- .../custom_subscriber_info.hpp | 50 +-- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 16 +- .../src/custom_publisher_info.cpp | 6 +- .../src/custom_subscriber_info.cpp | 4 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 26 +- rmw_fastrtps_shared_cpp/src/publisher.cpp | 33 +- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 66 ++- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 66 ++- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 61 +-- rmw_fastrtps_shared_cpp/src/subscription.cpp | 43 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 25 ++ 19 files changed, 879 insertions(+), 496 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index b0be5129f..68f4b698e 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -39,6 +39,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_cpp/publisher.hpp" @@ -81,16 +82,22 @@ rmw_fastrtps_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + ///// // Get Participant and Publisher - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant; + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; RMW_CHECK_ARGUMENT_FOR_NULL(domainParticipant, nullptr); - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; RMW_CHECK_ARGUMENT_FOR_NULL(publisher, nullptr); ///// - // Get Type Support + // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -151,7 +158,6 @@ rmw_fastrtps_cpp::create_publisher( // using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - info->type_support_->createData(); // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; @@ -182,38 +188,36 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - return nullptr; - } // Create Topic name auto topic_name_mangled = _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - // Create topic - // it looks for the topic in case it is already created - // info->topic_ = domainParticipant->find_topic(topic_name_mangled, 0); // TODO eprosima - if (info->topic_ == nullptr){ - // the topic does not exist yet it creates a new one - info->topic_ = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; } - if (info->topic_ == nullptr) { + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); + + if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } - // lambda to unregister topic - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, info]() { - domainParticipant->delete_topic(info->topic_); - info->topic_ = nullptr; - }); + eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); + if (des_topic == nullptr) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } ///// // Create DataWriter @@ -240,13 +244,13 @@ rmw_fastrtps_cpp::create_publisher( } // Get QoS from ROS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { return nullptr; } // Creates DataWriter (with publisher name to not change name policy) info->publisher_ = publisher->create_datawriter( - info->topic_, + topic, dataWriterQos, info->listener_); @@ -295,7 +299,6 @@ rmw_fastrtps_cpp::create_publisher( cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); - cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 18f364c84..d71b1c2df 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -14,6 +14,15 @@ #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "rcpputils/scope_exit.hpp" #include "rcutils/error_handling.h" #include "rcutils/logging_macros.h" @@ -31,19 +40,12 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" #include "./type_support_common.hpp" -#include "fastrtps/xmlparser/XMLProfileManager.h" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; -using XMLP_ret = eprosima::fastrtps::xmlparser::XMLP_ret; - extern "C" { rmw_client_t * @@ -52,6 +54,8 @@ rmw_create_client( const rosidl_service_type_support_t * type_supports, const char * service_name, const rmw_qos_profile_t * qos_policies) { + ///// + // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -78,11 +82,38 @@ rmw_create_client( } } + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + ///// + // Get Participant and SubEntities auto common_context = static_cast(node->context->impl->common); auto participant_info = static_cast(node->context->impl->participant_info); - Participant * participant = participant_info->participant; + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + if (!domainParticipant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return nullptr; + } + + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + if (!subscriber) { + RMW_SET_ERROR_MSG("subscriber handle is null"); + return nullptr; + } + + ///// + // Get RMW Type Support const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -103,27 +134,26 @@ rmw_create_client( } } + ///// + // Create the RMW Client struct (info) CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate client info"); return nullptr; } - auto cleanup_base_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->request_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); - } - if (info->response_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); - } + + auto cleanup_info = rcpputils::make_scope_exit( + [info]() { delete info; }); - info->participant_ = participant; + info->participant_ = domainParticipant; info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; + ///// + // Create the Type Support struct const service_type_support_callbacks_t * service_members; const message_type_support_callbacks_t * request_members; const message_type_support_callbacks_t * response_members; @@ -140,155 +170,229 @@ rmw_create_client( std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); - if ( - !Domain::getRegisteredType( - participant, request_type_name.c_str(), - reinterpret_cast(&info->request_type_support_))) - { - info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); - return nullptr; - } - _register_type(participant, info->request_type_support_); + // These structs are not used in the future, as there is no need to unregister the types + info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; } + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info]() { + delete info->request_type_support_; + }); - if ( - !Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) - { - info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); - return nullptr; - } - _register_type(participant, info->response_type_support_); + info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; } + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info]() { + delete info->response_type_support_; + }); - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. - std::string topic_name_fallback = "client"; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - eprosima::fastrtps::fixed_string<255> sub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply"); - Domain::getDefaultSubscriberAttributes(subscriberParam); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) RequestTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + return nullptr; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = response_type_name; - subscriberParam.topic.topicName = sub_topic_name; + auto cleanup_type_support_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. - eprosima::fastrtps::fixed_string<255> pub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request"); - eprosima::fastrtps::PublisherAttributes publisherParam; - Domain::getDefaultPublisherAttributes(publisherParam); + info->pub_listener_ = new (std::nothrow) ClientPubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + return nullptr; + } - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + [info]() { + delete info->pub_listener_; + }); + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; } - if (!participant_info->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; - } + // Create response topic + std::string sub_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_sub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + response_type_name, + topicQos); + + if (des_sub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create response topic"); + return nullptr; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = request_type_name; - publisherParam.topic.topicName = pub_topic_name; + // Create request topic + std::string pub_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "************ Client Details *********"); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); - RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_pub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + request_type_name, + topicQos); - // Create Client Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + if (des_pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create request topic"); return nullptr; } - auto cleanup_response_subscriber = rcpputils::make_scope_exit( - [info]() { - if (info->response_subscriber_) { - if (!Domain::removeSubscriber(info->response_subscriber_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove response subscriber after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - } - if (info->listener_) { - delete info->listener_; - } - }); - info->listener_ = new (std::nothrow) ClientListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + + eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + if (pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; } - info->response_subscriber_ = - Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->response_subscriber_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber"); + + // Key word to find DataWrtier and DataReader QoS + std::string topic_name_fallback = "client"; + + ///// + // Create request DataReader + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile + // located based of topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); + subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + + if (!participant_info->leave_middleware_default_qos) { + dataReaderQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { return nullptr; } - // Create Client Publisher and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + // Creates DataReader (with subscriber name to not change name policy) + info->response_subscriber_ = subscriber->create_datareader( + des_sub_topic, + dataReaderQos, + info->listener_); + + if (!info->response_subscriber_) { + RMW_SET_ERROR_MSG("failed to create client request data reader"); return nullptr; } - auto cleanup_request_publisher = rcpputils::make_scope_exit( - [info]() { - if (info->request_publisher_) { - if (!Domain::removePublisher(info->request_publisher_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove request publisher after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - } - if (info->pub_listener_) { - delete info->pub_listener_; - } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->response_subscriber_); }); - info->pub_listener_ = new (std::nothrow) ClientPubListener(info); - if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + + + ///// + // Create response DataWriter + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile + // located based of topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); + publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { return nullptr; } - info->request_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); + + // Creates DataWriter (with publisher name to not change name policy) + info->request_publisher_ = publisher->create_datawriter( + pub_topic, + dataWriterQos, + info->listener_); + if (!info->request_publisher_) { - RMW_SET_ERROR_MSG("failed to create client request publisher"); + RMW_SET_ERROR_MSG("failed to create client request data writer"); return nullptr; } - info->writer_guid_ = info->request_publisher_->getGuid(); - info->reader_guid_ = info->response_subscriber_->getGuid(); + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->request_publisher_); + }); + + ///// + // Create client + + // Debug info + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "************ Client Details *********"); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Sub Topic %s", sub_topic_name); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Pub Topic %s", pub_topic_name); + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); + + info->writer_guid_ = info->request_publisher_->guid(); + info->reader_guid_ = info->response_subscriber_->guid(); rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { @@ -315,14 +419,14 @@ rmw_create_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + eprosima_fastrtps_identifier, info->request_publisher_->guid()); common_context->graph_cache.associate_writer( request_publisher_gid, common_context->gid, node->name, node->namespace_); rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->response_subscriber_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( response_subscriber_gid, @@ -350,9 +454,13 @@ rmw_create_client( } cleanup_rmw_client.cancel(); - cleanup_response_subscriber.cancel(); - cleanup_request_publisher.cancel(); - cleanup_base_info.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_type_support_pub_listener.cancel(); + cleanup_type_support_listener.cancel(); + cleanup_type_support_response.cancel(); + cleanup_type_support_request.cancel(); + cleanup_info.cancel() return rmw_client; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index e36e791ee..87c478847 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -25,6 +25,15 @@ #include #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "rcpputils/scope_exit.hpp" #include "rcutils/error_handling.h" #include "rcutils/logging_macros.h" @@ -42,20 +51,12 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" #include "type_support_common.hpp" -#include "fastrtps/xmlparser/XMLProfileManager.h" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using CustomParticipantInfo = CustomParticipantInfo; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; -using XMLP_ret = eprosima::fastrtps::xmlparser::XMLP_ret; - extern "C" { rmw_service_t * @@ -90,11 +91,38 @@ rmw_create_service( } } - const CustomParticipantInfo * impl = - static_cast(node->context->impl->participant_info); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + ///// + // Get Participant and SubEntities auto common_context = static_cast(node->context->impl->common); - Participant * participant = impl->participant; + auto participant_info = + static_cast(node->context->impl->participant_info); + + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + if (!domainParticipant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return nullptr; + } + + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + if (!subscriber) { + RMW_SET_ERROR_MSG("subscriber handle is null"); + return nullptr; + } + ///// + // Get RMW Type Support const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -115,25 +143,23 @@ rmw_create_service( } } + ///// + // Create the RMW Service struct (info) CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate service info"); return nullptr; } - auto cleanup_base_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->request_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); - } - if (info->response_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); - } + auto cleanup_info = rcpputils::make_scope_exit( + [info]() { delete info; }); - info->participant_ = participant; + info->participant_ = domainParticipant; info->typesupport_identifier_ = type_support->typesupport_identifier; + ///// + // Create the Type Support struct const service_type_support_callbacks_t * service_members; const message_type_support_callbacks_t * request_members; const message_type_support_callbacks_t * response_members; @@ -150,152 +176,226 @@ rmw_create_service( std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); - if ( - !Domain::getRegisteredType( - participant, request_type_name.c_str(), - reinterpret_cast(&info->request_type_support_))) - { - info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); - return nullptr; - } - _register_type(participant, info->request_type_support_); + // These structs are not used in the future, as there is no need to unregister the types + info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; } + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info]() { + delete info->request_type_support_; + }); - if ( - !Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) - { - info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); - return nullptr; - } - _register_type(participant, info->response_type_support_); + info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; } + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info]() { + delete info->response_type_support_; + }); - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. - std::string topic_name_fallback = "service"; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - eprosima::fastrtps::fixed_string<255> sub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request"); - Domain::getDefaultSubscriberAttributes(subscriberParam); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) RequestTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + return nullptr; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = request_type_name; - subscriberParam.topic.topicName = sub_topic_name; - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. - eprosima::fastrtps::fixed_string<255> pub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply"); - eprosima::fastrtps::PublisherAttributes publisherParam; - Domain::getDefaultPublisherAttributes(publisherParam); - - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + auto cleanup_type_support_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); + + info->pub_listener_ = new (std::nothrow) ServicePubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + return nullptr; } - if (!impl->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (impl->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (impl->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; - } + auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + [info]() { + delete info->pub_listener_; + }); + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = response_type_name; - publisherParam.topic.topicName = pub_topic_name; + // Create request topic + std::string sub_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "************ Service Details *********"); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); - RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_sub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + request_type_name, + topicQos); - // Create Service Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + if (des_sub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create request topic"); return nullptr; } - auto cleanup_request_subscriber = rcpputils::make_scope_exit( - [info]() { - if (info->request_subscriber_) { - if (!Domain::removeSubscriber(info->request_subscriber_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove request subscriber after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - } - if (info->listener_) { - delete info->listener_; - } - }); - info->listener_ = new (std::nothrow) ServiceListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create service request subscriber listener"); + + // Create response topic + std::string pub_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_pub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + response_type_name, + topicQos); + + if (des_pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create response topic"); return nullptr; } - info->request_subscriber_ = - Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->request_subscriber_) { - RMW_SET_ERROR_MSG("failed to create service request subscriber"); + + eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + if (pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; } - // Create Service Publisher and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + // Key word to find DataWrtier and DataReader QoS + std::string topic_name_fallback = "service"; + + ///// + // Create request DataReader + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile + // located based of topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); + subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + + if (!participant_info->leave_middleware_default_qos) { + dataReaderQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { return nullptr; } - auto cleanup_response_publisher = rcpputils::make_scope_exit( - [info]() { - if (info->response_publisher_) { - if (!Domain::removePublisher(info->response_publisher_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove response publisher after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - } - if (info->pub_listener_) { - delete info->pub_listener_; - } + + // Creates DataReader (with subscriber name to not change name policy) + info->request_subscriber_ = subscriber->create_datareader( + des_sub_topic, + dataReaderQos, + info->listener_); + + if (!info->request_subscriber_) { + RMW_SET_ERROR_MSG("failed to create client request data reader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->request_subscriber_); }); - info->pub_listener_ = new (std::nothrow) ServicePubListener(); - if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create service response publisher listener"); + + + ///// + // Create response DataWriter + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile + // located based of topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); + publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { return nullptr; } - info->response_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); + + // Creates DataWriter (with publisher name to not change name policy) + info->response_publisher_ = publisher->create_datawriter( + pub_topic, + dataWriterQos, + info->listener_); + if (!info->response_publisher_) { - RMW_SET_ERROR_MSG("failed to create service response publisher"); + RMW_SET_ERROR_MSG("failed to create client request data writer"); return nullptr; } + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->response_publisher_); + }); + + ///// + // Create Service + + // Debug info + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "************ Service Details *********"); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Sub Topic %s", subscriberParam.topic.topicName.c_str()); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Pub Topic %s", publisherParam.topic.topicName.c_str()); + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); + rmw_service_t * rmw_service = rmw_service_allocate(); if (!rmw_service) { RMW_SET_ERROR_MSG("failed to allocate memory for service"); @@ -321,14 +421,14 @@ rmw_create_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->request_subscriber_->guid()); common_context->graph_cache.associate_reader( request_subscriber_gid, common_context->gid, node->name, node->namespace_); rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + eprosima_fastrtps_identifier, info->response_publisher_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( response_publisher_gid, @@ -356,9 +456,13 @@ rmw_create_service( } cleanup_rmw_service.cancel(); - cleanup_request_subscriber.cancel(); - cleanup_response_publisher.cancel(); - cleanup_base_info.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_type_support_pub_listener.cancel(); + cleanup_type_support_listener.cancel(); + cleanup_type_support_response.cancel(); + cleanup_type_support_request.cancel(); + cleanup_info.cancel(); return rmw_service; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index b0642647a..f6bcf89fa 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -41,6 +41,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_cpp/subscription.hpp" @@ -86,6 +87,12 @@ create_subscription( } RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + ///// // Get Participant and Subscriber eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; @@ -95,7 +102,7 @@ create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(subscriber, nullptr); ///// - // Get Type Support + // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); @@ -157,18 +164,11 @@ create_subscription( // using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - info->type_support_->createData(); // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } - ///// - // Check ROS QoS - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - ///// // Create Listener info->listener_ = nullptr; @@ -187,40 +187,32 @@ create_subscription( }); ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - return nullptr; - } + // Create Topic // Create Topic name auto topic_name_mangled = _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - // Create topic - // it looks for the topic in case it is already created - // info->topic_ = domainParticipant->find_topic(topic_name_mangled, 0); // TODO eprosima - if (info->topic_ == nullptr){ - // the topic does not exist yet it creates a new one - info->topic_ = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; } - if (info->topic_ == nullptr) { + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); + + if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } - // lambda to unregister topic - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, info]() { - domainParticipant->delete_topic(info->topic_); - info->topic_ = nullptr; - }); - ///// // Create DataReader @@ -233,19 +225,18 @@ create_subscription( // It does not need to check the return code, as if the profile does not exist, the QoS is already the default subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); - if (!participant_info->leave_middleware_default_qos) { dataReaderQos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { return nullptr; } // Creates DataReader (with subscriber name to not change name policy) info->subscriber_ = subscriber->create_datareader( - info->topic_, + des_topic, dataReaderQos, info->listener_); @@ -291,7 +282,6 @@ create_subscription( cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); - cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 0a8bf0eed..f29119f45 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -47,15 +47,13 @@ typedef struct CustomClientInfo const void * response_type_support_impl_{nullptr}; eprosima::fastdds::dds::DataReader * response_subscriber_{nullptr}; eprosima::fastdds::dds::DataWriter * request_publisher_{nullptr}; - eprosima::fastdds::dds::Topic * response_topic_{nullptr}; - eprosima::fastdds::dds::Topic * request_topic_{nullptr}; ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; - eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; - eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; - eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + // eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; + // eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + // eprosima::fastdds::dds::Publisher * publisher_{nullptr}; const char * typesupport_identifier_{nullptr}; ClientPubListener * pub_listener_{nullptr}; std::atomic_size_t response_subscriber_matched_count_; @@ -69,7 +67,7 @@ typedef struct CustomClientResponse eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; -class ClientListener : public eprosima::fastrtps::SubscriberListener +class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: explicit ClientListener(CustomClientInfo * info) @@ -78,9 +76,9 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener void - onNewDataMessage(eprosima::fastrtps::Subscriber * sub) + on_data_available(eprosima::fastdds::dds::DataReader * reader) { - assert(sub); + assert(reader); CustomClientResponse response; // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 @@ -90,8 +88,10 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener data.is_cdr_buffer = true; data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true - if (sub->takeNextData(&data, &response.sample_info_)) { - if (eprosima::fastrtps::rtps::ALIVE == response.sample_info_.sampleKind) { + if (reader->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (eprosima::fastdds::dds::InstanceStateKind::ALIVE_INSTANCE_STATE == + response.sample_info_.instance_state) + { response.sample_identity_ = response.sample_info_.related_sample_identity; if (response.sample_identity_.writer_guid() == info_->reader_guid_ || @@ -151,18 +151,17 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener return list_has_data_.load(); } - void onSubscriptionMatched( - eprosima::fastrtps::Subscriber * sub, - eprosima::fastrtps::rtps::MatchingInfo & matchingInfo) + void on_subscription_matched( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { - (void)sub; if (info_ == nullptr) { return; } - if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) { - publishers_.insert(matchingInfo.remoteEndpointGuid); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) { - publishers_.erase(matchingInfo.remoteEndpointGuid); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } else { return; } @@ -190,7 +189,7 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener std::set publishers_; }; -class ClientPubListener : public eprosima::fastrtps::PublisherListener +class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { public: explicit ClientPubListener(CustomClientInfo * info) @@ -198,18 +197,17 @@ class ClientPubListener : public eprosima::fastrtps::PublisherListener { } - void onPublicationMatched( - eprosima::fastrtps::Publisher * pub, - eprosima::fastrtps::rtps::MatchingInfo & matchingInfo) + void on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final { - (void) pub; if (info_ == nullptr) { return; } - if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) { - subscriptions_.insert(matchingInfo.remoteEndpointGuid); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) { - subscriptions_.erase(matchingInfo.remoteEndpointGuid); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } else { return; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 6fdafe628..3361a9e13 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -53,11 +53,16 @@ enum class publishing_mode_t typedef struct CustomParticipantInfo { eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; - ::DomainParticipantListener * listener_{nullptr}; + ParticipantListener * listener_{nullptr}; eprosima::fastdds::dds::Publisher * publisher_{nullptr}; eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + // Struct to store every topic created in the local domain + // These variables needs to be + mutable std::vector topics_list_; + mutable std::mutex topic_creation_mutex_; + // Flag to establish if the QoS of the participant, // its publishers and its subscribers are going // to be configured only from an XML file or if diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 2c43d9399..1d66b1134 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -16,8 +16,8 @@ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ #include -#include #include +#include #include #include "fastdds/dds/publisher/DataWriter.hpp" @@ -39,7 +39,6 @@ typedef struct CustomPublisherInfo : public CustomEventInfo eprosima::fastdds::dds::DataWriter * publisher_{nullptr}; PubListener * listener_{nullptr}; - eprosima::fastdds::dds::Topic * topic_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; const void * type_support_impl_{nullptr}; rmw_gid_t publisher_gid{}; @@ -50,7 +49,7 @@ typedef struct CustomPublisherInfo : public CustomEventInfo getListener() const final; } CustomPublisherInfo; -class PubListener : public EventListenerInterface, public eprosima::fastrtps::PublisherListener +class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener { public: explicit PubListener(CustomPublisherInfo * info) @@ -65,29 +64,29 @@ class PubListener : public EventListenerInterface, public eprosima::fastrtps::Pu // PublisherListener implementation RMW_FASTRTPS_SHARED_CPP_PUBLIC void - onPublicationMatched( - eprosima::fastrtps::Publisher * pub, eprosima::fastrtps::rtps::MatchingInfo & info) final + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final { - (void) pub; std::lock_guard lock(internalMutex_); if (eprosima::fastrtps::rtps::MATCHED_MATCHING == info.status) { - subscriptions_.insert(info.remoteEndpointGuid); + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) { - subscriptions_.erase(info.remoteEndpointGuid); + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } } RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_offered_deadline_missed( - eprosima::fastrtps::Publisher * publisher, - const eprosima::fastrtps::OfferedDeadlineMissedStatus & status) final; + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) final; RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_liveliness_lost( - eprosima::fastrtps::Publisher * publisher, - const eprosima::fastrtps::LivelinessLostStatus & status) final; + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::LivelinessLostStatus & status) final; // EventListenerInterface implementation diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 94b5de839..ee5b07c2b 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -60,9 +60,9 @@ typedef struct CustomServiceInfo ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; - eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; - eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; - eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + // eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; + // eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + // eprosima::fastdds::dds::Publisher * publisher_{nullptr}; const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index d97512117..6f83e504e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -40,7 +40,6 @@ struct CustomSubscriberInfo : public CustomEventInfo eprosima::fastdds::dds::DataReader * subscriber_ {nullptr}; SubListener * listener_{nullptr}; - eprosima::fastdds::dds::Topic * topic_{nullptr}; rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; const void * type_support_impl_{nullptr}; rmw_gid_t subscription_gid_{}; @@ -51,11 +50,11 @@ struct CustomSubscriberInfo : public CustomEventInfo getListener() const final; }; -class SubListener : public EventListenerInterface, public eprosima::fastrtps::SubscriberListener +class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: explicit SubListener(CustomSubscriberInfo * info) - : data_(0), + : data_(false), deadline_changes_(false), liveliness_changes_(false), conditionMutex_(nullptr), @@ -67,36 +66,37 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su // SubscriberListener implementation void - onSubscriptionMatched( - eprosima::fastrtps::Subscriber * sub, eprosima::fastrtps::rtps::MatchingInfo & info) final + on_subscription_matched( + eprosima::fastdds::dds::DataReader * reader, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { { std::lock_guard lock(internalMutex_); - if (eprosima::fastrtps::rtps::MATCHED_MATCHING == info.status) { - publishers_.insert(info.remoteEndpointGuid); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) { - publishers_.erase(info.remoteEndpointGuid); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } } - update_unread_count(sub); + update_unread_count(reader); } void - onNewDataMessage(eprosima::fastrtps::Subscriber * sub) final + on_data_available(eprosima::fastdds::dds::DataReader * reader) final { - update_unread_count(sub); + update_unread_count(reader); } RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_requested_deadline_missed( - eprosima::fastrtps::Subscriber *, + eprosima::fastdds::dds::DataReader *, const eprosima::fastrtps::RequestedDeadlineMissedStatus &) final; RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_liveliness_changed( - eprosima::fastrtps::Subscriber *, + eprosima::fastdds::dds::DataReader *, const eprosima::fastrtps::LivelinessChangedStatus &) final; // EventListenerInterface implementation @@ -128,23 +128,27 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su bool hasData() const { - return data_.load(std::memory_order_relaxed) > 0; + return data_.load(std::memory_order_relaxed); } void - update_unread_count(eprosima::fastrtps::Subscriber * sub) + update_unread_count(eprosima::fastdds::dds::DataReader * reader) { + // TODO (eprosima) // Make sure to call into Fast-RTPS before taking the lock to avoid an // ABBA deadlock between internalMutex_ and mutexes inside of Fast-RTPS. -#if FASTRTPS_VERSION_MAJOR == 1 && FASTRTPS_VERSION_MINOR < 9 - uint64_t unread_count = sub->getUnreadCount(); -#else - uint64_t unread_count = sub->get_unread_count(); -#endif + eprosima::fastdds::dds::SampleInfo info; + ReturnCode_t has_data_ret = reader->get_first_untaken_info(&info); + + // In case there is data, get_first_untaken_info return OK. Else it returs NO_DATA + bool has_data = false; + if (has_data_ret == ReturnCode_t::RETCODE_OK){ + has_data = true; + } std::lock_guard lock(internalMutex_); ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - data_.store(unread_count, std::memory_order_relaxed); + data_.store(has_data, std::memory_order_relaxed); } size_t publisherCount() @@ -156,7 +160,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su private: mutable std::mutex internalMutex_; - std::atomic_size_t data_; + std::atomic_bool data_; std::atomic_bool deadline_changes_; eprosima::fastrtps::RequestedDeadlineMissedStatus requested_deadline_missed_status_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 77955d9e3..ab99cfec1 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -15,8 +15,16 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ +#include + +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "fastrtps/types/TypesBase.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + #include "rmw/rmw.h" @@ -25,6 +33,12 @@ namespace rmw_fastrtps_shared_cpp rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); -} +eprosima::fastdds::dds::TopicDescription* create_topic_rmw( + const CustomParticipantInfo * participant_info, + const std::string& topic_name, + const std::string& type_name, + const eprosima::fastdds::dds::TopicQos& qos); + +} // rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index f38f653dd..f910daca9 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -23,8 +23,8 @@ CustomPublisherInfo::getListener() const void PubListener::on_offered_deadline_missed( - eprosima::fastrtps::Publisher * /* publisher */, - const eprosima::fastrtps::OfferedDeadlineMissedStatus & status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -41,7 +41,7 @@ PubListener::on_offered_deadline_missed( } void PubListener::on_liveliness_lost( - eprosima::fastrtps::Publisher * /* publisher */, + eprosima::fastdds::dds::DataWriter * /* writer */, const eprosima::fastrtps::LivelinessLostStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 172c20ab1..8af36904a 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -23,7 +23,7 @@ CustomSubscriberInfo::getListener() const void SubListener::on_requested_deadline_missed( - eprosima::fastrtps::Subscriber * /* subscriber */, + eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastrtps::RequestedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -41,7 +41,7 @@ SubListener::on_requested_deadline_missed( } void SubListener::on_liveliness_changed( - eprosima::fastrtps::Subscriber * /* subscriber */, + eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastrtps::LivelinessChangedStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 40f6d6462..a4538ba9b 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -13,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -78,7 +78,7 @@ get_security_file_paths( CustomParticipantInfo * __create_participant( const char * identifier, - DomainParticipantQos domainParticipantQos, + eprosima::fastdds::dds::DomainParticipantQos domainParticipantQos, bool leave_middleware_default_qos, publishing_mode_t publishing_mode, rmw_dds_common::Context * common_context, @@ -104,7 +104,7 @@ __create_participant( ///// // Create listener try { - participant_info->listener_ = new ::ParticipantListener( + participant_info->listener_ = new ParticipantListener( identifier, common_context); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); @@ -118,7 +118,7 @@ __create_participant( ///// // Create Participant participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( - domain_id, domainParticipantQos, listener) + domain_id, domainParticipantQos, participant_info->listener_) ) if (!participant_info->participant_) { RMW_SET_ERROR_MSG("__create_participant failed to create participant"); @@ -335,9 +335,21 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant RMW_SET_ERROR_MSG("participant_info is null"); return RMW_RET_ERROR; } - Domain::removeParticipant(participant_info->participant); - delete participant_info->listener; - participant_info->listener = nullptr; + + // Topics must be deleted before delete the participant + for (auto topic : participant_info->topics_list_) + { + participant_info->participant_->delete_topic(topic); + } + + // Delete Domain Participant + ReturnCode_t ret = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); + + // Delete Listener + delete participant_info->listener_; + + // Delete Custom Participant delete participant_info; RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 90eb10984..aa0ff3e55 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -23,6 +23,7 @@ #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -38,17 +39,39 @@ destroy_publisher( assert(publisher->implementation_identifier == identifier); static_cast(identifier); + // Get RMW Publisher + rmw_ret_t ret = RMW_RET_OK; auto info = static_cast(publisher->data); - Domain::removePublisher(info->publisher_); - delete info->listener_; - _unregister_type(participant_info->participant, info->type_support_); - delete info; + // NOTE: Topic deletion and unregister type is done in the participant + if (nullptr != info){ + // Delete DataWriter + ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->publisher_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataWriter listener + if (nullptr != info->listener_){ + delete info->listener_; + } + + // Delete type support inside subscription + if (info->type_support_ != nullptr) { + delete info->type_support_; + } + + // Delete PublisherInfo structure + delete info; + }else{ + ret = RMW_RET_INVALID_ARGUMENT; + } rmw_free(const_cast(publisher->topic_name)); rmw_publisher_free(publisher); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return RMW_RET_OK; + return ret; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 67f226bdc..2c9666af7 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -28,10 +28,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_fastrtps_shared_cpp/utils.hpp" namespace rmw_fastrtps_shared_cpp { @@ -48,14 +45,14 @@ __rmw_destroy_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_publisher_->getGuid()); + identifier, info->request_publisher_->guid()); common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_subscriber_->getGuid()); + identifier, info->response_subscriber_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); @@ -66,13 +63,56 @@ __rmw_destroy_client( nullptr); } - Domain::removeSubscriber(info->response_subscriber_); - Domain::removePublisher(info->request_publisher_); - delete info->pub_listener_; - delete info->listener_; - _unregister_type(info->participant_, info->request_type_support_); - _unregister_type(info->participant_, info->response_type_support_); - delete info; + ///// + // Delete DataWriter and DataReader + auto participant_info = + static_cast(node->context->impl->participant_info); + + rmw_ret_t ret = RMW_RET_OK; + auto info = static_cast(client->data); + + // NOTE: Topic deletion and unregister type is done in the participant + if (nullptr != info){ + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_subscriber_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataReader listener + if (nullptr != info->listener_){ + delete info->listener_; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_publisher_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_){ + delete info->pub_listener_; + } + + // Delete request type support inside subscription + if (info->request_type_support_ != nullptr) { + delete info->request_type_support_; + } + + // Delete response type support inside subscription + if (info->response_type_support_ != nullptr) { + delete info->response_type_support_; + } + + // Delete ClientInfo structure + delete info; + }else{ + ret = RMW_RET_INVALID_ARGUMENT; + } rmw_free(const_cast(client->service_name)); rmw_client_free(client); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index df5b47133..e134f0eb7 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -40,10 +40,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_fastrtps_shared_cpp/utils.hpp" namespace rmw_fastrtps_shared_cpp { @@ -61,14 +58,14 @@ __rmw_destroy_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_subscriber_->getGuid()); + identifier, info->request_subscriber_->guid()); common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_publisher_->getGuid()); + identifier, info->response_publisher_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); @@ -79,13 +76,56 @@ __rmw_destroy_service( nullptr); } - Domain::removeSubscriber(info->request_subscriber_); - Domain::removePublisher(info->response_publisher_); - delete info->pub_listener_; - delete info->listener_; - _unregister_type(info->participant_, info->request_type_support_); - _unregister_type(info->participant_, info->response_type_support_); - delete info; + ///// + // Delete DataWriter and DataReader + auto participant_info = + static_cast(node->context->impl->participant_info); + + rmw_ret_t ret = RMW_RET_OK; + auto info = static_cast(service->data); + + // NOTE: Topic deletion and unregister type is done in the participant + if (nullptr != info){ + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_subscriber_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataReader listener + if (nullptr != info->listener_){ + delete info->listener_; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->response_publisher_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_){ + delete info->pub_listener_; + } + + // Delete request type support inside subscription + if (info->request_type_support_ != nullptr) { + delete info->request_type_support_; + } + + // Delete response type support inside subscription + if (info->response_type_support_ != nullptr) { + delete info->response_type_support_; + } + + // Delete ServiceInfo structure + delete info; + }else{ + ret = RMW_RET_INVALID_ARGUMENT; + } rmw_free(const_cast(service->service_name)); rmw_service_free(service); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index c915b91d5..a160cba0e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -17,9 +17,7 @@ #include "rmw/serialized_message.h" #include "rmw/rmw.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" @@ -35,10 +33,12 @@ void _assign_message_info( const char * identifier, rmw_message_info_t * message_info, - const eprosima::fastrtps::SampleInfo_t * sinfo) + const eprosima::fastdds::dds::SampleInfo * sinfo) { - message_info->source_timestamp = sinfo->sourceTimestamp.to_ns(); - message_info->received_timestamp = sinfo->receptionTimestamp.to_ns(); + message_info->source_timestamp = sinfo->source_timestamp.to_ns(); + // TODO (eprosima) + // message_info->received_timestamp = sinfo->receive_timestamp.to_ns(); + message_info->received_timestamp = sinfo->source_timestamp.to_ns(); rmw_gid_t * sender_gid = &message_info->publisher_gid; sender_gid->implementation_identifier = identifier; memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); @@ -68,16 +68,19 @@ _take( CustomSubscriberInfo * info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - eprosima::fastrtps::SampleInfo_t sinfo; + eprosima::fastdds::dds::SampleInfo sinfo; rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; data.data = ros_message; data.impl = info->type_support_impl_; - if (info->subscriber_->takeNextData(&data, &sinfo)) { + if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + + // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == sinfo.instance_state) { if (message_info) { _assign_message_info(identifier, message_info, &sinfo); } @@ -110,26 +113,32 @@ _take_sequence( CustomSubscriberInfo * info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - // Limit the upper bound of reads to the number unread at the beginning. - // This prevents any samples that are added after the beginning of the - // _take_sequence call from being read. - auto unread_count = info->subscriber_->get_unread_count(); - if (unread_count < count) { - count = unread_count; + // Take every message available (Maximum number of messages) + eprosima::fastdds::dds::LoanableSequence info_seq; + // Datas must be initialized before calling take + eprosima::fastdds::dds::LoanableSequence data_seq(count); + + // Initialize all RMW Type Support Data to send sequence to take + for (size_t i = 0; i < count; ++i) { + data_seq[i].is_cdr_buffer = false; + data_seq[i].data = message_sequence->data[i]; + data_seq[i].impl = info->type_support_impl_; } - for (size_t ii = 0; ii < count; ++ii) { - taken_flag = false; - ret = _take( - identifier, subscription, message_sequence->data[*taken], - &taken_flag, &message_info_sequence->data[*taken], allocation); - if (ret != RMW_RET_OK) { - break; - } + ReturnCode_t ret = info->subscriber_->take(data_seq, info_seq, count); + + // Update hasData from listener + info->listener_->update_unread_count(info->subscriber_); - if (taken_flag) { - (*taken)++; + for (size_t ii = 0; ii < info_seq.length(); ++ii) { + if (info_seq[ii].valid_data){ + if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == info_seq[ii].instance_state) { + if (message_info_sequence->data + *taken) { + _assign_message_info(identifier, message_info_sequence->data + *taken, &info_seq[ii]); + } + (*taken)++; + } } } @@ -274,7 +283,7 @@ _take_serialized_message( RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); eprosima::fastcdr::FastBuffer buffer; - eprosima::fastrtps::SampleInfo_t sinfo; + eprosima::fastdds::dds::SampleInfo sinfo; rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index c66955350..b8dc79cb0 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -19,11 +19,6 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "fastrtps/Domain.h" -#include "fastrtps/TopicDataType.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" @@ -31,10 +26,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/subscription.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_fastrtps_shared_cpp/utils.hpp" namespace rmw_fastrtps_shared_cpp { @@ -47,17 +39,34 @@ destroy_subscription( assert(subscription->implementation_identifier == identifier); static_cast(identifier); + // Get RMW Subscriber rmw_ret_t ret = RMW_RET_OK; auto info = static_cast(subscription->data); - if (!Domain::removeSubscriber(info->subscriber_)) { - RMW_SET_ERROR_MSG("failed to remove subscriber"); - ret = RMW_RET_ERROR; - } - delete info->listener_; - Participant * participant = participant_info->participant; - _unregister_type(participant, info->type_support_); - delete info; + // NOTE: Topic deletion and unregister type is done in the participant + if (nullptr != info){ + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->subscriber_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete datareader"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + // Delete DataReader listener + if (nullptr != info->listener_){ + delete info->listener_; + } + + // Delete type support inside subscription + if (info->type_support_ != nullptr) { + delete info->type_support_; + } + + // Delete SubscriberInfo structure + delete info; + }else{ + ret = RMW_RET_INVALID_ARGUMENT; + } rmw_free(const_cast(subscription->topic_name)); rmw_subscription_free(subscription); diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index cef8c40d5..ff36c5c9f 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -21,6 +21,7 @@ using ReturnCode_t = eprosima::fastrtps::types::ReturnCode_t; namespace rmw_fastrtps_shared_cpp { + rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) { // not switch because it is not an enum class @@ -42,4 +43,28 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) return RMW_RET_ERROR; } } + +eprosima::fastdds::dds::TopicDescription* create_topic_rmw( + const CustomParticipantInfo * participant_info, + const std::string& topic_name, + const std::string& type_name, + const eprosima::fastdds::dds::TopicQos& qos) +{ + // This block will lock the topic creations in this participant + std::lock_guard lck (participant_info->topic_creation_mutex_); + + // Searchs for an already existing topic + eprosima::fastdds::dds::TopicDescription* des_topic = participant_info->participant_->lookup_topicdescription(topic_name); + if (nullptr != des_topic) + { + // TODO the already existing topic type must fit with the desired type for new topic + return des_topic; + } + + // Creates topic + eprosima::fastdds::dds::Topic* topic = participant_info->participant_->create_topic(topic_name, type_name, qos); + participant_info->topics_list_.push_back(topic); + return topic; +} + } // namespace rmw_fastrtps_shared_cpp From d44ea5f986bac574dc1e1943b4e2f150a060f949 Mon Sep 17 00:00:00 2001 From: jparisu Date: Wed, 3 Feb 2021 17:06:15 +0100 Subject: [PATCH 04/60] rebase third steps, compiling Signed-off-by: jparisu Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/get_participant.cpp | 2 +- .../src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 9 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 33 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 11 +- rmw_fastrtps_cpp/src/subscription.cpp | 3 +- rmw_fastrtps_cpp/src/type_support_common.hpp | 9 - .../test/test_get_native_entities.cpp | 2 +- rmw_fastrtps_cpp/test/test_logging.cpp | 14 +- .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 2 +- .../src/get_participant.cpp | 1 + rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 215 ++++++--- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 452 +++++++++++------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 451 ++++++++++------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 196 +++++--- .../test/test_get_native_entities.cpp | 2 +- .../test/test_logging.cpp | 2 +- .../custom_client_info.hpp | 3 + .../custom_participant_info.hpp | 4 +- .../custom_publisher_info.hpp | 9 +- .../custom_service_info.hpp | 57 ++- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 98 +++- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 1 + rmw_fastrtps_shared_cpp/src/participant.cpp | 78 ++- rmw_fastrtps_shared_cpp/src/qos.cpp | 56 ++- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 11 +- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 6 +- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 8 +- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 11 +- .../src/rmw_service_server_is_available.cpp | 4 +- .../src/rmw_subscription.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 11 +- rmw_fastrtps_shared_cpp/test/test_logging.cpp | 2 +- .../test/test_rmw_qos_to_dds_attributes.cpp | 126 ++--- 35 files changed, 1175 insertions(+), 724 deletions(-) diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index de4542088..e6263e1cb 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -32,7 +32,7 @@ get_participant(rmw_node_t * node) return nullptr; } auto impl = static_cast(node->context->impl->participant_info); - return impl->participant; + return impl->participant_; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index fa3420d19..51138919e 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -152,7 +152,7 @@ init_context_impl(rmw_context_t * context) } common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant->guid()); + eprosima_fastrtps_identifier, participant_info->participant_->guid()); common_context->pub = publisher.get(); common_context->sub = subscription.get(); common_context->graph_guard_condition = graph_guard_condition.get(); diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 68f4b698e..1832e1dc5 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -53,7 +53,7 @@ rmw_fastrtps_cpp::create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // always false + bool /* keyed */, bool create_publisher_listener) { ///// @@ -141,7 +141,6 @@ rmw_fastrtps_cpp::create_publisher( auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); - // This struct is not used in the future, as there is no need to unregister the types info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); if (!info->type_support_) { RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); @@ -163,12 +162,6 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - ///// - // Check ROS QoS - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - ///// // Create Listener info->listener_ = nullptr; diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index d71b1c2df..c38927189 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -52,7 +52,8 @@ rmw_client_t * rmw_create_client( const rmw_node_t * node, const rosidl_service_type_support_t * type_supports, - const char * service_name, const rmw_qos_profile_t * qos_policies) + const char * service_name, + const rmw_qos_profile_t * qos_policies) { ///// // Check input parameters @@ -147,7 +148,6 @@ rmw_create_client( delete info; }); - info->participant_ = domainParticipant; info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; @@ -170,7 +170,6 @@ rmw_create_client( std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); - // These structs are not used in the future, as there is no need to unregister the types info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); if (!info->request_type_support_) { RMW_SET_ERROR_MSG("failed to allocate request typesupport"); @@ -202,7 +201,7 @@ rmw_create_client( return nullptr; } - ReturnCode_t ret = domainParticipant->register_type( + ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { @@ -217,7 +216,7 @@ rmw_create_client( return nullptr; } - auto cleanup_type_support_listener = rcpputils::make_scope_exit( + auto cleanup_listener = rcpputils::make_scope_exit( [info]() { delete info->listener_; }); @@ -228,7 +227,7 @@ rmw_create_client( return nullptr; } - auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + auto cleanup_pub_listener = rcpputils::make_scope_exit( [info]() { delete info->pub_listener_; }); @@ -283,6 +282,9 @@ rmw_create_client( return nullptr; } + info->request_topic_ = pub_topic_name; + info->response_topic_ = sub_topic_name; + // Key word to find DataWrtier and DataReader QoS std::string topic_name_fallback = "client"; @@ -317,7 +319,7 @@ rmw_create_client( info->listener_); if (!info->response_subscriber_) { - RMW_SET_ERROR_MSG("failed to create client request data reader"); + RMW_SET_ERROR_MSG("failed to create client response data reader"); return nullptr; } @@ -327,10 +329,6 @@ rmw_create_client( subscriber->delete_datareader(info->response_subscriber_); }); - - ///// - // Create response DataWriter - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default attributes. @@ -363,7 +361,7 @@ rmw_create_client( info->request_publisher_ = publisher->create_datawriter( pub_topic, dataWriterQos, - info->listener_); + info->pub_listener_); if (!info->request_publisher_) { RMW_SET_ERROR_MSG("failed to create client request data writer"); @@ -385,10 +383,10 @@ rmw_create_client( "************ Client Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Sub Topic %s", sub_topic_name); + "Sub Topic %s", sub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Pub Topic %s", pub_topic_name); + "Pub Topic %s", pub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); info->writer_guid_ = info->request_publisher_->guid(); @@ -425,6 +423,7 @@ rmw_create_client( common_context->gid, node->name, node->namespace_); + rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_subscriber_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = @@ -456,11 +455,11 @@ rmw_create_client( cleanup_rmw_client.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_type_support_pub_listener.cancel(); - cleanup_type_support_listener.cancel(); + cleanup_pub_listener.cancel(); + cleanup_listener.cancel(); cleanup_type_support_response.cancel(); cleanup_type_support_request.cancel(); - cleanup_info.cancel() + cleanup_info.cancel(); return rmw_client; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 87c478847..e8ceb0385 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -65,6 +65,8 @@ rmw_create_service( const rosidl_service_type_support_t * type_supports, const char * service_name, const rmw_qos_profile_t * qos_policies) { + ///// + // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -155,7 +157,6 @@ rmw_create_service( delete info; }); - info->participant_ = domainParticipant; info->typesupport_identifier_ = type_support->typesupport_identifier; ///// @@ -208,7 +209,7 @@ rmw_create_service( return nullptr; } - ReturnCode_t ret = domainParticipant->register_type( + ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { @@ -368,7 +369,7 @@ rmw_create_service( info->response_publisher_ = publisher->create_datawriter( pub_topic, dataWriterQos, - info->listener_); + info->pub_listener_); if (!info->response_publisher_) { RMW_SET_ERROR_MSG("failed to create client request data writer"); @@ -390,10 +391,10 @@ rmw_create_service( "************ Service Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); + "Sub Topic %s", sub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", pub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); rmw_service_t * rmw_service = rmw_service_allocate(); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index f6bcf89fa..571c9eb42 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -58,7 +58,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, + bool /* keyed */, bool create_subscription_listener) { ///// @@ -147,7 +147,6 @@ create_subscription( auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); - // This struct is not used in the future, as there is no need to unregister the types info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); if (!info->type_support_) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 148c94acc..44da0ca7f 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -61,13 +61,4 @@ _create_type_name( return ss.str(); } -inline void -_register_type( - eprosima::fastdds::dds::DomainParticipant * participant, - rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) -{ - // TODO eprosima - // eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); -} - #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp index 78ed3008c..532931e47 100644 --- a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include "fastrtps/xmlparser/XMLProfileManager.h" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" #include "rcutils/error_handling.h" @@ -32,6 +37,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" #include "rmw_fastrtps_dynamic_cpp/publisher.hpp" @@ -39,12 +45,7 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; rmw_publisher_t * rmw_fastrtps_dynamic_cpp::create_publisher( @@ -56,6 +57,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( bool keyed, bool create_publisher_listener) { + ///// + // Check input parameters (void)keyed; (void)create_publisher_listener; @@ -81,9 +84,22 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); - Participant * participant = participant_info->participant; - RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + RMW_CHECK_ARGUMENT_FOR_NULL(domainParticipant, nullptr); + + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, nullptr); + ///// + // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (!type_support) { @@ -108,30 +124,23 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // publisher which profile name matches with topic_name. If such profile does not exist, - // then use the default attributes. - eprosima::fastrtps::PublisherAttributes publisherParam; - Domain::getDefaultPublisherAttributes(publisherParam); // Loads the XML file if not loaded - XMLProfileManager::fillPublisherAttributes(topic_name, publisherParam, false); - + ///// + // Create the RMW Publisher struct (info) CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; info = new (std::nothrow) CustomPublisherInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate CustomPublisherInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->type_support_) { - _unregister_type(participant, info->type_support_); - } - delete info->listener_; + [info]() { delete info; }); + ///// + // Create the Type Support struct TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_impl = type_registry.get_message_type_support(type_support); if (!type_impl) { @@ -146,71 +155,141 @@ rmw_fastrtps_dynamic_cpp::create_publisher( info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_impl; + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); + return nullptr; + } + auto cleanup_type_support = rcpputils::make_scope_exit( + [info]() { + delete info->type_support_; + }); + + ///// + // Register the Type in the participant std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) - { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - return nullptr; - } - _register_type(participant, info->type_support_); + + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!participant_info->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; - } + ///// + // Create Listener + info->listener_ = nullptr; + if (create_publisher_listener) { + info->listener_ = new (std::nothrow) PubListener(info); } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + return nullptr; + } - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; + auto cleanup_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; + ///// + // Create and register Topic - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + // Create Topic name + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { return nullptr; } - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); + + if (des_topic == nullptr) { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } - info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); + eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); + if (des_topic == nullptr) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } + + ///// + // Create DataWriter + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datawriter which profile name matches with topic_name. If such profile does not exist, + // then use the default QoS. + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + // Get QoS from ROS + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + return nullptr; + } + + // Creates DataWriter (with publisher name to not change name policy) + info->publisher_ = publisher->create_datawriter( + topic, + dataWriterQos, + info->listener_); + if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } - auto cleanup_publisher = rcpputils::make_scope_exit( - [info]() { - if (!Domain::removePublisher(info->publisher_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove publisher after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->publisher_); }); + // 1 Heartbeat every 10ms + // publisherParam.times.heartbeatPeriod.seconds = 0; + // publisherParam.times.heartbeatPeriod.fraction = 42949673; + + // 300000 bytes each 10ms + // ThroughputControllerDescriptor throughputController{3000000, 10}; + // publisherParam.throughputController = throughputController; + + ///// + // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->getGuid()); + eprosima_fastrtps_identifier, info->publisher_->guid()); + + ///// + // Allocate publisher + rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); - rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { RMW_SET_ERROR_MSG("failed to allocate publisher"); return nullptr; @@ -235,8 +314,10 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->options = *publisher_options; cleanup_rmw_publisher.cancel(); - cleanup_publisher.cancel(); - cleanup_info.cancel(); + cleanup_datawriter.cancel(); + cleanup_listener.cancel(); + cleanup_type_support.cancel(); return_type_support.cancel(); + cleanup_info.cancel(); return rmw_publisher; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index ebc4412a9..8529714d0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -14,6 +14,16 @@ #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" #include "rmw/allocators.h" @@ -33,6 +43,7 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" @@ -40,16 +51,6 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" -#include "fastrtps/xmlparser/XMLProfileManager.h" - -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; -using XMLP_ret = eprosima::fastrtps::xmlparser::XMLP_ret; - extern "C" { rmw_client_t * @@ -58,6 +59,8 @@ rmw_create_client( const rosidl_service_type_support_t * type_supports, const char * service_name, const rmw_qos_profile_t * qos_policies) { + ///// + // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -84,20 +87,38 @@ rmw_create_client( } } + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + ///// + // Get Participant and SubEntities auto common_context = static_cast(node->context->impl->common); auto participant_info = static_cast(node->context->impl->participant_info); - if (!participant_info) { - RMW_SET_ERROR_MSG("participant info is null"); + + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + if (!domainParticipant) { + RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; } - Participant * participant = participant_info->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); return nullptr; } + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + if (!subscriber) { + RMW_SET_ERROR_MSG("subscriber handle is null"); + return nullptr; + } + + ///// + // Get RMW Type Support const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (!type_support) { @@ -118,39 +139,72 @@ rmw_create_client( } } - CustomClientInfo * info = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - eprosima::fastrtps::PublisherAttributes publisherParam; - rmw_client_t * rmw_client = nullptr; - eprosima::fastrtps::fixed_string<255> sub_topic_name; - eprosima::fastrtps::fixed_string<255> pub_topic_name; - std::string topic_name_fallback; - - info = new CustomClientInfo(); - info->participant_ = participant; - info->typesupport_identifier_ = type_support->typesupport_identifier; + ///// + // Create the RMW Client struct (info) + CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate client info"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info]() { + delete info; + }); + + // info->participant_ = domainParticipant; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; + ///// + // Create the Type Support struct TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { - delete info; RMW_SET_ERROR_MSG("failed to allocate request type support"); return nullptr; } + auto return_request_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_request_type_support(type_support); + }); auto response_type_impl = type_registry.get_response_type_support(type_support); if (!response_type_impl) { type_registry.return_request_type_support(type_support); - delete info; RMW_SET_ERROR_MSG("failed to allocate response type support"); return nullptr; } + auto return_response_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_response_type_support(type_support); + }); info->request_type_support_impl_ = request_type_impl; info->response_type_support_impl_ = response_type_impl; + info->request_type_support_ = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info]() { + delete info->request_type_support_; + }); + + info->response_type_support_ = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info]() { + delete info->response_type_support_; + }); + const void * untyped_request_members; const void * untyped_response_members; @@ -164,125 +218,216 @@ rmw_create_client( std::string response_type_name = _create_type_name( untyped_response_members, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, request_type_name.c_str(), - reinterpret_cast(&info->request_type_support_))) - { - info->request_type_support_ = new (std::nothrow) TypeSupportProxy(request_type_impl); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->request_type_support_); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) - { - info->response_type_support_ = new (std::nothrow) TypeSupportProxy(response_type_impl); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->response_type_support_); + ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + return nullptr; + } + + auto cleanup_type_support_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); + + info->pub_listener_ = new (std::nothrow) ClientPubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + return nullptr; + } + + auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + [info]() { + delete info->pub_listener_; + }); + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } + + // Create response topic + std::string sub_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_sub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + response_type_name, + topicQos); + + if (des_sub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create response topic"); + return nullptr; + } + + // Create request topic + std::string pub_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_pub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + request_type_name, + topicQos); + + if (des_pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create request topic"); + return nullptr; } + eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + if (pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + return nullptr; + } + + // Key word to find DataWrtier and DataReader QoS + std::string topic_name_fallback = "client"; + + ///// + // Create request DataReader + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default attributes. - topic_name_fallback = "client"; - sub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply"); - Domain::getDefaultSubscriberAttributes(subscriberParam); + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); - } + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); + subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = + dataReaderQos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = response_type_name; - subscriberParam.topic.topicName = sub_topic_name; + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + return nullptr; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->response_subscriber_ = subscriber->create_datareader( + des_sub_topic, + dataReaderQos, + info->listener_); + + if (!info->response_subscriber_) { + RMW_SET_ERROR_MSG("failed to create client response data reader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->response_subscriber_); + }); // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default attributes. - pub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request"); - Domain::getDefaultPublisherAttributes(publisherParam); + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); - } + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); + publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } + + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + return nullptr; + } + + // Creates DataWriter (with publisher name to not change name policy) + info->request_publisher_ = publisher->create_datawriter( + pub_topic, + dataWriterQos, + info->pub_listener_); + + if (!info->request_publisher_) { + RMW_SET_ERROR_MSG("failed to create client request data writer"); + return nullptr; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = request_type_name; - publisherParam.topic.topicName = pub_topic_name; + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->request_publisher_); + }); + + ///// + // Create client RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", "************ Client Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); + "Sub Topic %s", sub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", pub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); - // Create Client Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - info->listener_ = new ClientListener(info); - info->response_subscriber_ = - Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->response_subscriber_) { - RMW_SET_ERROR_MSG("create_client() could not create subscriber"); - goto fail; - } + info->writer_guid_ = info->request_publisher_->guid(); + info->reader_guid_ = info->response_subscriber_->guid(); - // Create Client Subscriber and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - info->pub_listener_ = new ClientPubListener(info); - info->request_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); - if (!info->request_publisher_) { - RMW_SET_ERROR_MSG("create_client() could not create publisher"); - goto fail; - } - - info->writer_guid_ = info->request_publisher_->getGuid(); - info->reader_guid_ = info->response_subscriber_->getGuid(); - - rmw_client = rmw_client_allocate(); + rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { RMW_SET_ERROR_MSG("failed to allocate memory for client"); - goto fail; + return nullptr; } + auto cleanup_rmw_client = rcpputils::make_scope_exit( + [rmw_client]() { + rmw_free(const_cast(rmw_client->service_name)); + rmw_free(rmw_client); + }); rmw_client->implementation_identifier = eprosima_fastrtps_identifier; rmw_client->data = info; @@ -290,7 +435,7 @@ rmw_create_client( rmw_allocate(strlen(service_name) + 1)); if (!rmw_client->service_name) { RMW_SET_ERROR_MSG("failed to allocate memory for client name"); - goto fail; + return nullptr; } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); @@ -298,90 +443,49 @@ rmw_create_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + eprosima_fastrtps_identifier, info->request_publisher_->guid()); common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + + rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( - gid, common_context->gid, node->name, node->namespace_); + gid_response, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, common_context->pub, static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - goto fail; - } - } - - return rmw_client; - -fail: - if (info != nullptr) { - if (info->request_publisher_ != nullptr) { - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); - common_context->graph_cache.dissociate_writer( - gid, + common_context->graph_cache.dissociate_reader( + gid_response, common_context->gid, node->name, node->namespace_); - Domain::removePublisher(info->request_publisher_); - } - - if (info->response_subscriber_ != nullptr) { - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); - common_context->graph_cache.dissociate_reader( + common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); - Domain::removeSubscriber(info->response_subscriber_); - } - - if (info->pub_listener_ != nullptr) { - delete info->pub_listener_; - } - - if (info->listener_ != nullptr) { - delete info->listener_; - } - - if (participant_info) { - if (info->request_type_support_ != nullptr) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); - } - - if (info->response_type_support_ != nullptr) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); - } - } else { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_dynamic_cpp", - "leaking type support objects because node impl is null"); - } - - type_registry.return_request_type_support(type_support); - type_registry.return_response_type_support(type_support); - delete info; - info = nullptr; - } - - if (nullptr != rmw_client) { - if (rmw_client->service_name != nullptr) { - rmw_free(const_cast(rmw_client->service_name)); - rmw_client->service_name = nullptr; + return nullptr; } - rmw_client_free(rmw_client); } - return nullptr; + cleanup_rmw_client.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_type_support_pub_listener.cancel(); + cleanup_type_support_listener.cancel(); + cleanup_type_support_response.cancel(); + cleanup_type_support_request.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_client; } rmw_ret_t @@ -405,12 +509,12 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto impl = static_cast(const_cast(info->request_type_support_impl_)); + auto impl = static_cast(const_cast(info->request_type_support_impl_)); auto ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_request_type_support(ros_type_support); - impl = static_cast(const_cast(info->response_type_support_impl_)); + impl = static_cast(const_cast(info->response_type_support_impl_)); ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_response_type_support(ros_type_support); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 85ecddbc0..c221f932c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -25,6 +25,16 @@ #include #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "rcpputils/scope_exit.hpp" #include "rcutils/error_handling.h" #include "rcutils/logging_macros.h" @@ -45,6 +55,7 @@ #include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" @@ -52,16 +63,6 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" -#include "fastrtps/xmlparser/XMLProfileManager.h" - -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; -using XMLP_ret = eprosima::fastrtps::xmlparser::XMLP_ret; - extern "C" { rmw_service_t * @@ -70,6 +71,8 @@ rmw_create_service( const rosidl_service_type_support_t * type_supports, const char * service_name, const rmw_qos_profile_t * qos_policies) { + ///// + // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -96,20 +99,38 @@ rmw_create_service( } } - const CustomParticipantInfo * impl = - static_cast(node->context->impl->participant_info); - auto common_context = static_cast(node->context->impl->common); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { return nullptr; } - Participant * participant = impl->participant; - if (!participant) { + ///// + // Get Participant and SubEntities + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + if (!domainParticipant) { RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; } + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return nullptr; + } + + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + if (!subscriber) { + RMW_SET_ERROR_MSG("subscriber handle is null"); + return nullptr; + } + + ///// + // Get RMW Type Support const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (!type_support) { @@ -130,19 +151,23 @@ rmw_create_service( } } - CustomServiceInfo * info = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - eprosima::fastrtps::PublisherAttributes publisherParam; - rmw_service_t * rmw_service = nullptr; - eprosima::fastrtps::fixed_string<255> sub_topic_name; - eprosima::fastrtps::fixed_string<255> pub_topic_name; - std::string topic_name_fallback; - + ///// + // Create the RMW Service struct (info) + CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate service info"); + return nullptr; + } + auto cleanup_info = rcpputils::make_scope_exit( + [info]() { + delete info; + }); - info = new CustomServiceInfo(); - info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; + ///// + // Create the Type Support struct + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { @@ -150,14 +175,20 @@ rmw_create_service( RMW_SET_ERROR_MSG("failed to allocate request type support"); return nullptr; } + auto return_request_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_request_type_support(type_support); + }); auto response_type_impl = type_registry.get_response_type_support(type_support); if (!response_type_impl) { - type_registry.return_request_type_support(type_support); - delete info; RMW_SET_ERROR_MSG("failed to allocate response type support"); return nullptr; } + auto return_response_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_response_type_support(type_support); + }); info->request_type_support_impl_ = request_type_impl; info->response_type_support_impl_ = response_type_impl; @@ -165,6 +196,28 @@ rmw_create_service( const void * untyped_request_members; const void * untyped_response_members; + info->request_type_support_ = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info]() { + delete info->request_type_support_; + }); + + info->response_type_support_ = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info]() { + delete info->response_type_support_; + }); + untyped_request_members = get_request_ptr(type_support->data, info->typesupport_identifier_); untyped_response_members = get_response_ptr( @@ -175,128 +228,224 @@ rmw_create_service( std::string response_type_name = _create_type_name( untyped_response_members, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, request_type_name.c_str(), - reinterpret_cast(&info->request_type_support_))) - { - info->request_type_support_ = new (std::nothrow) TypeSupportProxy(request_type_impl); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->request_type_support_); + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) - { - info->response_type_support_ = new (std::nothrow) TypeSupportProxy(response_type_impl); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->response_type_support_); + ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; + } + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + return nullptr; + } + + auto cleanup_type_support_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); + + info->pub_listener_ = new (std::nothrow) ServicePubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + return nullptr; + } + + auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + [info]() { + delete info->pub_listener_; + }); + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } + + // Create request topic + std::string sub_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_sub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + request_type_name, + topicQos); + + if (des_sub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create request topic"); + return nullptr; + } + + // Create response topic + std::string pub_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_pub_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + response_type_name, + topicQos); + + if (des_pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed to create response topic"); + return nullptr; } + eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + if (pub_topic == nullptr) { + RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + return nullptr; + } + + // Key word to find DataWrtier and DataReader QoS + std::string topic_name_fallback = "service"; + + ///// + // Create request DataReader + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. - topic_name_fallback = "service"; - sub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request"); - Domain::getDefaultSubscriberAttributes(subscriberParam); - - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); + subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + + if (!participant_info->leave_middleware_default_qos) { + dataReaderQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + return nullptr; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->request_subscriber_ = subscriber->create_datareader( + des_sub_topic, + dataReaderQos, + info->listener_); + + if (!info->request_subscriber_) { + RMW_SET_ERROR_MSG("failed to create client request data reader"); + return nullptr; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = request_type_name; - subscriberParam.topic.topicName = sub_topic_name; + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->request_subscriber_); + }); + + + ///// + // Create response DataWriter // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. - pub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply"); - Domain::getDefaultPublisherAttributes(publisherParam); + // with profile_name "client" is attempted. Else, use the default attributes. + eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + + // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); + publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + dataWriterQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!impl->leave_middleware_default_qos) { - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - if (impl->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - } else if (impl->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; - } + if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + return nullptr; + } + + // Creates DataWriter (with publisher name to not change name policy) + info->response_publisher_ = publisher->create_datawriter( + pub_topic, + dataWriterQos, + info->pub_listener_); + + if (!info->response_publisher_) { + RMW_SET_ERROR_MSG("failed to create client request data writer"); + return nullptr; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = response_type_name; - publisherParam.topic.topicName = pub_topic_name; + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->response_publisher_); + }); + + ///// + // Create Service RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", "************ Service Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); + "Sub Topic %s", sub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", pub_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); - // Create Service Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - info->listener_ = new ServiceListener(info); - info->request_subscriber_ = - Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->request_subscriber_) { - RMW_SET_ERROR_MSG("create_client() could not create subscriber"); - goto fail; - } - - // Create Service Publisher and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - info->pub_listener_ = new ServicePubListener(); - info->response_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); - if (!info->response_publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; - } - - rmw_service = rmw_service_allocate(); + rmw_service_t * rmw_service = rmw_service_allocate(); if (!rmw_service) { RMW_SET_ERROR_MSG("failed to allocate memory for service"); - goto fail; + return nullptr; } + auto cleanup_rmw_service = rcpputils::make_scope_exit( + [rmw_service]() { + rmw_free(const_cast(rmw_service->service_name)); + rmw_free(rmw_service); + }); + rmw_service->implementation_identifier = eprosima_fastrtps_identifier; rmw_service->data = info; rmw_service->service_name = reinterpret_cast( rmw_allocate(strlen(service_name) + 1)); if (!rmw_service->service_name) { RMW_SET_ERROR_MSG("failed to allocate memory for service name"); - goto fail; + return nullptr; } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); @@ -304,14 +453,15 @@ rmw_create_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->request_subscriber_->guid()); common_context->graph_cache.associate_reader( gid, common_context->gid, node->name, node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + + rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); @@ -321,65 +471,30 @@ rmw_create_service( static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - goto fail; - } - } - - return rmw_service; - -fail: - - if (info) { - if (info->response_publisher_) { - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); common_context->graph_cache.dissociate_writer( - gid, + gid_response, common_context->gid, node->name, node->namespace_); - Domain::removePublisher(info->response_publisher_); - } - - if (info->pub_listener_) { - delete info->pub_listener_; - } - - if (info->request_subscriber_) { - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); - Domain::removeSubscriber(info->request_subscriber_); - } - - if (info->listener_) { - delete info->listener_; - } - - if (info->request_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); - } - - if (info->response_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); - } - - type_registry.return_request_type_support(type_support); - type_registry.return_response_type_support(type_support); - delete info; + return nullptr; } } - if (rmw_service && rmw_service->service_name) { - rmw_free(const_cast(rmw_service->service_name)); - rmw_service->service_name = nullptr; - } - rmw_service_free(rmw_service); - - return nullptr; + cleanup_rmw_service.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_type_support_pub_listener.cancel(); + cleanup_type_support_listener.cancel(); + cleanup_type_support_response.cancel(); + cleanup_type_support_request.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_service; } rmw_ret_t @@ -402,12 +517,12 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto impl = static_cast(const_cast(info->request_type_support_impl_)); + auto impl = static_cast(const_cast(info->request_type_support_impl_)); auto ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_request_type_support(ros_type_support); - impl = static_cast(const_cast(info->response_type_support_impl_)); + impl = static_cast(const_cast(info->response_type_support_impl_)); ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_response_type_support(ros_type_support); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 156fc6ece..89f928f7b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -15,6 +15,14 @@ #include #include +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDataType.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "rcutils/error_handling.h" #include "rmw/allocators.h" @@ -30,6 +38,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" #include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" @@ -41,13 +50,6 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; - namespace rmw_fastrtps_dynamic_cpp { @@ -61,6 +63,8 @@ create_subscription( bool keyed, bool create_subscription_listener) { + ///// + // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); @@ -85,9 +89,22 @@ create_subscription( (void)keyed; (void)create_subscription_listener; - Participant * participant = participant_info->participant; - RMW_CHECK_FOR_NULL_WITH_MSG(participant, "participant handle is null", return nullptr); + ///// + // Check ROS QoS + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); + + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + RMW_CHECK_ARGUMENT_FOR_NULL(subscriber, nullptr); + ///// + // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (!type_support) { @@ -108,93 +125,147 @@ create_subscription( } } - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } + ///// + // Create the RMW Subscriber struct (info) + CustomSubscriberInfo * info = nullptr; - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // subscriber which profile name matches with topic_name. If such profile does not exist, then use - // the default attributes. - eprosima::fastrtps::SubscriberAttributes subscriberParam; - Domain::getDefaultSubscriberAttributes(subscriberParam); // Loads the XML file if not loaded - XMLProfileManager::fillSubscriberAttributes(topic_name, subscriberParam, false); - - CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); + info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( - [info, participant, type_support]() { - if (info->type_support_impl_) { - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - type_registry.return_message_type_support(type_support); - } - if (info->type_support_) { - _unregister_type(participant, info->type_support_); - } - delete info->listener_; + [info]() { delete info; }); + ///// + // Create the Type Support struct TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_support_impl = type_registry.get_message_type_support(type_support); if (!type_support_impl) { RMW_SET_ERROR_MSG("failed to allocate type support"); return nullptr; } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_message_type_support(type_support); + }); + info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support_impl; + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; + } + auto cleanup_type_support = rcpputils::make_scope_exit( + [info]() { + delete info->type_support_; + }); + std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) - { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_support_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - return nullptr; - } - _register_type(participant, info->type_support_); + + ///// + // Register the Type in the participant + // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep + // using it. Thus we use a new TypeSupport created only to register it. + ReturnCode_t ret = domainParticipant->register_type( + eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_support_impl))); + // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + return nullptr; } - if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + ///// + // Create Listener + info->listener_ = nullptr; + if (create_subscription_listener) { + info->listener_ = new (std::nothrow) SubListener(info); } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + auto cleanup_listener = rcpputils::make_scope_exit( + [info]() { + delete info->listener_; + }); - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + ///// + // Create Topic + + // Create Topic name + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topicQos)) { return nullptr; } - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + // General function to create or get an already existing topic + eprosima::fastdds::dds::TopicDescription * des_topic = + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); + + if (des_topic == nullptr) { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default QoS. + eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); + + if (!participant_info->leave_middleware_default_qos) { + dataReaderQos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + return nullptr; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->subscriber_ = subscriber->create_datareader( + des_topic, + dataReaderQos, + info->listener_); + if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); return nullptr; } - auto cleanup_subscription = rcpputils::make_scope_exit( - [info]() { - if (!Domain::removeSubscriber(info->subscriber_)) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to remove subscriber after '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->subscriber_); }); + + ///// + // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->subscriber_->guid()); rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { @@ -221,7 +292,10 @@ create_subscription( rmw_subscription->can_loan_messages = false; cleanup_rmw_subscription.cancel(); - cleanup_subscription.cancel(); + cleanup_datareader.cancel(); + cleanup_listener.cancel(); + cleanup_type_support.cancel(); + return_type_support.cancel(); cleanup_info.cancel(); return rmw_subscription; } diff --git a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp index da1c9b9ab..21289bd5e 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 1d66b1134..09d11ba23 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -20,6 +20,7 @@ #include #include +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/topic/Topic.hpp" @@ -69,9 +70,9 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds const eprosima::fastdds::dds::PublicationMatchedStatus & info) final { std::lock_guard lock(internalMutex_); - if (eprosima::fastrtps::rtps::MATCHED_MATCHING == info.status) { + if (info.current_count_change == 1) { subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) { + } else if (info.current_count_change == -1) { subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } } @@ -128,11 +129,11 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool deadline_changes_; - eprosima::fastrtps::OfferedDeadlineMissedStatus offered_deadline_missed_status_ + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; - eprosima::fastrtps::LivelinessLostStatus liveliness_lost_status_ + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index ee5b07c2b..0702aa359 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -55,8 +55,8 @@ typedef struct CustomServiceInfo const void * response_type_support_impl_{nullptr}; eprosima::fastdds::dds::DataReader * request_subscriber_{nullptr}; eprosima::fastdds::dds::DataWriter * response_publisher_{nullptr}; - eprosima::fastdds::dds::Topic * response_topic_{nullptr}; - eprosima::fastdds::dds::Topic * request_topic_{nullptr}; + //eprosima::fastdds::dds::Topic * response_topic_{nullptr}; + //eprosima::fastdds::dds::Topic * request_topic_{nullptr}; ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; @@ -70,13 +70,13 @@ typedef struct CustomServiceRequest { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; eprosima::fastcdr::FastBuffer * buffer_; - eprosima::fastrtps::SampleInfo_t sample_info_ {}; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; CustomServiceRequest() : buffer_(nullptr) {} } CustomServiceRequest; -class ServicePubListener : public eprosima::fastrtps::PublisherListener +class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { using subscriptions_set_t = std::unordered_set; public: - ServicePubListener() = default; + ServicePubListener(CustomServiceInfo * info) + { + (void) info; + } void - onPublicationMatched( - eprosima::fastrtps::Publisher * pub, - eprosima::fastrtps::rtps::MatchingInfo & matchingInfo) + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final { - (void) pub; std::lock_guard lock(mutex_); - if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) { - subscriptions_.insert(matchingInfo.remoteEndpointGuid); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) { - subscriptions_.erase(matchingInfo.remoteEndpointGuid); - auto endpoint = clients_endpoints_.find(matchingInfo.remoteEndpointGuid); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + subscriptions_.erase(erase_endpoint_guid); + auto endpoint = clients_endpoints_.find(erase_endpoint_guid); if (endpoint != clients_endpoints_.end()) { clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(matchingInfo.remoteEndpointGuid); + clients_endpoints_.erase(erase_endpoint_guid); } } else { return; @@ -171,7 +175,7 @@ class ServicePubListener : public eprosima::fastrtps::PublisherListener std::condition_variable cv_; }; -class ServiceListener : public eprosima::fastrtps::SubscriberListener +class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: explicit ServiceListener(CustomServiceInfo * info) @@ -182,20 +186,21 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener } void - onSubscriptionMatched( - eprosima::fastrtps::Subscriber * sub, - eprosima::fastrtps::rtps::MatchingInfo & matchingInfo) + on_subscription_matched( + eprosima::fastdds::dds::DataReader * reader, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { - (void) sub; - if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) { - info_->pub_listener_->endpoint_erase_if_exists(matchingInfo.remoteEndpointGuid); + (void) reader; + if (info.current_count_change == -1) { + info_->pub_listener_->endpoint_erase_if_exists( + eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } } void - onNewDataMessage(eprosima::fastrtps::Subscriber * sub) + on_data_available(eprosima::fastdds::dds::DataReader * reader) final { - assert(sub); + assert(reader); CustomServiceRequest request; request.buffer_ = new eprosima::fastcdr::FastBuffer(); @@ -204,8 +209,8 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener data.is_cdr_buffer = true; data.data = request.buffer_; data.impl = nullptr; // not used when is_cdr_buffer is true - if (sub->takeNextData(&data, &request.sample_info_)) { - if (eprosima::fastrtps::rtps::ALIVE == request.sample_info_.sampleKind) { + if (reader->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == request.sample_info_.instance_state) { request.sample_identity_ = request.sample_info_.sample_identity; // Use response subscriber guid (on related_sample_identity) when present. const eprosima::fastrtps::rtps::GUID_t & reader_guid = diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index befa39a31..e44d3eeea 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -62,15 +62,15 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); */ template void -dds_qos_to_rmw_qos( +dds_qos_to_rmw_qos( // TODO (eprosima) not in use const DDSQoSPolicyT & dds_qos, rmw_qos_profile_t * qos) { switch (dds_qos.reliability().kind) { - case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + case eprosima::fastdds::dds::ReliabilityQosPolicyKind::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; - case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + case eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; break; default: @@ -79,10 +79,10 @@ dds_qos_to_rmw_qos( } switch (dds_qos.durability().kind) { - case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + case eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; break; - case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + case eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; break; default: @@ -94,10 +94,10 @@ dds_qos_to_rmw_qos( qos->lifespan = dds_duration_to_rmw(dds_qos.lifespan().duration); switch (dds_qos.liveliness().kind) { - case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + case eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; break; - case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + case eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; break; default: @@ -106,8 +106,82 @@ dds_qos_to_rmw_qos( } qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.liveliness().lease_duration); + + switch (dds_qos.history().kind) { + case eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + break; + case eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + break; + default: + qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + break; + } + qos->depth = static_cast(dds_qos.history().depth); +} + +template +void +rtps_qos_to_rmw_qos( + const DDSQoSPolicyT & dds_qos, + rmw_qos_profile_t * qos) +{ + switch (dds_qos.m_reliability.kind) { + case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (dds_qos.m_durability.kind) { + case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline.sec = dds_qos.m_deadline.period.seconds; + qos->deadline.nsec = dds_qos.m_deadline.period.nanosec; + + qos->lifespan.sec = dds_qos.m_lifespan.duration.seconds; + qos->lifespan.nsec = dds_qos.m_lifespan.duration.nanosec; + + switch (dds_qos.m_liveliness.kind) { + case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + default: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + break; + } + qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.m_liveliness.lease_duration); } +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataWriterQos & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataReaderQos & dds_qos, + rmw_qos_profile_t * qos); + + template void dds_attributes_to_rmw_qos( @@ -115,13 +189,15 @@ dds_attributes_to_rmw_qos( rmw_qos_profile_t * qos); extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void dds_attributes_to_rmw_qos( - const eprosima::fastdds::dds::DataWriterQos & dds_qos, +void +dds_attributes_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, rmw_qos_profile_t * qos); extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void dds_attributes_to_rmw_qos( - const eprosima::fastdds::dds::DataReaderQos & dds_qos, +void +dds_attributes_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, rmw_qos_profile_t * qos); #endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index ab99cfec1..291897178 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -24,6 +24,7 @@ #include "fastrtps/types/TypesBase.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw/rmw.h" diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index a4538ba9b..663e4ff86 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -21,7 +21,10 @@ #include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" #include "fastdds/dds/publisher/qos/PublisherQos.hpp" #include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" +#include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" +#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" +#include "rcpputils/scope_exit.hpp" #include "rcutils/filesystem.h" #include "rcutils/get_env.h" @@ -31,11 +34,7 @@ #include "rmw_fastrtps_shared_cpp/participant.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" - -#if FASTRTPS_VERSION_MAJOR >= 2 -#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" -using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; -#endif +#include "rmw_fastrtps_shared_cpp/utils.hpp" #if HAVE_SECURITY static @@ -102,7 +101,7 @@ __create_participant( }); ///// - // Create listener + // Create Participant listener try { participant_info->listener_ = new ParticipantListener( identifier, common_context); @@ -118,8 +117,8 @@ __create_participant( ///// // Create Participant participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( - domain_id, domainParticipantQos, participant_info->listener_) - ) + domain_id, domainParticipantQos, participant_info->listener_); + if (!participant_info->participant_) { RMW_SET_ERROR_MSG("__create_participant failed to create participant"); return nullptr; @@ -127,7 +126,8 @@ __create_participant( // lambda to delete participant auto cleanup_participant = rcpputils::make_scope_exit( [participant_info]() { - delete participant_info->participant_; + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); }); ///// @@ -137,7 +137,7 @@ __create_participant( ///// // Create Publisher - PublisherQos publisherQos = participant_info->participant_->get_default_publisher_qos(); + eprosima::fastdds::dds::PublisherQos publisherQos = participant_info->participant_->get_default_publisher_qos(); publisherQos.entity_factory(domainParticipantQos.entity_factory()); participant_info->publisher_ = participant_info->participant_->create_publisher(publisherQos); @@ -149,12 +149,12 @@ __create_participant( // lambda to delete publisher auto cleanup_publisher = rcpputils::make_scope_exit( [participant_info]() { - participant_info->participant->delete_publisher(participant_info->publisher); + participant_info->participant_->delete_publisher(participant_info->publisher_); }); ///// // Create Subscriber - SubscriberQos subscriberQos = participant_info->participant_->get_default_subscriber_qos(); + eprosima::fastdds::dds::SubscriberQos subscriberQos = participant_info->participant_->get_default_subscriber_qos(); subscriberQos.entity_factory(domainParticipantQos.entity_factory()); participant_info->subscriber_ = participant_info->participant_->create_subscriber(subscriberQos); @@ -186,49 +186,38 @@ rmw_fastrtps_shared_cpp::create_participant( RMW_SET_ERROR_MSG("security_options is null"); return nullptr; } - ParticipantAttributes participantAttrs; - - - DomainParticipantQos domainParticipantQos; // Load default XML profile. - DomainParticipantFactory::get_instance()->load_profiles(); - domainParticipantQos = DomainParticipantFactory::get_instance()->get_default_participant_qos(); + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles(); + eprosima::fastdds::dds::DomainParticipantQos domainParticipantQos = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); if (localhost_only) { // In order to use the interface white list, we need to disable the default transport config - participantAttrs.rtps.useBuiltinTransports = false; + domainParticipantQos.transport().use_builtin_transports = false; // Add a UDPv4 transport with only localhost enabled - auto udp_transport = std::make_shared(); + auto udp_transport = std::make_shared(); udp_transport->interfaceWhiteList.emplace_back("127.0.0.1"); - participantAttrs.rtps.userTransports.push_back(udp_transport); + domainParticipantQos.transport().user_transports.push_back(udp_transport); // Add SHM transport if available -#if FASTRTPS_VERSION_MAJOR >= 2 - auto shm_transport = std::make_shared(); - participantAttrs.rtps.userTransports.push_back(shm_transport); -#endif + auto shm_transport = std::make_shared(); + domainParticipantQos.transport().user_transports.push_back(udp_transport); } - // No custom handling of RMW_DEFAULT_DOMAIN_ID. Simply use a reasonable domain id. -#if FASTRTPS_VERSION_MAJOR < 2 - participantAttrs.rtps.builtin.domainId = static_cast(domain_id); -#else - participantAttrs.domainId = static_cast(domain_id); -#endif - size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; - participantAttrs.rtps.userData.resize(length); + domainParticipantQos.user_data().resize(length); + int written = snprintf( - reinterpret_cast(participantAttrs.rtps.userData.data()), + reinterpret_cast(domainParticipantQos.user_data().data()), length, "enclave=%s;", enclave); if (written < 0 || written > static_cast(length) - 1) { RMW_SET_ERROR_MSG("failed to populate user_data buffer"); return nullptr; } - participantAttrs.rtps.setName(enclave); + domainParticipantQos.name(enclave); bool leave_middleware_default_qos = false; publishing_mode_t publishing_mode = publishing_mode_t::ASYNCHRONOUS; @@ -264,9 +253,9 @@ rmw_fastrtps_shared_cpp::create_participant( } // allow reallocation to support discovery messages bigger than 5000 bytes if (!leave_middleware_default_qos) { - participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = + domainParticipantQos.wire_protocol().builtin.readerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = + domainParticipantQos.wire_protocol().builtin.writerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } if (security_options->security_root_path) { @@ -322,10 +311,11 @@ rmw_fastrtps_shared_cpp::create_participant( } return __create_participant( identifier, - participantAttrs, + domainParticipantQos, leave_middleware_default_qos, publishing_mode, - common_context); + common_context, + domain_id); } rmw_ret_t @@ -339,12 +329,20 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant // Topics must be deleted before delete the participant for (auto topic : participant_info->topics_list_) { - participant_info->participant_->delete_topic(topic); + ReturnCode_t ret = participant_info->participant_->delete_topic(topic); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } } // Delete Domain Participant ReturnCode_t ret = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( participant_info->participant_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } // Delete Listener delete participant_info->listener_; diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index fcd49b4ae..63ea48732 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -105,10 +105,10 @@ bool fill_entity_qos_from_profile( } // ensure the history depth is at least the requested queue size - assert(entity_qos.depth >= 0); // TODO eprosima should not be after initialization? + assert(entity_qos.history().depth >= 0); // TODO eprosima should not be after initialization? if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(entity_qos.depth) < qos_policies.depth) + static_cast(entity_qos.history().depth) < qos_policies.depth) { if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { RMW_SET_ERROR_MSG( @@ -173,15 +173,13 @@ get_datawriter_qos( bool get_topic_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastdds::dds::TopicQos & topicQos) + eprosima::fastdds::dds::TopicQos & topic_qos) { switch (qos_policies.history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - entity_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; topic_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - entity_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; topic_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: @@ -192,10 +190,10 @@ get_topic_qos( } // ensure the history depth is at least the requested queue size - assert(topic_qos.depth >= 0); // TODO eprosima should not be after initialization? + assert(topic_qos.history().depth >= 0); // TODO eprosima should not be after initialization? if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(topic_qos.depth) < qos_policies.depth) + static_cast(topic_qos.history().depth) < qos_policies.depth) { if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { RMW_SET_ERROR_MSG( @@ -204,6 +202,8 @@ get_topic_qos( } topic_qos.history().depth = static_cast(qos_policies.depth); } + + return true; } bool @@ -212,14 +212,13 @@ is_valid_qos(const rmw_qos_profile_t & /* qos_policies */) return true; } - -template +template void -dds_qos_to_rmw_qos - const DDSQoSPolicyT & dds_qos, +dds_attributes_to_rmw_qos( + const AttributeT & dds_qos, rmw_qos_profile_t * qos) { - switch (dds_qos.history().kind) { + switch (dds_qos.topic.historyQos.kind) { case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; break; @@ -230,17 +229,26 @@ dds_qos_to_rmw_qos qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; break; } - qos->depth = static_cast(dds_qos.history().depth); - dds_qos_to_rmw_qos(dds_qos, qos); + qos->depth = static_cast(dds_qos.topic.historyQos.depth); + rtps_qos_to_rmw_qos(dds_qos.qos, qos); } -// TODO eprosima -// template -// void dds_attributes_to_rmw_qos( -// const eprosima::fastdds::dds::DataWriterQos & dds_qos, -// rmw_qos_profile_t * qos); - -// template -// void dds_attributes_to_rmw_qos( -// const eprosima::fastdds::dds::DataReaderQos & dds_qos, -// rmw_qos_profile_t * qos); +template +void dds_attributes_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_attributes_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataWriterQos & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataReaderQos & dds_qos, + rmw_qos_profile_t * qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 2c9666af7..19f8380a2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -38,7 +38,7 @@ __rmw_destroy_client( rmw_node_t * node, rmw_client_t * client) { - rmw_ret_t ret = RMW_RET_OK; + rmw_ret_t final_ret = RMW_RET_OK; auto common_context = static_cast(node->context->impl->common); auto info = static_cast(client->data); { @@ -56,7 +56,7 @@ __rmw_destroy_client( rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, static_cast(&msg), @@ -68,9 +68,6 @@ __rmw_destroy_client( auto participant_info = static_cast(node->context->impl->participant_info); - rmw_ret_t ret = RMW_RET_OK; - auto info = static_cast(client->data); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info){ @@ -111,13 +108,13 @@ __rmw_destroy_client( // Delete ClientInfo structure delete info; }else{ - ret = RMW_RET_INVALID_ARGUMENT; + final_ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(client->service_name)); rmw_client_free(client); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return ret; + return final_ret; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 76129bf73..d3178c2e6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -126,10 +126,10 @@ __rmw_publisher_get_actual_qos( { auto info = static_cast(publisher->data); eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->publisher_; - const eprosima::fastdds::dds::DataWriterQos & attributes = + const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastrtps_pub->get_qos(); - dds_attributes_to_rmw_qos(attributes, qos); + dds_qos_to_rmw_qos(dds_qos, qos); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 9b4a40d55..ae98932d8 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -104,8 +104,10 @@ __rmw_take_request( request_header->request_id.sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; - request_header->source_timestamp = request.sample_info_.sourceTimestamp.to_ns(); - request_header->received_timestamp = request.sample_info_.receptionTimestamp.to_ns(); + request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); + // TODO eprosima + // request_header->received_timestamp = request.sample_info_.received_timestamp.to_ns(); + request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); *taken = true; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 2c700c8f6..c89b3ce4c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -62,12 +62,14 @@ __rmw_take_response( if (info->response_type_support_->deserializeROSmessage( deser, ros_response, info->response_type_support_impl_)) { - request_header->source_timestamp = response.sample_info_.sourceTimestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.receptionTimestamp.to_ns(); + request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); + // TODO eprosima + // request_header->received_timestamp = request.sample_info_.received_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.source_timestamp.to_ns(); request_header->request_id.sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; - + *taken = true; } } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index e134f0eb7..11ca6fa53 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -50,7 +50,7 @@ __rmw_destroy_service( rmw_node_t * node, rmw_service_t * service) { - rmw_ret_t ret = RMW_RET_OK; + rmw_ret_t final_ret = RMW_RET_OK; auto common_context = static_cast(node->context->impl->common); auto info = static_cast(service->data); @@ -69,7 +69,7 @@ __rmw_destroy_service( rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, static_cast(&msg), @@ -81,9 +81,6 @@ __rmw_destroy_service( auto participant_info = static_cast(node->context->impl->participant_info); - rmw_ret_t ret = RMW_RET_OK; - auto info = static_cast(service->data); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info){ @@ -124,13 +121,13 @@ __rmw_destroy_service( // Delete ServiceInfo structure delete info; }else{ - ret = RMW_RET_INVALID_ARGUMENT; + final_ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(service->service_name)); rmw_service_free(service); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return ret; + return final_ret; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp index b20855da9..74201087e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp @@ -73,10 +73,10 @@ __rmw_service_server_is_available( } auto pub_topic_name = - client_info->request_topic_->get_name(); + client_info->request_topic_; auto sub_topic_name = - client_info->response_topic_->get_name(); + client_info->response_topic_; *is_available = false; auto common_context = static_cast(node->context->impl->common); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index c61fe1d39..25f3258d4 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -100,10 +100,10 @@ __rmw_subscription_get_actual_qos( { auto info = static_cast(subscription->data); eprosima::fastdds::dds::DataReader * fastrtps_sub = info->subscriber_; - const eprosima::fastdds::dds::DataReaderQos & attributes = + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastrtps_sub->get_qos(); - dds_attributes_to_rmw_qos(attributes, qos); + dds_qos_to_rmw_qos(dds_qos, qos); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index a160cba0e..c88be5183 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -125,8 +125,8 @@ _take_sequence( data_seq[i].impl = info->type_support_impl_; } - - ReturnCode_t ret = info->subscriber_->take(data_seq, info_seq, count); + ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); + static_cast (take_ret); // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); @@ -289,10 +289,13 @@ _take_serialized_message( data.is_cdr_buffer = true; data.data = &buffer; data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->subscriber_->takeNextData(&data, &sinfo)) { + + if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + + // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == sinfo.instance_state) { auto buffer_size = static_cast(buffer.getBufferSize()); if (serialized_message->buffer_capacity < buffer_size) { auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); diff --git a/rmw_fastrtps_shared_cpp/test/test_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_logging.cpp index ea3f036ec..73365e05b 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include (subscriber_attributes_.topic.historyQos.depth)); + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + subscriber_qos_.history().kind); + EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); - using depth_type = decltype(subscriber_attributes_.topic.historyQos.depth); + using depth_type = decltype(subscriber_qos_.history().depth); constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_attributes_)); - EXPECT_LE(depth, static_cast(subscriber_attributes_.topic.historyQos.depth)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); } } -using eprosima::fastrtps::PublisherAttributes; +using eprosima::fastdds::dds::DataWriterQos; class GetDataWriterQoSTest : public ::testing::Test { protected: rmw_qos_profile_t qos_profile_{rmw_qos_profile_default}; - PublisherAttributes publisher_attributes_{}; + DataWriterQos publisher_qos_{}; }; TEST_F(GetDataWriterQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -175,51 +175,51 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, - publisher_attributes_.qos.m_reliability.kind); + publisher_qos_.reliability().kind); EXPECT_EQ( eprosima::fastrtps::VOLATILE_DURABILITY_QOS, - publisher_attributes_.qos.m_durability.kind); + publisher_qos_.durability().kind); EXPECT_EQ( eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, - publisher_attributes_.qos.m_liveliness.kind); - EXPECT_EQ(0, publisher_attributes_.qos.m_lifespan.duration.seconds); - EXPECT_EQ(500000000u, publisher_attributes_.qos.m_lifespan.duration.nanosec); - EXPECT_EQ(0, publisher_attributes_.qos.m_deadline.period.seconds); - EXPECT_EQ(100000000u, publisher_attributes_.qos.m_deadline.period.nanosec); - EXPECT_EQ(10, publisher_attributes_.qos.m_liveliness.lease_duration.seconds); - EXPECT_EQ(0u, publisher_attributes_.qos.m_liveliness.lease_duration.nanosec); + publisher_qos_.liveliness().kind); + EXPECT_EQ(0, publisher_qos_.lifespan().duration.seconds); + EXPECT_EQ(500000000u, publisher_qos_.lifespan().duration.nanosec); + EXPECT_EQ(0, publisher_qos_.deadline().period.seconds); + EXPECT_EQ(100000000u, publisher_qos_.deadline().period.nanosec); + EXPECT_EQ(10, publisher_qos_.liveliness().lease_duration.seconds); + EXPECT_EQ(0u, publisher_qos_.liveliness().lease_duration.nanosec); EXPECT_EQ( eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, - publisher_attributes_.topic.historyQos.kind); - EXPECT_GE(10, publisher_attributes_.topic.historyQos.depth); + publisher_qos_.history().kind); + EXPECT_GE(10, publisher_qos_.history().depth); } TEST_F(GetDataWriterQoSTest, large_depth_conversion) { - size_t depth = publisher_attributes_.topic.historyQos.depth + 1; + size_t depth = publisher_qos_.history().depth + 1; qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, - publisher_attributes_.topic.historyQos.kind); - EXPECT_LE(depth, static_cast(publisher_attributes_.topic.historyQos.depth)); + publisher_qos_.history().kind); + EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); - using depth_type = decltype(publisher_attributes_.topic.historyQos.depth); + using depth_type = decltype(publisher_qos_.history().depth); constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_attributes_)); - EXPECT_LE(depth, static_cast(publisher_attributes_.topic.historyQos.depth)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_attributes_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); } } @@ -228,10 +228,10 @@ TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_attributes_)); - EXPECT_EQ(subscriber_attributes_.qos.m_lifespan.duration, InfiniteDuration); - EXPECT_EQ(subscriber_attributes_.qos.m_deadline.period, InfiniteDuration); - EXPECT_EQ(subscriber_attributes_.qos.m_liveliness.lease_duration, InfiniteDuration); + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); + EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); + EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); } TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) @@ -239,8 +239,8 @@ TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_attributes_)); - EXPECT_EQ(publisher_attributes_.qos.m_lifespan.duration, InfiniteDuration); - EXPECT_EQ(publisher_attributes_.qos.m_deadline.period, InfiniteDuration); - EXPECT_EQ(publisher_attributes_.qos.m_liveliness.lease_duration, InfiniteDuration); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_EQ(publisher_qos_.lifespan().duration, InfiniteDuration); + EXPECT_EQ(publisher_qos_.deadline().period, InfiniteDuration); + EXPECT_EQ(publisher_qos_.liveliness().lease_duration, InfiniteDuration); } From 780780d167f5d977c49789963434e6d0ac4024fc Mon Sep 17 00:00:00 2001 From: jparisu Date: Thu, 4 Feb 2021 09:41:20 +0100 Subject: [PATCH 05/60] uncrustify Signed-off-by: jparisu --- rmw_fastrtps_cpp/src/publisher.cpp | 14 +++---- rmw_fastrtps_cpp/src/rmw_client.cpp | 27 ++++++------- rmw_fastrtps_cpp/src/rmw_service.cpp | 27 ++++++------- rmw_fastrtps_cpp/src/subscription.cpp | 10 ++--- rmw_fastrtps_cpp/test/test_logging.cpp | 4 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 14 +++---- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 35 +++++++++-------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 38 +++++++++++-------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 10 ++--- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 11 ------ .../custom_participant_info.hpp | 4 +- .../custom_service_info.hpp | 4 +- .../custom_subscriber_info.hpp | 3 +- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 3 +- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 10 ++--- rmw_fastrtps_shared_cpp/src/participant.cpp | 15 +++++--- rmw_fastrtps_shared_cpp/src/publisher.cpp | 6 +-- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 8 ++-- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 8 ++-- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 4 +- rmw_fastrtps_shared_cpp/src/subscription.cpp | 6 +-- rmw_fastrtps_shared_cpp/src/utils.cpp | 23 ++++++----- 22 files changed, 147 insertions(+), 137 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 1832e1dc5..6595e789a 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -195,11 +195,11 @@ rmw_fastrtps_cpp::create_publisher( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); @@ -226,9 +226,9 @@ rmw_fastrtps_cpp::create_publisher( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c38927189..2447bf083 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -248,11 +248,11 @@ rmw_create_client( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - response_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + response_type_name, + topicQos); if (des_sub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create response topic"); @@ -265,18 +265,19 @@ rmw_create_client( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - request_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + request_type_name, + topicQos); if (des_pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create request topic"); return nullptr; } - eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + eprosima::fastdds::dds::Topic * pub_topic = + dynamic_cast(des_pub_topic); if (pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; @@ -343,9 +344,9 @@ rmw_create_client( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index e8ceb0385..2ff0376df 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -255,11 +255,11 @@ rmw_create_service( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - request_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + request_type_name, + topicQos); if (des_sub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create request topic"); @@ -272,18 +272,19 @@ rmw_create_service( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - response_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + response_type_name, + topicQos); if (des_pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create response topic"); return nullptr; } - eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + eprosima::fastdds::dds::Topic * pub_topic = + dynamic_cast(des_pub_topic); if (pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; @@ -351,9 +352,9 @@ rmw_create_service( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 571c9eb42..d9d43fac7 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -201,11 +201,11 @@ create_subscription( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); diff --git a/rmw_fastrtps_cpp/test/test_logging.cpp b/rmw_fastrtps_cpp/test/test_logging.cpp index eb25ebcdb..7bf706d54 100644 --- a/rmw_fastrtps_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_cpp/test/test_logging.cpp @@ -29,7 +29,9 @@ TEST(TestLogging, rmw_logging) EXPECT_EQ(eprosima::fastdds::dds::Log::Kind::Info, eprosima::fastdds::dds::Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastdds::dds::Log::Kind::Warning, eprosima::fastdds::dds::Log::GetVerbosity()); + EXPECT_EQ( + eprosima::fastdds::dds::Log::Kind::Warning, + eprosima::fastdds::dds::Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_EQ(eprosima::fastdds::dds::Log::Kind::Error, eprosima::fastdds::dds::Log::GetVerbosity()); diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 469a4fc82..23b90d2a2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -210,11 +210,11 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); @@ -241,9 +241,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 8529714d0..b8dff3cf0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -278,11 +278,11 @@ rmw_create_client( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - response_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + response_type_name, + topicQos); if (des_sub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create response topic"); @@ -295,18 +295,19 @@ rmw_create_client( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - request_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + request_type_name, + topicQos); if (des_pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create request topic"); return nullptr; } - eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + eprosima::fastdds::dds::Topic * pub_topic = + dynamic_cast(des_pub_topic); if (pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; @@ -370,9 +371,9 @@ rmw_create_client( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } @@ -509,12 +510,16 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto impl = static_cast(const_cast(info->request_type_support_impl_)); + auto impl = + static_cast(const_cast(info-> + request_type_support_impl_)); auto ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_request_type_support(ros_type_support); - impl = static_cast(const_cast(info->response_type_support_impl_)); + impl = + static_cast(const_cast(info-> + response_type_support_impl_)); ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_response_type_support(ros_type_support); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index c221f932c..97e11e605 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -287,11 +287,11 @@ rmw_create_service( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - request_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + sub_topic_name, + request_type_name, + topicQos); if (des_sub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create request topic"); @@ -304,18 +304,19 @@ rmw_create_service( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - response_type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + pub_topic_name, + response_type_name, + topicQos); if (des_pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed to create response topic"); return nullptr; } - eprosima::fastdds::dds::Topic * pub_topic = dynamic_cast(des_pub_topic); + eprosima::fastdds::dds::Topic * pub_topic = + dynamic_cast(des_pub_topic); if (pub_topic == nullptr) { RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); return nullptr; @@ -383,9 +384,9 @@ rmw_create_service( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { - if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS){ + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - }else if(participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } @@ -481,7 +482,8 @@ rmw_create_service( common_context->gid, node->name, node->namespace_); - return nullptr; } + return nullptr; + } } cleanup_rmw_service.cancel(); @@ -517,12 +519,16 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto impl = static_cast(const_cast(info->request_type_support_impl_)); + auto impl = + static_cast(const_cast(info-> + request_type_support_impl_)); auto ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_request_type_support(ros_type_support); - impl = static_cast(const_cast(info->response_type_support_impl_)); + impl = + static_cast(const_cast(info-> + response_type_support_impl_)); ros_type_support = static_cast(impl->ros_type_support()); type_registry.return_response_type_support(ros_type_support); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 89f928f7b..6021ccd6a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -213,11 +213,11 @@ create_subscription( // General function to create or get an already existing topic eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + rmw_fastrtps_shared_cpp::create_topic_rmw( + participant_info, + topic_name_mangled, + type_name, + topicQos); if (des_topic == nullptr) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index cfb41a57b..85a479879 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -84,17 +84,6 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType bool max_size_bound_; }; -// TODO eprosima -// inline void -// _unregister_type( -// eprosima::fastrtps::Participant * participant, -// TypeSupport * typed_typesupport) -// { -// if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { -// delete typed_typesupport; -// } -// } - } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index e170cf901..ac1e1cb63 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -60,7 +60,7 @@ typedef struct CustomParticipantInfo // Struct to store every topic created in the local domain // These variables needs to be - mutable std::vector topics_list_; + mutable std::vector topics_list_; mutable std::mutex topic_creation_mutex_; // Flag to establish if the QoS of the participant, @@ -146,7 +146,7 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList { if (is_alive) { rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; - rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); // TODO eprosima : is this a real QoS or is it Attributes + rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 0702aa359..9f716db49 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -102,7 +102,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } else if (info.current_count_change == -1) { eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); subscriptions_.erase(erase_endpoint_guid); auto endpoint = clients_endpoints_.find(erase_endpoint_guid); if (endpoint != clients_endpoints_.end()) { @@ -175,7 +175,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener std::condition_variable cv_; }; -class ServiceListener : public eprosima::fastdds::dds::DataReaderListener +class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: explicit ServiceListener(CustomServiceInfo * info) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 6f83e504e..1b9c9e364 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -134,7 +134,6 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds void update_unread_count(eprosima::fastdds::dds::DataReader * reader) { - // TODO (eprosima) // Make sure to call into Fast-RTPS before taking the lock to avoid an // ABBA deadlock between internalMutex_ and mutexes inside of Fast-RTPS. eprosima::fastdds::dds::SampleInfo info; @@ -142,7 +141,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds // In case there is data, get_first_untaken_info return OK. Else it returs NO_DATA bool has_data = false; - if (has_data_ret == ReturnCode_t::RETCODE_OK){ + if (has_data_ret == ReturnCode_t::RETCODE_OK) { has_data = true; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index e44d3eeea..3dec499f9 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -62,7 +62,8 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); */ template void -dds_qos_to_rmw_qos( // TODO (eprosima) not in use +dds_qos_to_rmw_qos( + // TODO (eprosima) not in use const DDSQoSPolicyT & dds_qos, rmw_qos_profile_t * qos) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 291897178..9b16f30a9 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -34,11 +34,11 @@ namespace rmw_fastrtps_shared_cpp rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); -eprosima::fastdds::dds::TopicDescription* create_topic_rmw( - const CustomParticipantInfo * participant_info, - const std::string& topic_name, - const std::string& type_name, - const eprosima::fastdds::dds::TopicQos& qos); +eprosima::fastdds::dds::TopicDescription * create_topic_rmw( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & qos); } // rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 663e4ff86..c8ff49a17 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -116,7 +116,8 @@ __create_participant( ///// // Create Participant - participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( + participant_info->participant_ = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( domain_id, domainParticipantQos, participant_info->listener_); if (!participant_info->participant_) { @@ -137,7 +138,8 @@ __create_participant( ///// // Create Publisher - eprosima::fastdds::dds::PublisherQos publisherQos = participant_info->participant_->get_default_publisher_qos(); + eprosima::fastdds::dds::PublisherQos publisherQos = + participant_info->participant_->get_default_publisher_qos(); publisherQos.entity_factory(domainParticipantQos.entity_factory()); participant_info->publisher_ = participant_info->participant_->create_publisher(publisherQos); @@ -154,7 +156,8 @@ __create_participant( ///// // Create Subscriber - eprosima::fastdds::dds::SubscriberQos subscriberQos = participant_info->participant_->get_default_subscriber_qos(); + eprosima::fastdds::dds::SubscriberQos subscriberQos = + participant_info->participant_->get_default_subscriber_qos(); subscriberQos.entity_factory(domainParticipantQos.entity_factory()); participant_info->subscriber_ = participant_info->participant_->create_subscriber(subscriberQos); @@ -327,8 +330,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant } // Topics must be deleted before delete the participant - for (auto topic : participant_info->topics_list_) - { + for (auto topic : participant_info->topics_list_) { ReturnCode_t ret = participant_info->participant_->delete_topic(topic); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete participant"); @@ -337,7 +339,8 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant } // Delete Domain Participant - ReturnCode_t ret = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + ReturnCode_t ret = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( participant_info->participant_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete participant"); diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index aa0ff3e55..18aec29a9 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -44,7 +44,7 @@ destroy_publisher( auto info = static_cast(publisher->data); // NOTE: Topic deletion and unregister type is done in the participant - if (nullptr != info){ + if (nullptr != info) { // Delete DataWriter ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->publisher_); if (ret != ReturnCode_t::RETCODE_OK) { @@ -53,7 +53,7 @@ destroy_publisher( } // Delete DataWriter listener - if (nullptr != info->listener_){ + if (nullptr != info->listener_) { delete info->listener_; } @@ -64,7 +64,7 @@ destroy_publisher( // Delete PublisherInfo structure delete info; - }else{ + } else { ret = RMW_RET_INVALID_ARGUMENT; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 19f8380a2..43a0ebc1d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -69,7 +69,7 @@ __rmw_destroy_client( static_cast(node->context->impl->participant_info); // NOTE: Topic deletion and unregister type is done in the participant - if (nullptr != info){ + if (nullptr != info) { // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_subscriber_); @@ -79,7 +79,7 @@ __rmw_destroy_client( } // Delete DataReader listener - if (nullptr != info->listener_){ + if (nullptr != info->listener_) { delete info->listener_; } @@ -91,7 +91,7 @@ __rmw_destroy_client( } // Delete DataWriter listener - if (nullptr != info->pub_listener_){ + if (nullptr != info->pub_listener_) { delete info->pub_listener_; } @@ -107,7 +107,7 @@ __rmw_destroy_client( // Delete ClientInfo structure delete info; - }else{ + } else { final_ret = RMW_RET_INVALID_ARGUMENT; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 11ca6fa53..f453a560e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -82,7 +82,7 @@ __rmw_destroy_service( static_cast(node->context->impl->participant_info); // NOTE: Topic deletion and unregister type is done in the participant - if (nullptr != info){ + if (nullptr != info) { // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_subscriber_); @@ -92,7 +92,7 @@ __rmw_destroy_service( } // Delete DataReader listener - if (nullptr != info->listener_){ + if (nullptr != info->listener_) { delete info->listener_; } @@ -104,7 +104,7 @@ __rmw_destroy_service( } // Delete DataWriter listener - if (nullptr != info->pub_listener_){ + if (nullptr != info->pub_listener_) { delete info->pub_listener_; } @@ -120,7 +120,7 @@ __rmw_destroy_service( // Delete ServiceInfo structure delete info; - }else{ + } else { final_ret = RMW_RET_INVALID_ARGUMENT; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index c88be5183..7299ad7f8 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -126,13 +126,13 @@ _take_sequence( } ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); - static_cast (take_ret); + static_cast(take_ret); // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); for (size_t ii = 0; ii < info_seq.length(); ++ii) { - if (info_seq[ii].valid_data){ + if (info_seq[ii].valid_data) { if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == info_seq[ii].instance_state) { if (message_info_sequence->data + *taken) { _assign_message_info(identifier, message_info_sequence->data + *taken, &info_seq[ii]); diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index b8dc79cb0..3676838ac 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -44,7 +44,7 @@ destroy_subscription( auto info = static_cast(subscription->data); // NOTE: Topic deletion and unregister type is done in the participant - if (nullptr != info){ + if (nullptr != info) { // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->subscriber_); if (ret != ReturnCode_t::RETCODE_OK) { @@ -53,7 +53,7 @@ destroy_subscription( } // Delete DataReader listener - if (nullptr != info->listener_){ + if (nullptr != info->listener_) { delete info->listener_; } @@ -64,7 +64,7 @@ destroy_subscription( // Delete SubscriberInfo structure delete info; - }else{ + } else { ret = RMW_RET_INVALID_ARGUMENT; } diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index ff36c5c9f..ac3e35d6f 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -44,25 +44,28 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) } } -eprosima::fastdds::dds::TopicDescription* create_topic_rmw( - const CustomParticipantInfo * participant_info, - const std::string& topic_name, - const std::string& type_name, - const eprosima::fastdds::dds::TopicQos& qos) +eprosima::fastdds::dds::TopicDescription * create_topic_rmw( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & qos) { // This block will lock the topic creations in this participant - std::lock_guard lck (participant_info->topic_creation_mutex_); + std::lock_guard lck(participant_info->topic_creation_mutex_); // Searchs for an already existing topic - eprosima::fastdds::dds::TopicDescription* des_topic = participant_info->participant_->lookup_topicdescription(topic_name); - if (nullptr != des_topic) - { + eprosima::fastdds::dds::TopicDescription * des_topic = + participant_info->participant_->lookup_topicdescription(topic_name); + if (nullptr != des_topic) { // TODO the already existing topic type must fit with the desired type for new topic return des_topic; } // Creates topic - eprosima::fastdds::dds::Topic* topic = participant_info->participant_->create_topic(topic_name, type_name, qos); + eprosima::fastdds::dds::Topic * topic = participant_info->participant_->create_topic( + topic_name, + type_name, + qos); participant_info->topics_list_.push_back(topic); return topic; } From bd69cbcc5ef5c2a38bdeb879287a3aad1ebf0aa4 Mon Sep 17 00:00:00 2001 From: jparisu Date: Fri, 5 Feb 2021 08:00:36 +0100 Subject: [PATCH 06/60] rmw fastrtps clean state Signed-off-by: jparisu --- rmw_fastrtps_cpp/src/publisher.cpp | 3 +++ rmw_fastrtps_cpp/src/rmw_publisher.cpp | 1 + .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 5 +++-- .../custom_client_info.hpp | 5 ++--- .../custom_service_info.hpp | 8 ++------ .../rmw_fastrtps_shared_cpp/participant.hpp | 3 ++- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 1 - .../include/rmw_fastrtps_shared_cpp/utils.hpp | 3 ++- .../src/TypeSupport_impl.cpp | 5 +++-- rmw_fastrtps_shared_cpp/src/participant.cpp | 17 ++++++++++++++++- rmw_fastrtps_shared_cpp/src/qos.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 1 - rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 2 -- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 4 +--- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 1 - rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 10 +++------- rmw_fastrtps_shared_cpp/src/utils.cpp | 5 ++++- 17 files changed, 44 insertions(+), 34 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 6595e789a..e3df50b82 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -85,6 +85,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Check ROS QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -159,6 +160,7 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + RMW_SET_ERROR_MSG("Error registering type in publisher"); return nullptr; } @@ -190,6 +192,7 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 81ad0c1e7..8603c6605 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -86,6 +86,7 @@ rmw_create_publisher( true); if (!publisher) { + RMW_SET_ERROR_MSG("Publisher creation failed"); return nullptr; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 85a479879..8d715bd65 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -15,13 +15,14 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ +#include +#include + #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDataType.hpp" #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include -#include #include "rcutils/logging_macros.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 37d8748cb..ec94d01ad 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include "fastcdr/FastBuffer.h" @@ -54,9 +55,7 @@ typedef struct CustomClientInfo ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; - // eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; - // eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; - // eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + const char * typesupport_identifier_{nullptr}; ClientPubListener * pub_listener_{nullptr}; std::atomic_size_t response_subscriber_matched_count_; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 9f716db49..18901b55e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -55,14 +55,10 @@ typedef struct CustomServiceInfo const void * response_type_support_impl_{nullptr}; eprosima::fastdds::dds::DataReader * request_subscriber_{nullptr}; eprosima::fastdds::dds::DataWriter * response_publisher_{nullptr}; - //eprosima::fastdds::dds::Topic * response_topic_{nullptr}; - //eprosima::fastdds::dds::Topic * request_topic_{nullptr}; ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; - // eprosima::fastdds::dds::DomainParticipant * domainParticipant_{nullptr}; - // eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; - // eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; @@ -87,7 +83,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener rmw_fastrtps_shared_cpp::hash_fastrtps_guid>; public: - ServicePubListener(CustomServiceInfo * info) + explicit ServicePubListener(CustomServiceInfo * info) { (void) info; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 4cc4438b6..7d851814d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -26,7 +26,8 @@ namespace rmw_fastrtps_shared_cpp { // The Entities Publisher and Subscriber are created with the Participant -// Endpoint SubEntities as DataWriter and DataReader are created with create_publisher and create_subscription +// Endpoint SubEntities as DataWriter and DataReader are created with +// create_publisher and create_subscription RMW_FASTRTPS_SHARED_CPP_PUBLIC CustomParticipantInfo * create_participant( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index 3dec499f9..c95373499 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -63,7 +63,6 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); template void dds_qos_to_rmw_qos( - // TODO (eprosima) not in use const DDSQoSPolicyT & dds_qos, rmw_qos_profile_t * qos) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 9b16f30a9..f1999a335 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -16,6 +16,7 @@ #define RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ #include +#include #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDescription.hpp" @@ -40,6 +41,6 @@ eprosima::fastdds::dds::TopicDescription * create_topic_rmw( const std::string & type_name, const eprosima::fastdds::dds::TopicQos & qos); -} // rmw_fastrtps_shared_cpp +} // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index c5a3c24d9..ce1b14929 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastcdr/FastBuffer.h" -#include "fastcdr/Cdr.h" #include #include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index c8ff49a17..c6c49398e 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -338,10 +338,24 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant } } + // Remove publisher and subcriber from participant + ReturnCode_t ret = participant_info->participant_->delete_publisher(participant_info->publisher_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete dds publisher from participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + + ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete dds subscriber from participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } + // Delete Domain Participant - ReturnCode_t ret = + ret = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( participant_info->participant_); + if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete participant"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -354,5 +368,6 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant delete participant_info; RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 63ea48732..faf08afa9 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -105,7 +105,7 @@ bool fill_entity_qos_from_profile( } // ensure the history depth is at least the requested queue size - assert(entity_qos.history().depth >= 0); // TODO eprosima should not be after initialization? + assert(entity_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && static_cast(entity_qos.history().depth) < qos_policies.depth) @@ -190,7 +190,7 @@ get_topic_qos( } // ensure the history depth is at least the requested queue size - assert(topic_qos.history().depth >= 0); // TODO eprosima should not be after initialization? + assert(topic_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && static_cast(topic_qos.history().depth) < qos_policies.depth) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 43a0ebc1d..d26fb2995 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -70,7 +70,6 @@ __rmw_destroy_client( // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { - // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_subscriber_); if (ret != ReturnCode_t::RETCODE_OK) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index ae98932d8..59dc5cf8f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -105,8 +105,6 @@ __rmw_take_request( ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); - // TODO eprosima - // request_header->received_timestamp = request.sample_info_.received_timestamp.to_ns(); request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); *taken = true; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index c89b3ce4c..4d107a77d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -63,9 +63,7 @@ __rmw_take_response( deser, ros_response, info->response_type_support_impl_)) { request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); - // TODO eprosima - // request_header->received_timestamp = request.sample_info_.received_timestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); request_header->request_id.sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index f453a560e..a53105de6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -83,7 +83,6 @@ __rmw_destroy_service( // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { - // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_subscriber_); if (ret != ReturnCode_t::RETCODE_OK) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 7299ad7f8..2d8745f4d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -36,9 +36,7 @@ _assign_message_info( const eprosima::fastdds::dds::SampleInfo * sinfo) { message_info->source_timestamp = sinfo->source_timestamp.to_ns(); - // TODO (eprosima) - // message_info->received_timestamp = sinfo->receive_timestamp.to_ns(); - message_info->received_timestamp = sinfo->source_timestamp.to_ns(); + message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); rmw_gid_t * sender_gid = &message_info->publisher_gid; sender_gid->implementation_identifier = identifier; memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); @@ -76,7 +74,6 @@ _take( data.data = ros_message; data.impl = info->type_support_impl_; if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); @@ -101,8 +98,8 @@ _take_sequence( size_t * taken, rmw_subscription_allocation_t * allocation) { + (void) allocation; *taken = 0; - bool taken_flag = false; rmw_ret_t ret = RMW_RET_OK; RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -131,7 +128,7 @@ _take_sequence( // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); - for (size_t ii = 0; ii < info_seq.length(); ++ii) { + for (size_t ii = 0; ii < static_cast(info_seq.length()); ++ii) { if (info_seq[ii].valid_data) { if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == info_seq[ii].instance_state) { if (message_info_sequence->data + *taken) { @@ -291,7 +288,6 @@ _take_serialized_message( data.impl = nullptr; // not used when is_cdr_buffer is true if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index ac3e35d6f..7d0ba04c3 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw/rmw.h" @@ -57,7 +59,8 @@ eprosima::fastdds::dds::TopicDescription * create_topic_rmw( eprosima::fastdds::dds::TopicDescription * des_topic = participant_info->participant_->lookup_topicdescription(topic_name); if (nullptr != des_topic) { - // TODO the already existing topic type must fit with the desired type for new topic + // If this is not an instantiated object of type Topic, it cannot be used to + // create a new DataWriter and though the publisher creation will fail return des_topic; } From 0da237ba5880d7e51264c3bcc5594e9347a7076d Mon Sep 17 00:00:00 2001 From: jparisu Date: Wed, 10 Feb 2021 12:47:34 +0100 Subject: [PATCH 07/60] fix bugs in client, participant and take Signed-off-by: jparisu --- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 11 ++++++----- rmw_fastrtps_shared_cpp/src/participant.cpp | 15 ++++++++++++++- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 14 ++++++++++++-- 3 files changed, 32 insertions(+), 8 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index b8dff3cf0..8a5a8d1c6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -153,6 +153,7 @@ rmw_create_client( }); // info->participant_ = domainParticipant; + info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; @@ -171,7 +172,6 @@ rmw_create_client( auto response_type_impl = type_registry.get_response_type_support(type_support); if (!response_type_impl) { - type_registry.return_request_type_support(type_support); RMW_SET_ERROR_MSG("failed to allocate response type support"); return nullptr; } @@ -208,10 +208,8 @@ rmw_create_client( const void * untyped_request_members; const void * untyped_response_members; - untyped_request_members = - get_request_ptr(type_support->data, info->typesupport_identifier_); - untyped_response_members = get_response_ptr( - type_support->data, info->typesupport_identifier_); + untyped_request_members = get_request_ptr(type_support->data, info->typesupport_identifier_); + untyped_response_members = get_response_ptr(type_support->data, info->typesupport_identifier_); std::string request_type_name = _create_type_name( untyped_request_members, info->typesupport_identifier_); @@ -313,6 +311,9 @@ rmw_create_client( return nullptr; } + info->request_topic_ = pub_topic_name; + info->response_topic_ = sub_topic_name; + // Key word to find DataWrtier and DataReader QoS std::string topic_name_fallback = "client"; diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index c6c49398e..1035f32a1 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -114,11 +114,24 @@ __create_participant( delete participant_info->listener_; }); + ///// + // Force to load XML configuration file + if (ReturnCode_t::RETCODE_OK != eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles()) + { + RMW_SET_ERROR_MSG("__create_participant failed to load FastDDS XML configuration file"); + } + ///// // Create Participant + + // Create Participant Listener mask so it only uses the domain participant listener non inheritated callbacks + // Without this mask, callback of DataReaders on_data_available is never called + // Publisher and subscriber dont need masks as they do not have listeners + eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); + participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( - domain_id, domainParticipantQos, participant_info->listener_); + domain_id, domainParticipantQos, participant_info->listener_, participant_mask); if (!participant_info->participant_) { RMW_SET_ERROR_MSG("__create_participant failed to create participant"); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 2d8745f4d..d93e8e930 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -26,6 +26,7 @@ #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" namespace rmw_fastrtps_shared_cpp { @@ -111,10 +112,13 @@ _take_sequence( RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); // Take every message available (Maximum number of messages) - eprosima::fastdds::dds::LoanableSequence info_seq; + eprosima::fastdds::dds::LoanableSequence info_seq(count); // Datas must be initialized before calling take eprosima::fastdds::dds::LoanableSequence data_seq(count); + // Work around to iniziatlize data. If length < i FastDDS throws an error + data_seq.length(count); + // Initialize all RMW Type Support Data to send sequence to take for (size_t i = 0; i < count; ++i) { data_seq[i].is_cdr_buffer = false; @@ -122,8 +126,14 @@ _take_sequence( data_seq[i].impl = info->type_support_impl_; } + // After initialize all the data, length returns to 0 to add samples in first places of data_seq + data_seq.length(0); + ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); - static_cast(take_ret); + if (take_ret != ReturnCode_t::RETCODE_OK) + { + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(take_ret); + } // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); From 0ecc2091692de61bab91adaf7b2ec782deeb5cd8 Mon Sep 17 00:00:00 2001 From: jparisu Date: Wed, 10 Feb 2021 13:25:03 +0100 Subject: [PATCH 08/60] fix cpplint and uncrustify Signed-off-by: jparisu --- .../rmw_fastrtps_cpp/ServiceTypeSupport.hpp | 3 +- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 5 +-- .../include/rmw_fastrtps_cpp/publisher.hpp | 2 +- .../include/rmw_fastrtps_cpp/subscription.hpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 10 +++--- rmw_fastrtps_cpp/src/rmw_client.cpp | 29 ++++++++++------ rmw_fastrtps_cpp/src/rmw_service.cpp | 28 +++++++++------ rmw_fastrtps_cpp/src/subscription.cpp | 13 ++++--- .../MessageTypeSupport.hpp | 6 ++-- .../MessageTypeSupport_impl.hpp | 6 ++-- .../ServiceTypeSupport.hpp | 3 +- .../ServiceTypeSupport_impl.hpp | 5 +-- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 5 +-- .../TypeSupport_impl.hpp | 3 ++ .../rmw_fastrtps_dynamic_cpp/publisher.hpp | 2 +- .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 6 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 22 +++++++----- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 34 +++++++++++-------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 10 +++--- rmw_fastrtps_shared_cpp/src/participant.cpp | 6 ++-- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 3 +- 22 files changed, 126 insertions(+), 79 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp index fb38863c1..7284bd4f5 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp @@ -15,9 +15,10 @@ #ifndef RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ +#include + #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index b465a5f68..1a9a52bf7 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -15,13 +15,14 @@ #ifndef RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ +#include +#include + #include "rosidl_runtime_c/string.h" #include "rosidl_runtime_c/string_functions.h" #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" -#include -#include #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp index dae92d2af..a569d5595 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // unused + bool keyed, // unused bool create_publisher_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp index 23c2b09b7..8f66b56fa 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, // unused + bool keyed, // unused bool create_subscription_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index e3df50b82..84789302b 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -154,11 +154,12 @@ rmw_fastrtps_cpp::create_publisher( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { RMW_SET_ERROR_MSG("Error registering type in publisher"); return nullptr; @@ -224,7 +225,8 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); // Modify specific DataWriter Qos diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 2447bf083..933e61c48 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -192,18 +192,23 @@ rmw_create_client( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, + // so it is dangerous to keep using it. + // Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) RequestTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) RequestTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) ResponseTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -297,10 +302,12 @@ rmw_create_client( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); @@ -335,10 +342,12 @@ rmw_create_client( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 2ff0376df..2ce92f58b 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -200,18 +200,22 @@ rmw_create_service( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) RequestTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) RequestTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) ResponseTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) ResponseTypeSupport_cpp(service_members))); + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -301,10 +305,12 @@ rmw_create_service( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); @@ -343,10 +349,12 @@ rmw_create_service( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index d9d43fac7..1ddb29e99 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -159,11 +159,13 @@ create_subscription( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + eprosima::fastdds::dds::TypeSupport( + new (std::nothrow) MessageTypeSupport_cpp(callbacks))); + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -221,7 +223,8 @@ create_subscription( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp index feb25f590..67d8a72b8 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp @@ -15,12 +15,12 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ -#include "fastcdr/FastBuffer.h" -#include "fastcdr/Cdr.h" - #include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "TypeSupport.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp index 8414b903b..298e2485f 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -15,14 +15,14 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ -#include "fastcdr/FastBuffer.h" -#include "fastcdr/Cdr.h" - #include #include #include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "rcpputils/find_and_replace.hpp" #include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp index 1c7f42d69..c4ac036d7 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -15,9 +15,10 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ +#include + #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include #include "TypeSupport.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp index 63e7060c0..b14959bfb 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -15,12 +15,13 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ -#include "fastcdr/FastBuffer.h" -#include "fastcdr/Cdr.h" #include #include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "rcpputils/find_and_replace.hpp" #include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp index 0842b168c..270839275 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -15,13 +15,14 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ +#include +#include + #include "rosidl_runtime_c/string.h" #include "rosidl_runtime_c/string_functions.h" #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include -#include #include "rcutils/logging_macros.h" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index 892fc1cbb..90c679aef 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -23,6 +23,9 @@ #include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" #include "rmw_fastrtps_dynamic_cpp/macros.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp index 8b54e1423..4d419346c 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, //unused + bool keyed, // unused bool create_publisher_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp index 1b5ceaf0a..3113ca04c 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, //unused + bool keyed, // unused bool create_subscription_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 23b90d2a2..cea5f8c75 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -172,7 +172,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -236,7 +237,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); // Modify specific DataWriter Qos diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 8a5a8d1c6..37dea7eb4 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -218,12 +218,13 @@ rmw_create_client( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport( new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -231,7 +232,8 @@ rmw_create_client( ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport( new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -325,10 +327,12 @@ rmw_create_client( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); @@ -363,10 +367,12 @@ rmw_create_client( // with profile_name "client" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "client", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 97e11e605..ca630cfb1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -230,12 +230,13 @@ rmw_create_service( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport( new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -243,7 +244,8 @@ rmw_create_service( ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport( new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -252,7 +254,7 @@ rmw_create_service( // Create Listeners info->listener_ = new (std::nothrow) ServiceListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + RMW_SET_ERROR_MSG("failed to create service response subscriber listener"); return nullptr; } @@ -263,7 +265,7 @@ rmw_create_service( info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + RMW_SET_ERROR_MSG("failed to create service request publisher listener"); return nullptr; } @@ -330,13 +332,15 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // with profile_name "service" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "service", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); @@ -356,7 +360,7 @@ rmw_create_service( info->listener_); if (!info->request_subscriber_) { - RMW_SET_ERROR_MSG("failed to create client request data reader"); + RMW_SET_ERROR_MSG("failed to create service request data reader"); return nullptr; } @@ -372,13 +376,15 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // with profile_name "service" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - // Try to load the profile named "client", if it does not exist it tryes with the request topic name + // Try to load the profile named "service", + // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: - // If none exist is default, if only one exists is the one chosen, if both exist topic name is chosen + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); @@ -405,7 +411,7 @@ rmw_create_service( info->pub_listener_); if (!info->response_publisher_) { - RMW_SET_ERROR_MSG("failed to create client request data writer"); + RMW_SET_ERROR_MSG("failed to create service request data writer"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 6021ccd6a..2128d1df0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -171,11 +171,12 @@ create_subscription( ///// // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is dangerous to keep - // using it. Thus we use a new TypeSupport created only to register it. + // When a type is registered in a participant, it is converted to a shared_ptr, so it is + // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. ReturnCode_t ret = domainParticipant->register_type( eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_support_impl))); - // Register could fail if there is already a type with that name in participant, so not only OK retcode is possible + // Register could fail if there is already a type with that name in participant, + // so not only OK retcode is possible if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { return nullptr; } @@ -233,7 +234,8 @@ create_subscription( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, the QoS is already the default + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 1035f32a1..f0eb16215 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -116,7 +116,8 @@ __create_participant( ///// // Force to load XML configuration file - if (ReturnCode_t::RETCODE_OK != eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles()) + if (ReturnCode_t::RETCODE_OK != + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles()) { RMW_SET_ERROR_MSG("__create_participant failed to load FastDDS XML configuration file"); } @@ -124,7 +125,8 @@ __create_participant( ///// // Create Participant - // Create Participant Listener mask so it only uses the domain participant listener non inheritated callbacks + // Create Participant Listener mask so it only uses the domain + // participant listener non inheritated callbacks // Without this mask, callback of DataReaders on_data_available is never called // Publisher and subscriber dont need masks as they do not have listeners eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index d93e8e930..5933ec918 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -130,8 +130,7 @@ _take_sequence( data_seq.length(0); ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); - if (take_ret != ReturnCode_t::RETCODE_OK) - { + if (take_ret != ReturnCode_t::RETCODE_OK) { return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(take_ret); } From f5326014f9c28a7bff8eb106a49cc9159d512d41 Mon Sep 17 00:00:00 2001 From: jparisu Date: Wed, 10 Feb 2021 17:15:32 +0100 Subject: [PATCH 09/60] construct Lonable Sequence to avoid heap allocations Signed-off-by: jparisu --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 46 ++++++++++++++++++------ 1 file changed, 35 insertions(+), 11 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 5933ec918..812d21cea 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -30,6 +30,26 @@ namespace rmw_fastrtps_shared_cpp { + +class ConstructibleLoanableSequence : + public eprosima::fastdds::dds::LoanableSequence +{ +public: + ConstructibleLoanableSequence( + size_type count, + rmw_fastrtps_shared_cpp::SerializedData ** serialize_data) + { + maximum_ = count; + elements_ = reinterpret_cast(serialize_data); + } + + ~ConstructibleLoanableSequence() + { + maximum_ = 0; + elements_ = nullptr; + } +}; + void _assign_message_info( const char * identifier, @@ -113,21 +133,25 @@ _take_sequence( // Take every message available (Maximum number of messages) eprosima::fastdds::dds::LoanableSequence info_seq(count); - // Datas must be initialized before calling take - eprosima::fastdds::dds::LoanableSequence data_seq(count); - - // Work around to iniziatlize data. If length < i FastDDS throws an error - data_seq.length(count); - // Initialize all RMW Type Support Data to send sequence to take + // Datas must be initialized before calling take + // Two vectors are required because we want to avoid using new + // and [] type are not recomendded + std::vector serialized_data(count); + std::vector serialized_data_p(count); for (size_t i = 0; i < count; ++i) { - data_seq[i].is_cdr_buffer = false; - data_seq[i].data = message_sequence->data[i]; - data_seq[i].impl = info->type_support_impl_; + serialized_data[i].is_cdr_buffer = false; + serialized_data[i].data = message_sequence->data[i]; + serialized_data[i].impl = info->type_support_impl_; + serialized_data_p[i] = &serialized_data[i]; } - // After initialize all the data, length returns to 0 to add samples in first places of data_seq - data_seq.length(0); + // Creates specific Lonable collection to avoid heap allocs + rmw_fastrtps_shared_cpp::ConstructibleLoanableSequence data_seq( + count, + serialized_data_p.data()); + + rmw_fastrtps_shared_cpp::SerializedData * sd = reinterpret_cast(data_seq.buffer()[0]); ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); if (take_ret != ReturnCode_t::RETCODE_OK) { From d7cac45afd9a3897abca20e1e9298eaf79be13b7 Mon Sep 17 00:00:00 2001 From: jparisu Date: Wed, 10 Feb 2021 17:20:45 +0100 Subject: [PATCH 10/60] uncrustify Signed-off-by: jparisu --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 812d21cea..2a856472a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/serialized_message.h" @@ -31,16 +33,16 @@ namespace rmw_fastrtps_shared_cpp { -class ConstructibleLoanableSequence : - public eprosima::fastdds::dds::LoanableSequence +class ConstructibleLoanableSequence + : public eprosima::fastdds::dds::LoanableSequence { public: ConstructibleLoanableSequence( - size_type count, - rmw_fastrtps_shared_cpp::SerializedData ** serialize_data) + size_type count, + rmw_fastrtps_shared_cpp::SerializedData ** serialize_data) { maximum_ = count; - elements_ = reinterpret_cast(serialize_data); + elements_ = reinterpret_cast(serialize_data); } ~ConstructibleLoanableSequence() @@ -138,7 +140,7 @@ _take_sequence( // Two vectors are required because we want to avoid using new // and [] type are not recomendded std::vector serialized_data(count); - std::vector serialized_data_p(count); + std::vector serialized_data_p(count); for (size_t i = 0; i < count; ++i) { serialized_data[i].is_cdr_buffer = false; serialized_data[i].data = message_sequence->data[i]; @@ -151,7 +153,8 @@ _take_sequence( count, serialized_data_p.data()); - rmw_fastrtps_shared_cpp::SerializedData * sd = reinterpret_cast(data_seq.buffer()[0]); + rmw_fastrtps_shared_cpp::SerializedData * sd = + reinterpret_cast(data_seq.buffer()[0]); ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); if (take_ret != ReturnCode_t::RETCODE_OK) { From abff9e44ea7281337765d82ae77aa9b0fab8968f Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 4 Mar 2021 10:50:28 +0100 Subject: [PATCH 11/60] Use valid_data instead of instance_state. Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_client_info.hpp | 3 +-- .../rmw_fastrtps_shared_cpp/custom_service_info.hpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 12 +++++------- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index ec94d01ad..145090b59 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -91,8 +91,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true if (reader->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (eprosima::fastdds::dds::InstanceStateKind::ALIVE_INSTANCE_STATE == - response.sample_info_.instance_state) + if (response.sample_info_.valid_data) { response.sample_identity_ = response.sample_info_.related_sample_identity; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 18901b55e..145b6e160 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -206,7 +206,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener data.data = request.buffer_; data.impl = nullptr; // not used when is_cdr_buffer is true if (reader->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == request.sample_info_.instance_state) { + if (request.sample_info_.valid_data) { request.sample_identity_ = request.sample_info_.sample_identity; // Use response subscriber guid (on related_sample_identity) when present. const eprosima::fastrtps::rtps::GUID_t & reader_guid = diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 2a856472a..35f789de1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -100,7 +100,7 @@ _take( // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); - if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == sinfo.instance_state) { + if (sinfo.valid_data) { if (message_info) { _assign_message_info(identifier, message_info, &sinfo); } @@ -166,12 +166,10 @@ _take_sequence( for (size_t ii = 0; ii < static_cast(info_seq.length()); ++ii) { if (info_seq[ii].valid_data) { - if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == info_seq[ii].instance_state) { - if (message_info_sequence->data + *taken) { - _assign_message_info(identifier, message_info_sequence->data + *taken, &info_seq[ii]); - } - (*taken)++; + if (message_info_sequence->data + *taken) { + _assign_message_info(identifier, message_info_sequence->data + *taken, &info_seq[ii]); } + (*taken)++; } } @@ -327,7 +325,7 @@ _take_serialized_message( // Update hasData from listener info->listener_->update_unread_count(info->subscriber_); - if (eprosima::fastdds::dds::ALIVE_INSTANCE_STATE == sinfo.instance_state) { + if (sinfo.valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); if (serialized_message->buffer_capacity < buffer_size) { auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); From 13918dc0cd99a62418c238bc0d99344c6dab266a Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 4 Mar 2021 12:25:36 +0100 Subject: [PATCH 12/60] Fix build with security enabled. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index f0eb16215..c9fd5bf3e 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -315,7 +315,7 @@ rmw_fastrtps_shared_cpp::create_participant( return nullptr; } - participantAttrs.rtps.properties = property_policy; + domainParticipantQos.properties(property_policy); } else if (security_options->enforce_security) { RMW_SET_ERROR_MSG("couldn't find all security files!"); return nullptr; From 290836c682cbb2f31903f6a75fe6e7d4b2b449fd Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 4 Mar 2021 12:31:04 +0100 Subject: [PATCH 13/60] Solve link problems on Windows. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index f1999a335..7cf162c8c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -33,9 +33,13 @@ namespace rmw_fastrtps_shared_cpp { -rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); -eprosima::fastdds::dds::TopicDescription * create_topic_rmw( +RMW_FASTRTPS_SHARED_CPP_PUBLIC +eprosima::fastdds::dds::TopicDescription * +create_topic_rmw( const CustomParticipantInfo * participant_info, const std::string & topic_name, const std::string & type_name, From 50b53573829a40921ad11cc329fad3d5ba108569 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 5 Mar 2021 07:49:25 +0100 Subject: [PATCH 14/60] Restore old rmw_take_sequence code. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 68 ++++++------------------ 1 file changed, 16 insertions(+), 52 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 35f789de1..85427b661 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -33,25 +33,6 @@ namespace rmw_fastrtps_shared_cpp { -class ConstructibleLoanableSequence - : public eprosima::fastdds::dds::LoanableSequence -{ -public: - ConstructibleLoanableSequence( - size_type count, - rmw_fastrtps_shared_cpp::SerializedData ** serialize_data) - { - maximum_ = count; - elements_ = reinterpret_cast(serialize_data); - } - - ~ConstructibleLoanableSequence() - { - maximum_ = 0; - elements_ = nullptr; - } -}; - void _assign_message_info( const char * identifier, @@ -121,8 +102,8 @@ _take_sequence( size_t * taken, rmw_subscription_allocation_t * allocation) { - (void) allocation; *taken = 0; + bool taken_flag = false; rmw_ret_t ret = RMW_RET_OK; RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -133,42 +114,25 @@ _take_sequence( CustomSubscriberInfo * info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - // Take every message available (Maximum number of messages) - eprosima::fastdds::dds::LoanableSequence info_seq(count); - - // Datas must be initialized before calling take - // Two vectors are required because we want to avoid using new - // and [] type are not recomendded - std::vector serialized_data(count); - std::vector serialized_data_p(count); - for (size_t i = 0; i < count; ++i) { - serialized_data[i].is_cdr_buffer = false; - serialized_data[i].data = message_sequence->data[i]; - serialized_data[i].impl = info->type_support_impl_; - serialized_data_p[i] = &serialized_data[i]; + // Limit the upper bound of reads to the number unread at the beginning. + // This prevents any samples that are added after the beginning of the + // _take_sequence call from being read. + auto unread_count = info->subscriber_->get_unread_count(); + if (unread_count < count) { + count = unread_count; } - // Creates specific Lonable collection to avoid heap allocs - rmw_fastrtps_shared_cpp::ConstructibleLoanableSequence data_seq( - count, - serialized_data_p.data()); + for (size_t ii = 0; ii < count; ++ii) { + taken_flag = false; + ret = _take( + identifier, subscription, message_sequence->data[*taken], + &taken_flag, &message_info_sequence->data[*taken], allocation); - rmw_fastrtps_shared_cpp::SerializedData * sd = - reinterpret_cast(data_seq.buffer()[0]); - - ReturnCode_t take_ret = info->subscriber_->take(data_seq, info_seq, count); - if (take_ret != ReturnCode_t::RETCODE_OK) { - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(take_ret); - } - - // Update hasData from listener - info->listener_->update_unread_count(info->subscriber_); + if (ret != RMW_RET_OK) { + break; + } - for (size_t ii = 0; ii < static_cast(info_seq.length()); ++ii) { - if (info_seq[ii].valid_data) { - if (message_info_sequence->data + *taken) { - _assign_message_info(identifier, message_info_sequence->data + *taken, &info_seq[ii]); - } + if (taken_flag) { (*taken)++; } } From 80307e705d63e4e9951adc22515806424eb14731 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 5 Mar 2021 09:08:53 +0100 Subject: [PATCH 15/60] Fixed warning. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index c9fd5bf3e..8050ce68d 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -13,7 +13,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -133,7 +132,7 @@ __create_participant( participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( - domain_id, domainParticipantQos, participant_info->listener_, participant_mask); + static_cast(domain_id), domainParticipantQos, participant_info->listener_, participant_mask); if (!participant_info->participant_) { RMW_SET_ERROR_MSG("__create_participant failed to create participant"); From 429b9810ee7130b109a29709386bdb06f3b20bcd Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 5 Mar 2021 11:05:10 +0100 Subject: [PATCH 16/60] Added more topic related methods to utils. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 16 +++++++ rmw_fastrtps_shared_cpp/src/utils.cpp | 45 +++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 7cf162c8c..36c431cd5 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -37,6 +37,22 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +find_and_check_topic_and_type( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + eprosima::fastdds::dds::TopicDescription * & returned_topic, + eprosima::fastdds::dds::TypeSupport & returned_type); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +remove_topic_and_type( + const CustomParticipantInfo * participant_info, + const eprosima::fastdds::dds::TopicDescription * topic, + const eprosima::fastdds::dds::TypeSupport & type); + RMW_FASTRTPS_SHARED_CPP_PUBLIC eprosima::fastdds::dds::TopicDescription * create_topic_rmw( diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 7d0ba04c3..9e3203390 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -46,6 +46,51 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) } } +bool +find_and_check_topic_and_type( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + eprosima::fastdds::dds::TopicDescription * & returned_topic, + eprosima::fastdds::dds::TypeSupport & returned_type) +{ + // Avoid races against remove_topic_and_type + std::lock_guard lck(participant_info->topic_creation_mutex_); + + // Searchs for an already existing topic + returned_topic = participant_info->participant_->lookup_topicdescription(topic_name); + if (nullptr != returned_topic) { + if (returned_topic->get_type_name() != type_name) { + return false; + } + } + + returned_type = participant_info->participant_->find_type(type_name); + return true; +} + +void +remove_topic_and_type( + const CustomParticipantInfo * participant_info, + const eprosima::fastdds::dds::TopicDescription * topic_desc, + const eprosima::fastdds::dds::TypeSupport & type) +{ + // Avoid races against find_and_check_topic_and_type + std::lock_guard lck(participant_info->topic_creation_mutex_); + + // TODO (Miguel C): We only create Topic instances at the moment, but this may + // change in the future if we start supporting other kinds of TopicDescription + // (like ContentFilteredTopic) + auto topic = dynamic_cast(topic_desc); + if (nullptr != topic) { + participant_info->participant_->delete_topic(topic); + } + + if (type) { + participant_info->participant_->unregister_type(type.get_type_name()); + } +} + eprosima::fastdds::dds::TopicDescription * create_topic_rmw( const CustomParticipantInfo * participant_info, const std::string & topic_name, From 932ce837027d28afc2170c0092ffef26ce9bb816 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 8 Mar 2021 07:54:41 +0100 Subject: [PATCH 17/60] Mutex renamed. Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 2 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index ac1e1cb63..69110682c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -61,7 +61,7 @@ typedef struct CustomParticipantInfo // Struct to store every topic created in the local domain // These variables needs to be mutable std::vector topics_list_; - mutable std::mutex topic_creation_mutex_; + mutable std::mutex entity_creation_mutex_; // Flag to establish if the QoS of the participant, // its publishers and its subscribers are going diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 9e3203390..8463f853d 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -54,9 +54,6 @@ find_and_check_topic_and_type( eprosima::fastdds::dds::TopicDescription * & returned_topic, eprosima::fastdds::dds::TypeSupport & returned_type) { - // Avoid races against remove_topic_and_type - std::lock_guard lck(participant_info->topic_creation_mutex_); - // Searchs for an already existing topic returned_topic = participant_info->participant_->lookup_topicdescription(topic_name); if (nullptr != returned_topic) { @@ -75,9 +72,6 @@ remove_topic_and_type( const eprosima::fastdds::dds::TopicDescription * topic_desc, const eprosima::fastdds::dds::TypeSupport & type) { - // Avoid races against find_and_check_topic_and_type - std::lock_guard lck(participant_info->topic_creation_mutex_); - // TODO (Miguel C): We only create Topic instances at the moment, but this may // change in the future if we start supporting other kinds of TopicDescription // (like ContentFilteredTopic) @@ -97,9 +91,6 @@ eprosima::fastdds::dds::TopicDescription * create_topic_rmw( const std::string & type_name, const eprosima::fastdds::dds::TopicQos & qos) { - // This block will lock the topic creations in this participant - std::lock_guard lck(participant_info->topic_creation_mutex_); - // Searchs for an already existing topic eprosima::fastdds::dds::TopicDescription * des_topic = participant_info->participant_->lookup_topicdescription(topic_name); From 1ddb9d726e5ca5899403c39653a4d2894dc82eff Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 5 Mar 2021 11:08:39 +0100 Subject: [PATCH 18/60] Publisher types changed. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/get_publisher.cpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 126 ++++++++++-------- .../src/get_publisher.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 119 +++++++++-------- .../custom_publisher_info.hpp | 5 +- rmw_fastrtps_shared_cpp/src/publisher.cpp | 14 +- rmw_fastrtps_shared_cpp/src/rmw_publish.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 4 +- 8 files changed, 155 insertions(+), 121 deletions(-) diff --git a/rmw_fastrtps_cpp/src/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index 18af0ef77..71ae970e4 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -30,7 +30,7 @@ get_publisher(rmw_publisher_t * publisher) return nullptr; } auto impl = static_cast(publisher->data); - return impl->publisher_; + return impl->data_writer_; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 84789302b..34fdd3bdf 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -53,9 +53,11 @@ rmw_fastrtps_cpp::create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool /* keyed */, + bool keyed, bool create_publisher_listener) { + (void)keyed; + ///// // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -83,7 +85,7 @@ rmw_fastrtps_cpp::create_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; @@ -92,10 +94,10 @@ rmw_fastrtps_cpp::create_publisher( ///// // Get Participant and Publisher eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_ARGUMENT_FOR_NULL(domainParticipant, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_ARGUMENT_FOR_NULL(publisher, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); ///// // Get RMW Type Support @@ -119,6 +121,31 @@ rmw_fastrtps_cpp::create_publisher( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + des_topic, + fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called for existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Publisher struct (info) CustomPublisherInfo * info = nullptr; @@ -135,46 +162,41 @@ rmw_fastrtps_cpp::create_publisher( }); info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = callbacks; ///// // Create the Type Support struct - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); + if (!fastdds_type) { + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); + return nullptr; + } - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); - return nullptr; + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - auto cleanup_type_support = rcpputils::make_scope_exit( - [info]() { - delete info->type_support_; - }); - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { - RMW_SET_ERROR_MSG("Error registering type in publisher"); + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; + auto cleanup_type_support = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + }); ///// // Create Listener info->listener_ = nullptr; if (create_publisher_listener) { info->listener_ = new (std::nothrow) PubListener(info); - } - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - return nullptr; + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + return nullptr; + } } auto cleanup_listener = rcpputils::make_scope_exit( @@ -184,34 +206,28 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create and register Topic + if (!des_topic) { + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - // Create Topic name - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); - return nullptr; - } + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); + return nullptr; + } - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + des_topic = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); - if (des_topic == nullptr) { - RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); - return nullptr; + if (!des_topic) { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (des_topic == nullptr) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); return nullptr; } @@ -247,12 +263,12 @@ rmw_fastrtps_cpp::create_publisher( } // Creates DataWriter (with publisher name to not change name policy) - info->publisher_ = publisher->create_datawriter( + info->data_writer_ = publisher->create_datawriter( topic, dataWriterQos, info->listener_); - if (!info->publisher_) { + if (!info->data_writer_) { RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } @@ -260,13 +276,13 @@ rmw_fastrtps_cpp::create_publisher( // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->publisher_); + publisher->delete_datawriter(info->data_writer_); }); ///// // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->guid()); + eprosima_fastrtps_identifier, info->data_writer_->guid()); ///// // Allocate publisher diff --git a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp index ac08515e7..1651dce5e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -30,7 +30,7 @@ get_publisher(rmw_publisher_t * publisher) return nullptr; } auto impl = static_cast(publisher->data); - return impl->publisher_; + return impl->data_writer_; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index cea5f8c75..dd00be21e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -60,7 +60,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Check input parameters (void)keyed; - (void)create_publisher_listener; RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); @@ -85,18 +84,19 @@ rmw_fastrtps_dynamic_cpp::create_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } ///// // Get Participant and Publisher eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_ARGUMENT_FOR_NULL(domainParticipant, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_ARGUMENT_FOR_NULL(publisher, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); ///// // Get RMW Type Support @@ -120,7 +120,28 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } } - if (!is_valid_qos(*qos_policies)) { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + des_topic, + fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); return nullptr; } @@ -155,39 +176,38 @@ rmw_fastrtps_dynamic_cpp::create_publisher( info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_impl; - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); - return nullptr; - } - auto cleanup_type_support = rcpputils::make_scope_exit( - [info]() { - delete info->type_support_; - }); + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate TypeSupportProxy"); + return nullptr; + } - ///// - // Register the Type in the participant - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; + auto cleanup_type_support = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + }); + ///// // Create Listener info->listener_ = nullptr; if (create_publisher_listener) { info->listener_ = new (std::nothrow) PubListener(info); - } - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - return nullptr; + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + return nullptr; + } } auto cleanup_listener = rcpputils::make_scope_exit( @@ -197,33 +217,28 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create and register Topic + if (!des_topic) { + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - // Create Topic name - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - return nullptr; - } + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); + return nullptr; + } - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + des_topic = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); - if (des_topic == nullptr) { - RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); - return nullptr; + if (!des_topic) { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (des_topic == nullptr) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); return nullptr; } @@ -259,12 +274,12 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } // Creates DataWriter (with publisher name to not change name policy) - info->publisher_ = publisher->create_datawriter( + info->data_writer_ = publisher->create_datawriter( topic, dataWriterQos, info->listener_); - if (!info->publisher_) { + if (!info->data_writer_) { RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } @@ -272,7 +287,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->publisher_); + publisher->delete_datawriter(info->data_writer_); }); // 1 Heartbeat every 10ms @@ -286,7 +301,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->guid()); + eprosima_fastrtps_identifier, info->data_writer_->guid()); ///// // Allocate publisher diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 09d11ba23..10cb2362d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -24,6 +24,7 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" @@ -38,9 +39,9 @@ typedef struct CustomPublisherInfo : public CustomEventInfo { virtual ~CustomPublisherInfo() = default; - eprosima::fastdds::dds::DataWriter * publisher_{nullptr}; + eprosima::fastdds::dds::DataWriter * data_writer_{nullptr}; PubListener * listener_{nullptr}; - rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; const void * type_support_impl_{nullptr}; rmw_gid_t publisher_gid{}; const char * typesupport_identifier_{nullptr}; diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 18aec29a9..69913bcb8 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -43,10 +43,14 @@ destroy_publisher( rmw_ret_t ret = RMW_RET_OK; auto info = static_cast(publisher->data); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointer to topic, so we can remove it later + auto topic = info->data_writer_->get_topic(); + // Delete DataWriter - ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->publisher_); + ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -57,10 +61,8 @@ destroy_publisher( delete info->listener_; } - // Delete type support inside subscription - if (info->type_support_ != nullptr) { - delete info->type_support_; - } + // Delete topic and unregister type + remove_topic_and_type(participant_info, topic, info->type_support_); // Delete PublisherInfo structure delete info; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp index 947f1c55e..cc1d7e23f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp @@ -55,7 +55,7 @@ __rmw_publish( data.is_cdr_buffer = false; data.data = const_cast(ros_message); data.impl = info->type_support_impl_; - if (!info->publisher_->write(&data)) { + if (!info->data_writer_->write(&data)) { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; } @@ -101,7 +101,7 @@ __rmw_publish_serialized_message( data.is_cdr_buffer = true; data.data = &ser; data.impl = nullptr; // not used when is_cdr_buffer is true - if (!info->publisher_->write(&data)) { + if (!info->data_writer_->write(&data)) { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index d3178c2e6..b16195269 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -115,7 +115,7 @@ __rmw_publisher_assert_liveliness( return RMW_RET_ERROR; } - info->publisher_->assert_liveliness(); + info->data_writer_->assert_liveliness(); return RMW_RET_OK; } @@ -125,7 +125,7 @@ __rmw_publisher_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(publisher->data); - eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->publisher_; + eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->data_writer_; const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastrtps_pub->get_qos(); From befb40d94dc0c22af0935a6cf5a1eaee22fa8208 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 5 Mar 2021 12:11:52 +0100 Subject: [PATCH 19/60] Subscription types changed. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/get_subscriber.cpp | 2 +- rmw_fastrtps_cpp/src/subscription.cpp | 122 ++++++++++-------- .../src/get_subscriber.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 111 +++++++++------- .../custom_subscriber_info.hpp | 5 +- .../src/rmw_subscription.cpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 10 +- rmw_fastrtps_shared_cpp/src/subscription.cpp | 14 +- 8 files changed, 152 insertions(+), 116 deletions(-) diff --git a/rmw_fastrtps_cpp/src/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 5aae463b7..468a732b0 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -30,7 +30,7 @@ get_subscriber(rmw_subscription_t * subscription) return nullptr; } auto impl = static_cast(subscription->data); - return impl->subscriber_; + return impl->data_reader_; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 1ddb29e99..925abaf7c 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -58,9 +58,11 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool /* keyed */, + bool keyed, bool create_subscription_listener) { + (void)keyed; + ///// // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -88,8 +90,9 @@ create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -99,11 +102,10 @@ create_subscription( RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_ARGUMENT_FOR_NULL(subscriber, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -124,6 +126,31 @@ create_subscription( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + des_topic, + fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called for existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Subscriber struct (info) CustomSubscriberInfo * info = nullptr; @@ -144,42 +171,37 @@ create_subscription( ///// // Create the Type Support struct - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); + if (!fastdds_type) { + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; + } - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); - return nullptr; + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - auto cleanup_type_support = rcpputils::make_scope_exit( - [info]() { - delete info->type_support_; - }); - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) MessageTypeSupport_cpp(callbacks))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; + auto cleanup_type_support = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + }); ///// // Create Listener info->listener_ = nullptr; if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); - } - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } } auto cleanup_listener = rcpputils::make_scope_exit( @@ -188,30 +210,24 @@ create_subscription( }); ///// - // Create Topic + // Create and register Topic + if (!des_topic) { + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - // Create Topic name - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - return nullptr; - } + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + des_topic = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); - if (des_topic == nullptr) { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; + if (!des_topic) { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } } ///// @@ -237,12 +253,12 @@ create_subscription( } // Creates DataReader (with subscriber name to not change name policy) - info->subscriber_ = subscriber->create_datareader( + info->data_reader_ = subscriber->create_datareader( des_topic, dataReaderQos, info->listener_); - if (!info->subscriber_) { + if (!info->data_reader_) { RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); return nullptr; } @@ -250,13 +266,13 @@ create_subscription( // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->subscriber_); + subscriber->delete_datareader(info->data_reader_); }); ///// // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->guid()); + eprosima_fastrtps_identifier, info->data_reader_->guid()); ///// // Allocate subscription diff --git a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp index daaecad13..e8111fccc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -30,7 +30,7 @@ get_subscriber(rmw_subscription_t * subscription) return nullptr; } auto impl = static_cast(subscription->data); - return impl->subscriber_; + return impl->data_reader_; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 2128d1df0..aa3d6279f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -90,8 +90,9 @@ create_subscription( (void)create_subscription_listener; ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -101,7 +102,7 @@ create_subscription( RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_ARGUMENT_FOR_NULL(subscriber, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -125,6 +126,31 @@ create_subscription( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + des_topic, + fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Subscriber struct (info) CustomSubscriberInfo * info = nullptr; @@ -156,31 +182,28 @@ create_subscription( info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support_impl; - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_support_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); - return nullptr; - } - auto cleanup_type_support = rcpputils::make_scope_exit( - [info]() { - delete info->type_support_; - }); + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); + return nullptr; + } - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport(new (std::nothrow) TypeSupportProxy(type_support_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; + auto cleanup_type_support = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + }); + ///// // Create Listener info->listener_ = nullptr; @@ -199,30 +222,24 @@ create_subscription( }); ///// - // Create Topic - - // Create Topic name - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + // Create and register Topic + if (!des_topic) { + // Use Topic Qos Default + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - return nullptr; - } + if (!get_topic_qos(*qos_policies, topicQos)) { + return nullptr; + } - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - topic_name_mangled, - type_name, - topicQos); + des_topic = domainParticipant->create_topic( + topic_name_mangled, + type_name, + topicQos); - if (des_topic == nullptr) { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; + if (!des_topic) { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } } ///// @@ -248,12 +265,12 @@ create_subscription( } // Creates DataReader (with subscriber name to not change name policy) - info->subscriber_ = subscriber->create_datareader( + info->data_reader_ = subscriber->create_datareader( des_topic, dataReaderQos, info->listener_); - if (!info->subscriber_) { + if (!info->data_reader_) { RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); return nullptr; } @@ -261,13 +278,13 @@ create_subscription( // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->subscriber_); + subscriber->delete_datareader(info->data_reader_); }); ///// // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->guid()); + eprosima_fastrtps_identifier, info->data_reader_->guid()); rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 1b9c9e364..edcb18ba3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -23,6 +23,7 @@ #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -38,9 +39,9 @@ struct CustomSubscriberInfo : public CustomEventInfo { virtual ~CustomSubscriberInfo() = default; - eprosima::fastdds::dds::DataReader * subscriber_ {nullptr}; + eprosima::fastdds::dds::DataReader * data_reader_ {nullptr}; SubListener * listener_{nullptr}; - rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; const void * type_support_impl_{nullptr}; rmw_gid_t subscription_gid_{}; const char * typesupport_identifier_{nullptr}; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 25f3258d4..ab9bf5416 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -99,7 +99,7 @@ __rmw_subscription_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader * fastrtps_sub = info->subscriber_; + eprosima::fastdds::dds::DataReader * fastrtps_sub = info->data_reader_; const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastrtps_sub->get_qos(); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 85427b661..5fd17b58a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -77,9 +77,9 @@ _take( data.is_cdr_buffer = false; data.data = ros_message; data.impl = info->type_support_impl_; - if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->subscriber_); + info->listener_->update_unread_count(info->data_reader_); if (sinfo.valid_data) { if (message_info) { @@ -117,7 +117,7 @@ _take_sequence( // Limit the upper bound of reads to the number unread at the beginning. // This prevents any samples that are added after the beginning of the // _take_sequence call from being read. - auto unread_count = info->subscriber_->get_unread_count(); + auto unread_count = info->data_reader_->get_unread_count(); if (unread_count < count) { count = unread_count; } @@ -285,9 +285,9 @@ _take_serialized_message( data.data = &buffer; data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->subscriber_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->subscriber_); + info->listener_->update_unread_count(info->data_reader_); if (sinfo.valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index 3676838ac..f58acc562 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -43,10 +43,14 @@ destroy_subscription( rmw_ret_t ret = RMW_RET_OK; auto info = static_cast(subscription->data); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointer to topic, so we can remove it later + auto topic = info->data_reader_->get_topicdescription(); + // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->subscriber_); + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->data_reader_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -57,10 +61,8 @@ destroy_subscription( delete info->listener_; } - // Delete type support inside subscription - if (info->type_support_ != nullptr) { - delete info->type_support_; - } + // Delete topic and unregister type + remove_topic_and_type(participant_info, topic, info->type_support_); // Delete SubscriberInfo structure delete info; From fb5bb55a03421774bf2d4163bcd3762858c90a8c Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 8 Mar 2021 12:08:57 +0100 Subject: [PATCH 20/60] Cilent types changed. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/get_client.cpp | 4 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 242 ++++++++++-------- rmw_fastrtps_dynamic_cpp/src/get_client.cpp | 4 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 239 +++++++++-------- .../custom_client_info.hpp | 9 +- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 27 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 4 +- 8 files changed, 287 insertions(+), 244 deletions(-) diff --git a/rmw_fastrtps_cpp/src/get_client.cpp b/rmw_fastrtps_cpp/src/get_client.cpp index c4076b2f6..76e30662c 100644 --- a/rmw_fastrtps_cpp/src/get_client.cpp +++ b/rmw_fastrtps_cpp/src/get_client.cpp @@ -33,7 +33,7 @@ get_request_publisher(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->request_publisher_; + return impl->request_writer_; } eprosima::fastdds::dds::DataReader * @@ -46,7 +46,7 @@ get_response_subscriber(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->response_subscriber_; + return impl->response_reader_; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 933e61c48..f3ee90b35 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -84,8 +84,9 @@ rmw_create_client( } ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -96,22 +97,13 @@ rmw_create_client( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - if (!domainParticipant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - if (!subscriber) { - RMW_SET_ERROR_MSG("subscriber handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -135,6 +127,60 @@ rmw_create_client( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + const service_type_support_callbacks_t * service_members; + const message_type_support_callbacks_t * request_members; + const message_type_support_callbacks_t * response_members; + + service_members = static_cast(type_support->data); + request_members = static_cast( + service_members->request_members_->data); + response_members = static_cast( + service_members->response_members_->data); + + std::string request_type_name = _create_type_name(request_members); + std::string response_type_name = _create_type_name(response_members); + + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + request_topic, + request_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); + return nullptr; + } + + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + response_topic, + response_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Client struct (info) CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); @@ -153,65 +199,48 @@ rmw_create_client( info->response_subscriber_matched_count_ = 0; ///// - // Create the Type Support struct - const service_type_support_callbacks_t * service_members; - const message_type_support_callbacks_t * request_members; - const message_type_support_callbacks_t * response_members; - - service_members = static_cast(type_support->data); - request_members = static_cast( - service_members->request_members_->data); - response_members = static_cast( - service_members->response_members_->data); - + // Create the Type Support structs info->request_type_support_impl_ = request_members; info->response_type_support_impl_ = response_members; - std::string request_type_name = _create_type_name(request_members); - std::string response_type_name = _create_type_name(response_members); + if (!request_fastdds_type) { + auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } - info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); - return nullptr; + request_fastdds_type.reset(tsupport); } - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info]() { - delete info->request_type_support_; - }); + if (!response_fastdds_type) { + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } - info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); - return nullptr; + response_fastdds_type.reset(tsupport); } - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info]() { - delete info->response_type_support_; - }); - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, - // so it is dangerous to keep using it. - // Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) RequestTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } + info->request_type_support_ = request_fastdds_type; + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + }); - ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) ResponseTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } + info->response_type_support_ = response_fastdds_type; + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + }); ///// // Create Listeners @@ -247,55 +276,46 @@ rmw_create_client( } // Create response topic - std::string sub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); - - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - response_type_name, - topicQos); - - if (des_sub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create response topic"); - return nullptr; + if (!response_topic) { + response_topic = domainParticipant->create_topic( + response_topic_name, + response_type_name, + topicQos); + + if (!response_topic) { + RMW_SET_ERROR_MSG("failed to create client response topic"); + return nullptr; + } } // Create request topic - std::string pub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - request_type_name, - topicQos); - - if (des_pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create request topic"); - return nullptr; + if (!request_topic) { + request_topic = domainParticipant->create_topic( + request_topic_name, + request_type_name, + topicQos); + + if (!request_topic) { + RMW_SET_ERROR_MSG("failed to create client request topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(des_pub_topic); - if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + dynamic_cast(request_topic); + if (!pub_topic) { + RMW_SET_ERROR_MSG("failed, client request topic can only be of class Topic"); return nullptr; } - info->request_topic_ = pub_topic_name; - info->response_topic_ = sub_topic_name; + info->request_topic_ = request_topic_name; + info->response_topic_ = response_topic_name; - // Key word to find DataWrtier and DataReader QoS + // Keyword to find DataWriter and DataReader QoS std::string topic_name_fallback = "client"; ///// - // Create request DataReader + // Create response DataReader // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search @@ -303,13 +323,13 @@ rmw_create_client( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", - // if it does not exist it tryes with the request topic name + // if it does not exist it tryes with the response topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(response_topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { dataReaderQos.endpoint().history_memory_policy = @@ -321,12 +341,12 @@ rmw_create_client( } // Creates DataReader (with subscriber name to not change name policy) - info->response_subscriber_ = subscriber->create_datareader( - des_sub_topic, + info->response_reader_ = subscriber->create_datareader( + response_topic, dataReaderQos, info->listener_); - if (!info->response_subscriber_) { + if (!info->response_reader_) { RMW_SET_ERROR_MSG("failed to create client response data reader"); return nullptr; } @@ -334,7 +354,7 @@ rmw_create_client( // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->response_subscriber_); + subscriber->delete_datareader(info->response_reader_); }); // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile @@ -349,7 +369,7 @@ rmw_create_client( // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(request_topic_name, dataWriterQos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { @@ -368,12 +388,12 @@ rmw_create_client( } // Creates DataWriter (with publisher name to not change name policy) - info->request_publisher_ = publisher->create_datawriter( + info->request_writer_ = publisher->create_datawriter( pub_topic, dataWriterQos, info->pub_listener_); - if (!info->request_publisher_) { + if (!info->request_writer_) { RMW_SET_ERROR_MSG("failed to create client request data writer"); return nullptr; } @@ -381,7 +401,7 @@ rmw_create_client( // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->request_publisher_); + publisher->delete_datawriter(info->request_writer_); }); ///// @@ -393,14 +413,14 @@ rmw_create_client( "************ Client Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Sub Topic %s", sub_topic_name.c_str()); + "Sub Topic %s", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Pub Topic %s", pub_topic_name.c_str()); + "Pub Topic %s", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); - info->writer_guid_ = info->request_publisher_->guid(); - info->reader_guid_ = info->response_subscriber_->guid(); + info->writer_guid_ = info->request_writer_->guid(); + info->reader_guid_ = info->response_reader_->guid(); rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { @@ -427,7 +447,7 @@ rmw_create_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_publisher_->guid()); + eprosima_fastrtps_identifier, info->request_writer_->guid()); common_context->graph_cache.associate_writer( request_publisher_gid, common_context->gid, @@ -435,7 +455,7 @@ rmw_create_client( node->namespace_); rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->guid()); + eprosima_fastrtps_identifier, info->response_reader_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( response_subscriber_gid, diff --git a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp index 09f31e073..e25b657d9 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -30,7 +30,7 @@ get_request_publisher(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->request_publisher_; + return impl->request_writer_; } eprosima::fastdds::dds::DataReader * @@ -43,7 +43,7 @@ get_response_subscriber(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->response_subscriber_; + return impl->response_reader_; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 37dea7eb4..cc116498a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -88,8 +88,9 @@ rmw_create_client( } ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -100,22 +101,13 @@ rmw_create_client( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - if (!domainParticipant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - if (!subscriber) { - RMW_SET_ERROR_MSG("subscriber handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -139,6 +131,58 @@ rmw_create_client( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = get_request_ptr(type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr(type_support->data, type_support->typesupport_identifier); + + std::string request_type_name = _create_type_name( + untyped_request_members, type_support->typesupport_identifier); + std::string response_type_name = _create_type_name( + untyped_response_members, type_support->typesupport_identifier); + + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + request_topic, + request_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); + return nullptr; + } + + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + response_topic, + response_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Client struct (info) CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); @@ -152,13 +196,12 @@ rmw_create_client( delete info; }); - // info->participant_ = domainParticipant; info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; ///// - // Create the Type Support struct + // Create the Type Support structs TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { @@ -183,60 +226,47 @@ rmw_create_client( info->request_type_support_impl_ = request_type_impl; info->response_type_support_impl_ = response_type_impl; - info->request_type_support_ = - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); - return nullptr; - } - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info]() { - delete info->request_type_support_; - }); + if (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } - info->response_type_support_ = - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); - return nullptr; + request_fastdds_type.reset(tsupport); } - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info]() { - delete info->response_type_support_; - }); - - const void * untyped_request_members; - const void * untyped_response_members; - untyped_request_members = get_request_ptr(type_support->data, info->typesupport_identifier_); - untyped_response_members = get_response_ptr(type_support->data, info->typesupport_identifier_); + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } - std::string request_type_name = _create_type_name( - untyped_request_members, info->typesupport_identifier_); - std::string response_type_name = _create_type_name( - untyped_response_members, info->typesupport_identifier_); + response_fastdds_type.reset(tsupport); + } - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } + info->request_type_support_ = request_fastdds_type; + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + }); - ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } + info->response_type_support_ = response_fastdds_type; + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + }); ///// // Create Listeners @@ -272,51 +302,42 @@ rmw_create_client( } // Create response topic - std::string sub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); - - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - response_type_name, - topicQos); - - if (des_sub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create response topic"); - return nullptr; + if (!response_topic) { + response_topic = domainParticipant->create_topic( + response_topic_name, + response_type_name, + topicQos); + + if (!response_topic) { + RMW_SET_ERROR_MSG("failed to create client response topic"); + return nullptr; + } } // Create request topic - std::string pub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - request_type_name, - topicQos); - - if (des_pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create request topic"); - return nullptr; + if (!request_topic) { + request_topic = domainParticipant->create_topic( + request_topic_name, + request_type_name, + topicQos); + + if (!request_topic) { + RMW_SET_ERROR_MSG("failed to create client request topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(des_pub_topic); - if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + dynamic_cast(request_topic); + if (!pub_topic) { + RMW_SET_ERROR_MSG("failed, client request topic can only be of class Topic"); return nullptr; } - info->request_topic_ = pub_topic_name; - info->response_topic_ = sub_topic_name; + info->request_topic_ = request_topic_name; + info->response_topic_ = response_topic_name; - // Key word to find DataWrtier and DataReader QoS + // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "client"; ///// @@ -334,7 +355,7 @@ rmw_create_client( // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(response_topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { dataReaderQos.endpoint().history_memory_policy = @@ -346,12 +367,12 @@ rmw_create_client( } // Creates DataReader (with subscriber name to not change name policy) - info->response_subscriber_ = subscriber->create_datareader( - des_sub_topic, + info->response_reader_ = subscriber->create_datareader( + response_topic, dataReaderQos, info->listener_); - if (!info->response_subscriber_) { + if (!info->response_reader_) { RMW_SET_ERROR_MSG("failed to create client response data reader"); return nullptr; } @@ -359,7 +380,7 @@ rmw_create_client( // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->response_subscriber_); + subscriber->delete_datareader(info->response_reader_); }); // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile @@ -374,7 +395,7 @@ rmw_create_client( // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(request_topic_name, dataWriterQos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { @@ -393,12 +414,12 @@ rmw_create_client( } // Creates DataWriter (with publisher name to not change name policy) - info->request_publisher_ = publisher->create_datawriter( + info->request_writer_ = publisher->create_datawriter( pub_topic, dataWriterQos, info->pub_listener_); - if (!info->request_publisher_) { + if (!info->request_writer_) { RMW_SET_ERROR_MSG("failed to create client request data writer"); return nullptr; } @@ -406,7 +427,7 @@ rmw_create_client( // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->request_publisher_); + publisher->delete_datawriter(info->request_writer_); }); ///// @@ -417,14 +438,14 @@ rmw_create_client( "************ Client Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Sub Topic %s", sub_topic_name.c_str()); + "Sub Topic %s", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", pub_topic_name.c_str()); + "Pub Topic %s", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); - info->writer_guid_ = info->request_publisher_->guid(); - info->reader_guid_ = info->response_subscriber_->guid(); + info->writer_guid_ = info->request_writer_->guid(); + info->reader_guid_ = info->response_reader_->guid(); rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { @@ -451,7 +472,7 @@ rmw_create_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_publisher_->guid()); + eprosima_fastrtps_identifier, info->request_writer_->guid()); common_context->graph_cache.associate_writer( gid, common_context->gid, @@ -459,7 +480,7 @@ rmw_create_client( node->namespace_); rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->guid()); + eprosima_fastrtps_identifier, info->response_reader_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( gid_response, common_context->gid, node->name, node->namespace_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 145090b59..df0e05446 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -32,6 +32,7 @@ #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -42,12 +43,12 @@ class ClientPubListener; typedef struct CustomClientInfo { - rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; const void * request_type_support_impl_{nullptr}; - rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; const void * response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader * response_subscriber_{nullptr}; - eprosima::fastdds::dds::DataWriter * request_publisher_{nullptr}; + eprosima::fastdds::dds::DataReader * response_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * request_writer_{nullptr}; std::string request_topic_; std::string response_topic_; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index d26fb2995..ba1ba8e14 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -45,14 +45,14 @@ __rmw_destroy_client( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_publisher_->guid()); + identifier, info->request_writer_->guid()); common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_subscriber_->guid()); + identifier, info->response_reader_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); @@ -68,10 +68,15 @@ __rmw_destroy_client( auto participant_info = static_cast(node->context->impl->participant_info); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_reader_->get_topicdescription(); + auto request_topic = info->request_writer_->get_topic(); + // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_subscriber_); + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -83,7 +88,7 @@ __rmw_destroy_client( } // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->request_publisher_); + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -94,15 +99,9 @@ __rmw_destroy_client( delete info->pub_listener_; } - // Delete request type support inside subscription - if (info->request_type_support_ != nullptr) { - delete info->request_type_support_; - } - - // Delete response type support inside subscription - if (info->response_type_support_ != nullptr) { - delete info->response_type_support_; - } + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); // Delete ClientInfo structure delete info; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 59dc5cf8f..a3c15d8bb 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -56,7 +56,7 @@ __rmw_send_request( data.data = const_cast(ros_request); data.impl = info->request_type_support_impl_; wparams.related_sample_identity().writer_guid() = info->reader_guid_; - if (info->request_publisher_->write(&data, wparams)) { + if (info->request_writer_->write(&data, wparams)) { returnedValue = RMW_RET_OK; *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | wparams.sample_identity().sequence_number().low; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 4d107a77d..a12b8c71a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -55,11 +55,13 @@ __rmw_take_response( CustomClientResponse response; if (info->listener_->getResponse(response)) { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); eprosima::fastcdr::Cdr deser( *response.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (info->response_type_support_->deserializeROSmessage( + if (raw_type_support->deserializeROSmessage( deser, ros_response, info->response_type_support_impl_)) { request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); From a634cd343c2d0528c921dad97f1f59a121151233 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 8 Mar 2021 15:44:37 +0100 Subject: [PATCH 21/60] Service types changed. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/get_service.cpp | 4 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 241 ++++++++++-------- rmw_fastrtps_dynamic_cpp/src/get_service.cpp | 4 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 230 +++++++++-------- .../custom_service_info.hpp | 9 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 27 +- 8 files changed, 282 insertions(+), 239 deletions(-) diff --git a/rmw_fastrtps_cpp/src/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index 51b88341b..f92e80812 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -30,7 +30,7 @@ get_request_subscriber(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->request_subscriber_; + return impl->request_reader_; } eprosima::fastdds::dds::DataWriter * @@ -43,7 +43,7 @@ get_response_publisher(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->response_publisher_; + return impl->response_writer_; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 2ce92f58b..972a128f3 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -94,8 +94,9 @@ rmw_create_service( } ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -106,22 +107,13 @@ rmw_create_service( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - if (!domainParticipant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - if (!subscriber) { - RMW_SET_ERROR_MSG("subscriber handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -145,22 +137,12 @@ rmw_create_service( } } - ///// - // Create the RMW Service struct (info) - CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate service info"); - return nullptr; - } - auto cleanup_info = rcpputils::make_scope_exit( - [info]() { - delete info; - }); - - info->typesupport_identifier_ = type_support->typesupport_identifier; + std::lock_guard lck(participant_info->entity_creation_mutex_); ///// - // Create the Type Support struct + // Find and check existing topics and types + + // Create Topic and Type names const service_type_support_callbacks_t * service_members; const message_type_support_callbacks_t * request_members; const message_type_support_callbacks_t * response_members; @@ -171,60 +153,107 @@ rmw_create_service( response_members = static_cast( service_members->response_members_->data); - info->request_type_support_impl_ = request_members; - info->response_type_support_impl_ = response_members; - std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); - // These structs are not used in the future, as there is no need to unregister the types - info->request_type_support_ = new (std::nothrow) RequestTypeSupport_cpp(service_members); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + request_topic, + request_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); return nullptr; } - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info]() { - delete info->request_type_support_; - }); - info->response_type_support_ = new (std::nothrow) ResponseTypeSupport_cpp(service_members); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + response_topic, + response_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); return nullptr; } - auto cleanup_type_support_response = rcpputils::make_scope_exit( + + ///// + // Create the RMW Service struct (info) + CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate service info"); + return nullptr; + } + auto cleanup_info = rcpputils::make_scope_exit( [info]() { - delete info->response_type_support_; + delete info; }); + info->typesupport_identifier_ = type_support->typesupport_identifier; + ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) RequestTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + // Create the Type Support structs + info->request_type_support_impl_ = request_members; + info->response_type_support_impl_ = response_members; + + if (!request_fastdds_type) { + auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } + + request_fastdds_type.reset(tsupport); + } + if (!response_fastdds_type) { + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } + + response_fastdds_type.reset(tsupport); + } + + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } + info->request_type_support_ = request_fastdds_type; + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + }); - ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) ResponseTypeSupport_cpp(service_members))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } + info->response_type_support_ = response_fastdds_type; + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + }); ///// // Create Listeners info->listener_ = new (std::nothrow) ServiceListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + RMW_SET_ERROR_MSG("failed to create service response subscriber listener"); return nullptr; } @@ -235,7 +264,7 @@ rmw_create_service( info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + RMW_SET_ERROR_MSG("failed to create service request publisher listener"); return nullptr; } @@ -254,47 +283,39 @@ rmw_create_service( } // Create request topic - std::string sub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - request_type_name, - topicQos); - - if (des_sub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create request topic"); - return nullptr; + if (!request_topic) { + request_topic = domainParticipant->create_topic( + request_topic_name, + request_type_name, + topicQos); + + if (!request_topic) { + RMW_SET_ERROR_MSG("failed to create service request topic"); + return nullptr; + } } // Create response topic - std::string pub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - response_type_name, - topicQos); - - if (des_pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create response topic"); - return nullptr; + if (!response_topic) { + response_topic = domainParticipant->create_topic( + response_topic_name, + response_type_name, + topicQos); + + if (!response_topic) { + RMW_SET_ERROR_MSG("failed to create service response topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(des_pub_topic); + dynamic_cast(response_topic); if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + RMW_SET_ERROR_MSG("failed, service response topic can only be of class Topic"); return nullptr; } - // Key word to find DataWrtier and DataReader QoS + // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; ///// @@ -302,17 +323,17 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // with profile_name "service" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); - // Try to load the profile named "client", + // Try to load the profile named "service", // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(request_topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { dataReaderQos.endpoint().history_memory_policy = @@ -324,20 +345,20 @@ rmw_create_service( } // Creates DataReader (with subscriber name to not change name policy) - info->request_subscriber_ = subscriber->create_datareader( - des_sub_topic, + info->request_reader_ = subscriber->create_datareader( + request_topic, dataReaderQos, info->listener_); - if (!info->request_subscriber_) { - RMW_SET_ERROR_MSG("failed to create client request data reader"); + if (!info->request_reader_) { + RMW_SET_ERROR_MSG("failed to create service request data reader"); return nullptr; } // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->request_subscriber_); + subscriber->delete_datareader(info->request_reader_); }); @@ -346,17 +367,17 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // with profile_name "service" is attempted. Else, use the default attributes. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); - // Try to load the profile named "client", + // Try to load the profile named "service", // if it does not exist it tryes with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(response_topic_name, dataWriterQos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { @@ -375,20 +396,20 @@ rmw_create_service( } // Creates DataWriter (with publisher name to not change name policy) - info->response_publisher_ = publisher->create_datawriter( + info->response_writer_ = publisher->create_datawriter( pub_topic, dataWriterQos, info->pub_listener_); - if (!info->response_publisher_) { - RMW_SET_ERROR_MSG("failed to create client request data writer"); + if (!info->response_writer_) { + RMW_SET_ERROR_MSG("failed to create service request data writer"); return nullptr; } // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->response_publisher_); + publisher->delete_datawriter(info->response_writer_); }); ///// @@ -400,10 +421,10 @@ rmw_create_service( "************ Service Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Sub Topic %s", sub_topic_name.c_str()); + "Sub Topic %s", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Pub Topic %s", pub_topic_name.c_str()); + "Pub Topic %s", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); rmw_service_t * rmw_service = rmw_service_allocate(); @@ -431,14 +452,14 @@ rmw_create_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_subscriber_->guid()); + eprosima_fastrtps_identifier, info->request_reader_->guid()); common_context->graph_cache.associate_reader( request_subscriber_gid, common_context->gid, node->name, node->namespace_); rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->guid()); + eprosima_fastrtps_identifier, info->response_writer_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( response_publisher_gid, diff --git a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp index 96de84cce..68d8a75c2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -30,7 +30,7 @@ get_request_subscriber(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->request_subscriber_; + return impl->request_reader_; } eprosima::fastdds::dds::DataWriter * @@ -43,7 +43,7 @@ get_response_publisher(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->response_publisher_; + return impl->response_writer_; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index ca630cfb1..0b2fa3a39 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -100,8 +100,9 @@ rmw_create_service( } ///// - // Check ROS QoS + // Check RMW QoS if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("Invalid QoS"); return nullptr; } @@ -112,22 +113,13 @@ rmw_create_service( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - if (!domainParticipant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - if (!subscriber) { - RMW_SET_ERROR_MSG("subscriber handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -151,6 +143,58 @@ rmw_create_service( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = get_request_ptr(type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr(type_support->data, type_support->typesupport_identifier); + + std::string request_type_name = _create_type_name( + untyped_request_members, type_support->typesupport_identifier); + std::string response_type_name = _create_type_name( + untyped_response_members, type_support->typesupport_identifier); + + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + request_topic, + request_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); + return nullptr; + } + + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + response_topic, + response_fastdds_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + ///// // Create the RMW Service struct (info) CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); @@ -166,8 +210,7 @@ rmw_create_service( info->typesupport_identifier_ = type_support->typesupport_identifier; ///// - // Create the Type Support struct - + // Create the Type Support structs TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { @@ -193,62 +236,47 @@ rmw_create_service( info->request_type_support_impl_ = request_type_impl; info->response_type_support_impl_ = response_type_impl; - const void * untyped_request_members; - const void * untyped_response_members; - - info->request_type_support_ = - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); - if (!info->request_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); - return nullptr; - } - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info]() { - delete info->request_type_support_; - }); + if (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + return nullptr; + } - info->response_type_support_ = - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); - if (!info->response_type_support_) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); - return nullptr; + request_fastdds_type.reset(tsupport); } - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info]() { - delete info->response_type_support_; - }); - untyped_request_members = - get_request_ptr(type_support->data, info->typesupport_identifier_); - untyped_response_members = get_response_ptr( - type_support->data, info->typesupport_identifier_); + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + return nullptr; + } - std::string request_type_name = _create_type_name( - untyped_request_members, info->typesupport_identifier_); - std::string response_type_name = _create_type_name( - untyped_response_members, info->typesupport_identifier_); + response_fastdds_type.reset(tsupport); + } - ///// - // Register the Type in the participant - // When a type is registered in a participant, it is converted to a shared_ptr, so it is - // dangerous to keep using it. Thus we use a new TypeSupport created only to register it. - ReturnCode_t ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } + info->request_type_support_ = request_fastdds_type; + auto cleanup_type_support_request = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + }); - ret = domainParticipant->register_type( - eprosima::fastdds::dds::TypeSupport( - new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl))); - // Register could fail if there is already a type with that name in participant, - // so not only OK retcode is possible - if (ret != ReturnCode_t::RETCODE_OK && ret != ReturnCode_t::RETCODE_PRECONDITION_NOT_MET) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } + info->response_type_support_ = response_fastdds_type; + auto cleanup_type_support_response = rcpputils::make_scope_exit( + [info, domainParticipant]() { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + }); ///// // Create Listeners @@ -284,47 +312,39 @@ rmw_create_service( } // Create request topic - std::string sub_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_sub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - sub_topic_name, - request_type_name, - topicQos); - - if (des_sub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create request topic"); - return nullptr; + if (!request_topic) { + request_topic = domainParticipant->create_topic( + request_topic_name, + request_type_name, + topicQos); + + if (!request_topic) { + RMW_SET_ERROR_MSG("failed to create service request topic"); + return nullptr; + } } // Create response topic - std::string pub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); - - // General function to create or get an already existing topic - eprosima::fastdds::dds::TopicDescription * des_pub_topic = - rmw_fastrtps_shared_cpp::create_topic_rmw( - participant_info, - pub_topic_name, - response_type_name, - topicQos); - - if (des_pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed to create response topic"); - return nullptr; + if (!response_topic) { + response_topic = domainParticipant->create_topic( + response_topic_name, + response_type_name, + topicQos); + + if (!response_topic) { + RMW_SET_ERROR_MSG("failed to create service response topic"); + return nullptr; + } } eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(des_pub_topic); + dynamic_cast(response_topic); if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, publisher topic can only be of class Topic"); + RMW_SET_ERROR_MSG("failed, service response topic can only be of class Topic"); return nullptr; } - // Key word to find DataWrtier and DataReader QoS + // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; ///// @@ -342,7 +362,7 @@ rmw_create_service( // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(sub_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(request_topic_name, dataReaderQos); if (!participant_info->leave_middleware_default_qos) { dataReaderQos.endpoint().history_memory_policy = @@ -354,12 +374,12 @@ rmw_create_service( } // Creates DataReader (with subscriber name to not change name policy) - info->request_subscriber_ = subscriber->create_datareader( - des_sub_topic, + info->request_reader_ = subscriber->create_datareader( + request_topic, dataReaderQos, info->listener_); - if (!info->request_subscriber_) { + if (!info->request_reader_) { RMW_SET_ERROR_MSG("failed to create service request data reader"); return nullptr; } @@ -367,7 +387,7 @@ rmw_create_service( // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { - subscriber->delete_datareader(info->request_subscriber_); + subscriber->delete_datareader(info->request_reader_); }); @@ -386,7 +406,7 @@ rmw_create_service( // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(pub_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(response_topic_name, dataWriterQos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { @@ -405,12 +425,12 @@ rmw_create_service( } // Creates DataWriter (with publisher name to not change name policy) - info->response_publisher_ = publisher->create_datawriter( + info->response_writer_ = publisher->create_datawriter( pub_topic, dataWriterQos, info->pub_listener_); - if (!info->response_publisher_) { + if (!info->response_writer_) { RMW_SET_ERROR_MSG("failed to create service request data writer"); return nullptr; } @@ -418,7 +438,7 @@ rmw_create_service( // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { - publisher->delete_datawriter(info->response_publisher_); + publisher->delete_datawriter(info->response_writer_); }); ///// @@ -429,10 +449,10 @@ rmw_create_service( "************ Service Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Sub Topic %s", sub_topic_name.c_str()); + "Sub Topic %s", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", pub_topic_name.c_str()); + "Pub Topic %s", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); rmw_service_t * rmw_service = rmw_service_allocate(); @@ -460,7 +480,7 @@ rmw_create_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->request_subscriber_->guid()); + eprosima_fastrtps_identifier, info->request_reader_->guid()); common_context->graph_cache.associate_reader( gid, common_context->gid, @@ -468,7 +488,7 @@ rmw_create_service( node->namespace_); rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->guid()); + eprosima_fastrtps_identifier, info->response_writer_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 145b6e160..7ef5371e4 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -30,6 +30,7 @@ #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -49,12 +50,12 @@ enum class client_present_t typedef struct CustomServiceInfo { - rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; const void * request_type_support_impl_{nullptr}; - rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; const void * response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader * request_subscriber_{nullptr}; - eprosima::fastdds::dds::DataWriter * response_publisher_{nullptr}; + eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index a3c15d8bb..c89bea3c9 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -92,9 +92,11 @@ __rmw_take_request( CustomServiceRequest request = info->listener_->getRequest(); if (request.buffer_ != nullptr) { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (info->request_type_support_->deserializeROSmessage( + if (raw_type_support->deserializeROSmessage( deser, ros_request, info->request_type_support_impl_)) { // Get header diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index a12b8c71a..b062d26b0 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -135,7 +135,7 @@ __rmw_send_response( data.is_cdr_buffer = false; data.data = const_cast(ros_response); data.impl = info->response_type_support_impl_; - if (info->response_publisher_->write(&data, wparams)) { + if (info->response_writer_->write(&data, wparams)) { returnedValue = RMW_RET_OK; } else { RMW_SET_ERROR_MSG("cannot publish data"); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index a53105de6..32f85ff7a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -58,14 +58,14 @@ __rmw_destroy_service( // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_subscriber_->guid()); + identifier, info->request_reader_->guid()); common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_publisher_->guid()); + identifier, info->response_writer_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); @@ -81,10 +81,15 @@ __rmw_destroy_service( auto participant_info = static_cast(node->context->impl->participant_info); - // NOTE: Topic deletion and unregister type is done in the participant if (nullptr != info) { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_writer_->get_topic(); + auto request_topic = info->request_reader_->get_topicdescription(); + // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_subscriber_); + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -96,7 +101,7 @@ __rmw_destroy_service( } // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->response_publisher_); + ret = participant_info->publisher_->delete_datawriter(info->response_writer_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete datareader"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); @@ -107,15 +112,9 @@ __rmw_destroy_service( delete info->pub_listener_; } - // Delete request type support inside subscription - if (info->request_type_support_ != nullptr) { - delete info->request_type_support_; - } - - // Delete response type support inside subscription - if (info->response_type_support_ != nullptr) { - delete info->response_type_support_; - } + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); // Delete ServiceInfo structure delete info; From 6beea1f2e8bcdf724842f7d1592b41925ab37df6 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 9 Mar 2021 07:56:12 +0100 Subject: [PATCH 22/60] Removed create_topic_rmw Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 8 ------- rmw_fastrtps_shared_cpp/src/utils.cpp | 24 ------------------- 2 files changed, 32 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 36c431cd5..5899268f7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -53,14 +53,6 @@ remove_topic_and_type( const eprosima::fastdds::dds::TopicDescription * topic, const eprosima::fastdds::dds::TypeSupport & type); -RMW_FASTRTPS_SHARED_CPP_PUBLIC -eprosima::fastdds::dds::TopicDescription * -create_topic_rmw( - const CustomParticipantInfo * participant_info, - const std::string & topic_name, - const std::string & type_name, - const eprosima::fastdds::dds::TopicQos & qos); - } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 8463f853d..7b528ae8d 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -85,28 +85,4 @@ remove_topic_and_type( } } -eprosima::fastdds::dds::TopicDescription * create_topic_rmw( - const CustomParticipantInfo * participant_info, - const std::string & topic_name, - const std::string & type_name, - const eprosima::fastdds::dds::TopicQos & qos) -{ - // Searchs for an already existing topic - eprosima::fastdds::dds::TopicDescription * des_topic = - participant_info->participant_->lookup_topicdescription(topic_name); - if (nullptr != des_topic) { - // If this is not an instantiated object of type Topic, it cannot be used to - // create a new DataWriter and though the publisher creation will fail - return des_topic; - } - - // Creates topic - eprosima::fastdds::dds::Topic * topic = participant_info->participant_->create_topic( - topic_name, - type_name, - qos); - participant_info->topics_list_.push_back(topic); - return topic; -} - } // namespace rmw_fastrtps_shared_cpp From 179679b0c5656c76d44e98b11b521c29de60552c Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 9 Mar 2021 07:56:35 +0100 Subject: [PATCH 23/60] Removed topics_list_ Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 4 +--- rmw_fastrtps_shared_cpp/src/participant.cpp | 9 --------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 69110682c..af9daadff 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -58,9 +58,7 @@ typedef struct CustomParticipantInfo eprosima::fastdds::dds::Publisher * publisher_{nullptr}; eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; - // Struct to store every topic created in the local domain - // These variables needs to be - mutable std::vector topics_list_; + // Protects creation and destruction of topics, readers and writers mutable std::mutex entity_creation_mutex_; // Flag to establish if the QoS of the participant, diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 8050ce68d..690a5fa36 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -343,15 +343,6 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant return RMW_RET_ERROR; } - // Topics must be deleted before delete the participant - for (auto topic : participant_info->topics_list_) { - ReturnCode_t ret = participant_info->participant_->delete_topic(topic); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); - } - } - // Remove publisher and subcriber from participant ReturnCode_t ret = participant_info->participant_->delete_publisher(participant_info->publisher_); if (ret != ReturnCode_t::RETCODE_OK) { From 972775fe6047a8aa26101477ed6817762adbd115 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 9 Mar 2021 11:31:03 +0100 Subject: [PATCH 24/60] Linters Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 3 ++- rmw_fastrtps_cpp/src/rmw_client.cpp | 6 ++++-- rmw_fastrtps_cpp/src/rmw_service.cpp | 6 ++++-- rmw_fastrtps_cpp/src/subscription.cpp | 3 ++- .../rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp | 7 ++----- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 3 ++- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 12 ++++++++---- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 12 ++++++++---- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 3 ++- .../rmw_fastrtps_shared_cpp/custom_client_info.hpp | 3 +-- .../custom_publisher_info.hpp | 6 +++--- .../custom_subscriber_info.hpp | 4 ++-- .../include/rmw_fastrtps_shared_cpp/guid_utils.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 2 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 3 ++- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 2 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 2 +- 17 files changed, 46 insertions(+), 33 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 34fdd3bdf..cb1058edb 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -139,7 +139,8 @@ rmw_fastrtps_cpp::create_publisher( topic_name_mangled, type_name, des_topic, - fastdds_type)) { + fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_publisher() called for existing topic name %s with incompatible type %s", topic_name_mangled.c_str(), type_name.c_str()); diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index f3ee90b35..c30edc921 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -159,7 +159,8 @@ rmw_create_client( request_topic_name, request_type_name, request_topic, - request_fastdds_type)) { + request_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing request topic name %s with incompatible type %s", request_topic_name.c_str(), request_type_name.c_str()); @@ -174,7 +175,8 @@ rmw_create_client( response_topic_name, response_type_name, response_topic, - response_fastdds_type)) { + response_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing response topic name %s with incompatible type %s", response_topic_name.c_str(), response_type_name.c_str()); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 972a128f3..961e525ba 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -169,7 +169,8 @@ rmw_create_service( request_topic_name, request_type_name, request_topic, - request_fastdds_type)) { + request_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing request topic name %s with incompatible type %s", request_topic_name.c_str(), request_type_name.c_str()); @@ -184,7 +185,8 @@ rmw_create_service( response_topic_name, response_type_name, response_topic, - response_fastdds_type)) { + response_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing response topic name %s with incompatible type %s", response_topic_name.c_str(), response_type_name.c_str()); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 925abaf7c..8985139ce 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -144,7 +144,8 @@ create_subscription( topic_name_mangled, type_name, des_topic, - fastdds_type)) { + fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_subscription() called for existing topic name %s with incompatible type %s", topic_name_mangled.c_str(), type_name.c_str()); diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index 90c679aef..0f83e0017 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -15,16 +15,13 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ -#include "fastcdr/Cdr.h" -#include "fastcdr/FastBuffer.h" -#include "fastcdr/exceptions/Exception.h" - #include #include #include -#include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" +#include "fastcdr/exceptions/Exception.h" #include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" #include "rmw_fastrtps_dynamic_cpp/macros.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index dd00be21e..b8d2f73aa 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -138,7 +138,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( topic_name_mangled, type_name, des_topic, - fastdds_type)) { + fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_publisher() called with existing topic name %s with incompatible type %s", topic_name_mangled.c_str(), type_name.c_str()); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index cc116498a..97bf568bc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -140,8 +140,10 @@ rmw_create_client( const void * untyped_request_members; const void * untyped_response_members; - untyped_request_members = get_request_ptr(type_support->data, type_support->typesupport_identifier); - untyped_response_members = get_response_ptr(type_support->data, type_support->typesupport_identifier); + untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); @@ -161,7 +163,8 @@ rmw_create_client( request_topic_name, request_type_name, request_topic, - request_fastdds_type)) { + request_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing request topic name %s with incompatible type %s", request_topic_name.c_str(), request_type_name.c_str()); @@ -176,7 +179,8 @@ rmw_create_client( response_topic_name, response_type_name, response_topic, - response_fastdds_type)) { + response_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing response topic name %s with incompatible type %s", response_topic_name.c_str(), response_type_name.c_str()); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 0b2fa3a39..fb33de137 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -152,8 +152,10 @@ rmw_create_service( const void * untyped_request_members; const void * untyped_response_members; - untyped_request_members = get_request_ptr(type_support->data, type_support->typesupport_identifier); - untyped_response_members = get_response_ptr(type_support->data, type_support->typesupport_identifier); + untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); @@ -173,7 +175,8 @@ rmw_create_service( request_topic_name, request_type_name, request_topic, - request_fastdds_type)) { + request_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing request topic name %s with incompatible type %s", request_topic_name.c_str(), request_type_name.c_str()); @@ -188,7 +191,8 @@ rmw_create_service( response_topic_name, response_type_name, response_topic, - response_fastdds_type)) { + response_fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing response topic name %s with incompatible type %s", response_topic_name.c_str(), response_type_name.c_str()); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index aa3d6279f..17617ed3a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -144,7 +144,8 @@ create_subscription( topic_name_mangled, type_name, des_topic, - fastdds_type)) { + fastdds_type)) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_publisher() called with existing topic name %s with incompatible type %s", topic_name_mangled.c_str(), type_name.c_str()); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index df0e05446..d5a53778e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -92,8 +92,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true if (reader->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (response.sample_info_.valid_data) - { + if (response.sample_info_.valid_data) { response.sample_identity_ = response.sample_info_.related_sample_identity; if (response.sample_identity_.writer_guid() == info_->reader_guid_ || diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 10cb2362d..35779c60d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -127,15 +127,15 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds mutable std::mutex internalMutex_; std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool deadline_changes_; eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index edcb18ba3..89640db6a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -164,11 +164,11 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds std::atomic_bool deadline_changes_; eprosima::fastrtps::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; eprosima::fastrtps::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp index 7162a5749..e645f952b 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp @@ -63,7 +63,7 @@ struct hash_fastrtps_guid union u_convert { uint8_t plain_value[sizeof(guid)]; uint32_t plain_ints[sizeof(guid) / sizeof(uint32_t)]; - } u; + } u {}; static_assert( sizeof(guid) == 16 && diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 5899268f7..08c5b4354 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -35,7 +35,7 @@ namespace rmw_fastrtps_shared_cpp RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t -cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); + cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 690a5fa36..304cb90f7 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -132,7 +132,8 @@ __create_participant( participant_info->participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( - static_cast(domain_id), domainParticipantQos, participant_info->listener_, participant_mask); + static_cast(domain_id), domainParticipantQos, + participant_info->listener_, participant_mask); if (!participant_info->participant_) { RMW_SET_ERROR_MSG("__create_participant failed to create participant"); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index b062d26b0..e913c354d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -69,7 +69,7 @@ __rmw_take_response( request_header->request_id.sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; - + *taken = true; } } diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 7b528ae8d..f94ea65b1 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -72,7 +72,7 @@ remove_topic_and_type( const eprosima::fastdds::dds::TopicDescription * topic_desc, const eprosima::fastdds::dds::TypeSupport & type) { - // TODO (Miguel C): We only create Topic instances at the moment, but this may + // TODO(MiguelCompany): We only create Topic instances at the moment, but this may // change in the future if we start supporting other kinds of TopicDescription // (like ContentFilteredTopic) auto topic = dynamic_cast(topic_desc); From 5d2b138a57256d8f8151e2ae3cf3d8670311c396 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 11 Mar 2021 11:35:03 +0100 Subject: [PATCH 25/60] Addressed internal review comments Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/CMakeLists.txt | 4 +- .../rmw_fastrtps_cpp/get_subscriber.hpp | 4 +- .../include/rmw_fastrtps_cpp/publisher.hpp | 2 +- .../include/rmw_fastrtps_cpp/subscription.hpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 66 ++++++++++++------- .../src/rmw_compare_gids_equal.cpp | 2 - rmw_fastrtps_cpp/src/rmw_publisher.cpp | 1 - rmw_fastrtps_cpp/src/subscription.cpp | 46 +++++++++---- rmw_fastrtps_cpp/src/type_support_common.hpp | 3 - rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 4 +- .../get_subscriber.hpp | 4 +- .../rmw_fastrtps_dynamic_cpp/publisher.hpp | 2 +- .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 2 +- .../src/get_participant.cpp | 3 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 64 ++++++++++-------- .../src/rmw_compare_gids_equal.cpp | 2 - rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 46 +++++++++---- .../src/type_support_common.cpp | 8 --- .../src/type_support_common.hpp | 5 -- .../test/test_logging.cpp | 14 ++-- rmw_fastrtps_shared_cpp/CMakeLists.txt | 4 +- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 1 - .../custom_client_info.hpp | 7 +- .../custom_participant_info.hpp | 9 ++- .../custom_publisher_info.hpp | 7 +- .../custom_service_info.hpp | 12 ++-- .../custom_subscriber_info.hpp | 32 ++++----- .../rmw_fastrtps_shared_cpp/participant.hpp | 9 ++- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 40 ++++++----- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 5 +- .../src/create_rmw_gid.cpp | 2 +- .../src/custom_publisher_info.cpp | 4 +- .../src/custom_subscriber_info.cpp | 8 ++- rmw_fastrtps_shared_cpp/src/participant.cpp | 28 ++++---- rmw_fastrtps_shared_cpp/src/publisher.cpp | 10 +-- rmw_fastrtps_shared_cpp/src/qos.cpp | 54 ++++----------- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_logging.cpp | 5 +- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 7 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 2 + rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 2 +- .../src/rmw_security_logging.cpp | 15 +++-- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 4 +- .../src/rmw_subscription.cpp | 7 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 2 - rmw_fastrtps_shared_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 9 ++- .../test/test_guid_utils.cpp | 4 +- rmw_fastrtps_shared_cpp/test/test_logging.cpp | 14 ++-- .../test/test_rmw_qos_to_dds_attributes.cpp | 21 +++--- .../test/test_security_logging.cpp | 5 +- 52 files changed, 342 insertions(+), 281 deletions(-) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 3b376b9cc..744eb7b6e 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -38,8 +38,8 @@ find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_runtime_c REQUIRED) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp index c2ffac1ff..69af229a2 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -22,12 +22,12 @@ namespace rmw_fastrtps_cpp { -/// Return a native Fast DDS subscriber handle. +/// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or * when the subscription handle is from a different rmw implementation. * - * \return native Fast DDS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataReader * diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp index a569d5595..037930f55 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // unused + bool keyed, bool create_publisher_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp index 8f66b56fa..3413cea4c 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, // unused + bool keyed, bool create_subscription_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index cb1058edb..7920dd1aa 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -15,12 +15,15 @@ #include +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -56,8 +59,6 @@ rmw_fastrtps_cpp::create_publisher( bool keyed, bool create_publisher_listener) { - (void)keyed; - ///// // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -66,7 +67,7 @@ rmw_fastrtps_cpp::create_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_publisher() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -78,7 +79,8 @@ rmw_fastrtps_cpp::create_publisher( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with invalid topic name: %s", reason); return nullptr; } } @@ -87,7 +89,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); return nullptr; } @@ -146,9 +148,9 @@ rmw_fastrtps_cpp::create_publisher( topic_name_mangled.c_str(), type_name.c_str()); return nullptr; } - + ///// - // Create the RMW Publisher struct (info) + // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; info = new (std::nothrow) CustomPublisherInfo(); @@ -177,6 +179,11 @@ rmw_fastrtps_cpp::create_publisher( // Transfer ownership to fastdds_type fastdds_type.reset(tsupport); } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); @@ -207,38 +214,50 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create and register Topic + bool topic_created = false; + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS for publisher"); return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } + + topic_created = true; } - - eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; + else { + topic = dynamic_cast(des_topic); + if (!topic) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic, topic_created]() { + if (topic_created) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataWriter // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name @@ -258,12 +277,13 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - // Get QoS from ROS + // Get QoS from RMW if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->data_writer_ = publisher->create_datawriter( topic, dataWriterQos, @@ -287,11 +307,9 @@ rmw_fastrtps_cpp::create_publisher( ///// // Allocate publisher - rmw_publisher_t * rmw_publisher = nullptr; - - rmw_publisher = rmw_publisher_allocate(); + rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate rmw_publisher"); return nullptr; } auto cleanup_rmw_publisher = rcpputils::make_scope_exit( @@ -300,12 +318,13 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher_free(rmw_publisher); }); + rmw_publisher->can_loan_messages = false; rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate memory for rmw_publisher topic name"); return nullptr; } memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); @@ -314,6 +333,7 @@ rmw_fastrtps_cpp::create_publisher( cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp index 8cf195c6b..7c91a4108 100644 --- a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" - #include "rmw/rmw.h" #include "rmw/error_handling.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 8603c6605..81ad0c1e7 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -86,7 +86,6 @@ rmw_create_publisher( true); if (!publisher) { - RMW_SET_ERROR_MSG("Publisher creation failed"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 8985139ce..474a1eab8 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -20,9 +20,11 @@ #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" -#include "fastdds/dds/topic/TopicDataType.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcutils/allocator.h" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -61,8 +63,6 @@ create_subscription( bool keyed, bool create_subscription_listener) { - (void)keyed; - ///// // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -71,7 +71,7 @@ create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -83,7 +83,8 @@ create_subscription( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic_name argument: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with invalid topic name: %s", reason); return nullptr; } } @@ -92,7 +93,7 @@ create_subscription( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); return nullptr; } @@ -158,7 +159,7 @@ create_subscription( info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); return nullptr; } @@ -183,6 +184,11 @@ create_subscription( fastdds_type.reset(tsupport); } + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; @@ -212,31 +218,42 @@ create_subscription( ///// // Create and register Topic + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting topic QoS"); return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } + + des_topic = topic; } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic]() { + if (topic) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataReader // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name @@ -250,6 +267,7 @@ create_subscription( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } @@ -260,7 +278,7 @@ create_subscription( info->listener_); if (!info->data_reader_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); return nullptr; } @@ -279,7 +297,7 @@ create_subscription( // Allocate subscription rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); return nullptr; } auto cleanup_rmw_subscription = rcpputils::make_scope_exit( @@ -293,7 +311,8 @@ create_subscription( rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); return nullptr; } rmw_subscription->options = *subscription_options; @@ -301,6 +320,7 @@ create_subscription( cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 44da0ca7f..a86d45e1d 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -18,9 +18,6 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" -#include "fastdds/dds/topic/TopicDataType.hpp" - #include "rmw/error_handling.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index ec43fe20f..9e035a7de 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -38,8 +38,8 @@ find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_runtime_c REQUIRED) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp index 24dfe6acd..34812e883 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -23,12 +23,12 @@ namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS subscriber handle. +/// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or * when the subscription handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataReader * diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp index 4d419346c..4f98b6c14 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // unused + bool keyed, bool create_publisher_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp index 3113ca04c..0db879040 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, // unused + bool keyed, bool create_subscription_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index 471f7e508..34c046c01 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" + #include "fastdds/dds/domain/DomainParticipant.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" namespace rmw_fastrtps_dynamic_cpp { diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index b8d2f73aa..df8a2c471 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -20,8 +20,11 @@ #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcutils/error_handling.h" #include "rmw/allocators.h" @@ -59,13 +62,13 @@ rmw_fastrtps_dynamic_cpp::create_publisher( { ///// // Check input parameters - (void)keyed; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_publisher() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -77,7 +80,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with invalid topic name: %s", reason); return nullptr; } } @@ -86,7 +90,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); return nullptr; } @@ -147,7 +151,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } ///// - // Create the RMW Publisher struct (info) + // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; info = new (std::nothrow) CustomPublisherInfo(); @@ -166,7 +170,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_impl = type_registry.get_message_type_support(type_support); if (!type_impl) { - RMW_SET_ERROR_MSG("failed to allocate type support"); + RMW_SET_ERROR_MSG("create_publisher() failed to get message_type_support"); return nullptr; } auto return_type_support = rcpputils::make_scope_exit( @@ -188,6 +192,11 @@ rmw_fastrtps_dynamic_cpp::create_publisher( fastdds_type.reset(tsupport); } + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; @@ -218,6 +227,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create and register Topic + bool topic_created = false; + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); @@ -227,29 +238,37 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } } - - eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; + else { + topic = dynamic_cast(des_topic); + if (!topic) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic, topic_created]() { + if (topic_created) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataWriter // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name @@ -269,8 +288,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - // Get QoS from ROS + // Get QoS from RMW if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } @@ -291,14 +311,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( publisher->delete_datawriter(info->data_writer_); }); - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - ///// // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -307,9 +319,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Allocate publisher rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate rmw_publisher"); return nullptr; } auto cleanup_rmw_publisher = rcpputils::make_scope_exit( @@ -324,7 +335,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate memory for rmw_publisher topic name"); return nullptr; } memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); @@ -333,6 +344,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); return_type_support.cancel(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp index 3c5cab0f4..9ebeb4b8b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" - #include "rmw/rmw.h" #include "rmw/error_handling.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 17617ed3a..863d60c7c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -20,9 +20,11 @@ #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" -#include "fastdds/dds/topic/TopicDataType.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcutils/error_handling.h" #include "rmw/allocators.h" @@ -69,7 +71,7 @@ create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -81,7 +83,8 @@ create_subscription( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic_name argument: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with invalid topic name: %s", reason); return nullptr; } } @@ -92,7 +95,7 @@ create_subscription( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); return nullptr; } @@ -158,7 +161,7 @@ create_subscription( info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); return nullptr; } @@ -172,7 +175,7 @@ create_subscription( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_support_impl = type_registry.get_message_type_support(type_support); if (!type_support_impl) { - RMW_SET_ERROR_MSG("failed to allocate type support"); + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); return nullptr; } auto return_type_support = rcpputils::make_scope_exit( @@ -194,6 +197,11 @@ create_subscription( fastdds_type.reset(tsupport); } + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; @@ -224,31 +232,42 @@ create_subscription( ///// // Create and register Topic + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting topic QoS"); return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } + + des_topic = topic; } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic]() { + if (topic) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataReader // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name @@ -262,6 +281,7 @@ create_subscription( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } @@ -272,7 +292,7 @@ create_subscription( info->listener_); if (!info->data_reader_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create data reader"); + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); return nullptr; } @@ -289,7 +309,7 @@ create_subscription( rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); return nullptr; } auto cleanup_rmw_subscription = rcpputils::make_scope_exit( @@ -303,7 +323,8 @@ create_subscription( rmw_subscription->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); return nullptr; } memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); @@ -313,6 +334,7 @@ create_subscription( cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); return_type_support.cancel(); diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index 88109944e..d60206970 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -34,11 +34,3 @@ using_introspection_cpp_typesupport(const char * typesupport_identifier) return typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier; } - -// void -// _register_type( -// eprosima::fastrtps::Participant * participant, -// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) -// { -// eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); -// } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp index f0756fec8..1846b7ba5 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -109,9 +109,4 @@ _create_type_name( return ""; } -// void -// _register_type( -// eprosima::fastrtps::Participant * participant, -// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport); - #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp index 9f68be552..1892537e5 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp @@ -14,28 +14,30 @@ #include "gtest/gtest.h" -#include "fastrtps/log/Log.h" +#include "fastdds/dds/log/Log.hpp" #include "rmw/rmw.h" #include "rmw/error_handling.h" TEST(TestLogging, rmw_logging) { + using Log = eprosima::fastdds::dds::Log; + rmw_ret_t ret = rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Warning, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_FATAL); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); } TEST(TestLogging, rmw_logging_bad_verbosity) diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index d4b3ab88c..bce4e76db 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -41,8 +41,8 @@ find_package(rmw_dds_common REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 8d715bd65..cc3f2548d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -18,7 +18,6 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDataType.hpp" #include "fastcdr/FastBuffer.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index d5a53778e..dad3c01e3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -26,7 +26,8 @@ #include "fastcdr/FastBuffer.h" -#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" @@ -34,6 +35,10 @@ #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index af9daadff..1ae23eb20 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -24,7 +24,10 @@ #include "fastdds/dds/domain/DomainParticipantListener.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/subscriber/Subscriber.hpp" -#include "fastdds/rtps/common/Types.h" + +#include "fastdds/rtps/participant/ParticipantDiscoveryInfo.h" +#include "fastdds/rtps/reader/ReaderDiscoveryInfo.h" +#include "fastdds/rtps/writer/WriterDiscoveryInfo.h" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" @@ -61,8 +64,8 @@ typedef struct CustomParticipantInfo // Protects creation and destruction of topics, readers and writers mutable std::mutex entity_creation_mutex_; - // Flag to establish if the QoS of the participant, - // its publishers and its subscribers are going + // Flag to establish if the QoS of the DomainParticipant, + // its DataWriters, and its DataReaders are going // to be configured only from an XML file or if // their settings are going to be overwritten by code // with the default configuration. diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 35779c60d..b3492fb96 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -20,12 +20,17 @@ #include #include +#include "fastdds/dds/core/status/BaseStatus.hpp" #include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/topic/Topic.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" @@ -63,7 +68,7 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds (void) info; } - // PublisherListener implementation + // DataWriterListener implementation RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_publication_matched( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 7ef5371e4..ea17ef4d9 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -24,14 +24,20 @@ #include "fastcdr/FastBuffer.h" -#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/InstanceState.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" @@ -179,15 +185,13 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener : info_(info), list_has_data_(false), conditionMutex_(nullptr), conditionVariable_(nullptr) { - (void)info_; } void on_subscription_matched( - eprosima::fastdds::dds::DataReader * reader, + eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { - (void) reader; if (info.current_count_change == -1) { info_->pub_listener_->endpoint_erase_if_exists( eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 89640db6a..379a42703 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -21,10 +21,16 @@ #include #include +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/impl/cpp/macros.hpp" @@ -65,7 +71,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds (void)info; } - // SubscriberListener implementation + // DataReaderListener implementation void on_subscription_matched( eprosima::fastdds::dds::DataReader * reader, @@ -79,13 +85,13 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } } - update_unread_count(reader); + update_has_data(reader); } void on_data_available(eprosima::fastdds::dds::DataReader * reader) final { - update_unread_count(reader); + update_has_data(reader); } RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -133,18 +139,12 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds } void - update_unread_count(eprosima::fastdds::dds::DataReader * reader) + update_has_data(eprosima::fastdds::dds::DataReader * reader) { - // Make sure to call into Fast-RTPS before taking the lock to avoid an - // ABBA deadlock between internalMutex_ and mutexes inside of Fast-RTPS. - eprosima::fastdds::dds::SampleInfo info; - ReturnCode_t has_data_ret = reader->get_first_untaken_info(&info); - - // In case there is data, get_first_untaken_info return OK. Else it returs NO_DATA - bool has_data = false; - if (has_data_ret == ReturnCode_t::RETCODE_OK) { - has_data = true; - } + // Make sure to call into Fast DDS before taking the lock to avoid an + // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. + auto unread_count = reader->get_unread_count(); + bool has_data = unread_count > 0; std::lock_guard lock(internalMutex_); ConditionalScopedLock clock(conditionMutex_, conditionVariable_); @@ -163,11 +163,11 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds std::atomic_bool data_; std::atomic_bool deadline_changes_; - eprosima::fastrtps::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; - eprosima::fastrtps::LivelinessChangedStatus liveliness_changed_status_ + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 7d851814d..a123a304e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -25,9 +25,12 @@ namespace rmw_fastrtps_shared_cpp { -// The Entities Publisher and Subscriber are created with the Participant -// Endpoint SubEntities as DataWriter and DataReader are created with -// create_publisher and create_subscription +// This method will create a DDS DomainParticipant along with +// a DDS Publisher and a DDS Subscriber +// For the creation of DDS DataWriter see method create_publisher +// For the creation of DDS DataReader see method create_subscription +// Note that ROS 2 Publishers and Subscriptions correspond with DDS DataWriters +// and DataReaders respectively and not with DDS Publishers and Subscribers. RMW_FASTRTPS_SHARED_CPP_PUBLIC CustomParticipantInfo * create_participant( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index c95373499..cb41265d0 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -16,9 +16,7 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ -#include #include -#include #include #include @@ -53,12 +51,10 @@ rmw_time_t dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); /* - * Converts the low-level QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. - * Since DataWriterQos or DataReaderQos does not have information about history and depth, these values are not set - * by this function. + * Converts the DDS QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. * * \param[in] dds_qos of type DataWriterQos or DataReaderQos - * \param[out] qos the equivalent of the data in DataWriterQos or DataReaderQos in rmw_qos_profile_t + * \param[out] qos the equivalent of the data in dds_qos as a rmw_qos_profile_t */ template void @@ -67,10 +63,10 @@ dds_qos_to_rmw_qos( rmw_qos_profile_t * qos) { switch (dds_qos.reliability().kind) { - case eprosima::fastdds::dds::ReliabilityQosPolicyKind::BEST_EFFORT_RELIABILITY_QOS: + case eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; - case eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS: + case eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; break; default: @@ -121,13 +117,21 @@ dds_qos_to_rmw_qos( qos->depth = static_cast(dds_qos.history().depth); } -template +/* + * Converts the RTPS QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. + * Since WriterQos or ReaderQos do not have information about history and depth, + * these values are not set by this function. + * + * \param[in] rtps_qos of type WriterQos or ReaderQos + * \param[out] qos the equivalent of the data in rtps_qos as a rmw_qos_profile_t + */ +template void rtps_qos_to_rmw_qos( - const DDSQoSPolicyT & dds_qos, + const RTPSQoSPolicyT & rtps_qos, rmw_qos_profile_t * qos) { - switch (dds_qos.m_reliability.kind) { + switch (rtps_qos.m_reliability.kind) { case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; @@ -139,7 +143,7 @@ rtps_qos_to_rmw_qos( break; } - switch (dds_qos.m_durability.kind) { + switch (rtps_qos.m_durability.kind) { case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; break; @@ -151,13 +155,13 @@ rtps_qos_to_rmw_qos( break; } - qos->deadline.sec = dds_qos.m_deadline.period.seconds; - qos->deadline.nsec = dds_qos.m_deadline.period.nanosec; + qos->deadline.sec = rtps_qos.m_deadline.period.seconds; + qos->deadline.nsec = rtps_qos.m_deadline.period.nanosec; - qos->lifespan.sec = dds_qos.m_lifespan.duration.seconds; - qos->lifespan.nsec = dds_qos.m_lifespan.duration.nanosec; + qos->lifespan.sec = rtps_qos.m_lifespan.duration.seconds; + qos->lifespan.nsec = rtps_qos.m_lifespan.duration.nanosec; - switch (dds_qos.m_liveliness.kind) { + switch (rtps_qos.m_liveliness.kind) { case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; break; @@ -168,7 +172,7 @@ rtps_qos_to_rmw_qos( qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; break; } - qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.m_liveliness.lease_duration); + qos->liveliness_lease_duration = dds_duration_to_rmw(rtps_qos.m_liveliness.lease_duration); } extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 08c5b4354..6b12c0bce 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,9 +18,8 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDescription.hpp" -#include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "fastrtps/types/TypesBase.h" diff --git a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp index 72ffb3087..49b90492b 100644 --- a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index f910daca9..33e6c13f8 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "fastdds/dds/core/status/BaseStatus.hpp" +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" #include "types/event_types.hpp" EventListenerInterface * @@ -42,7 +44,7 @@ PubListener::on_offered_deadline_missed( void PubListener::on_liveliness_lost( eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastrtps::LivelinessLostStatus & status) + const eprosima::fastdds::dds::LivelinessLostStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 8af36904a..d9eb011d1 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -13,6 +13,10 @@ // limitations under the License. #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" + +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" + #include "types/event_types.hpp" EventListenerInterface * @@ -24,7 +28,7 @@ CustomSubscriberInfo::getListener() const void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader * /* reader */, - const eprosima::fastrtps::RequestedDeadlineMissedStatus & status) + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -42,7 +46,7 @@ SubListener::on_requested_deadline_missed( void SubListener::on_liveliness_changed( eprosima::fastdds::dds::DataReader * /* reader */, - const eprosima::fastrtps::LivelinessChangedStatus & status) + const eprosima::fastdds::dds::LivelinessChangedStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 304cb90f7..dbb8031c4 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -16,10 +16,13 @@ #include #include +#include "fastdds/dds/core/status/StatusMask.hpp" #include "fastdds/dds/domain/DomainParticipantFactory.hpp" #include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" #include "fastdds/dds/publisher/qos/PublisherQos.hpp" #include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" +#include "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/common/Property.h" #include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" #include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" @@ -73,17 +76,15 @@ get_security_file_paths( #endif // Private function to create Participant with QoS -CustomParticipantInfo * +static CustomParticipantInfo * __create_participant( const char * identifier, - eprosima::fastdds::dds::DomainParticipantQos domainParticipantQos, + const eprosima::fastdds::dds::DomainParticipantQos & domainParticipantQos, bool leave_middleware_default_qos, publishing_mode_t publishing_mode, rmw_dds_common::Context * common_context, size_t domain_id) { - ///// - // Declare everything before beginning to create things. CustomParticipantInfo * participant_info = nullptr; ///// @@ -92,6 +93,7 @@ __create_participant( participant_info = new CustomParticipantInfo(); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("__create_participant failed to allocate CustomParticipantInfo struct"); + return nullptr; } // lambda to delete participant info auto cleanup_participant_info = rcpputils::make_scope_exit( @@ -106,6 +108,7 @@ __create_participant( identifier, common_context); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); + return nullptr; } // lambda to delete listener auto cleanup_listener = rcpputils::make_scope_exit( @@ -113,21 +116,12 @@ __create_participant( delete participant_info->listener_; }); - ///// - // Force to load XML configuration file - if (ReturnCode_t::RETCODE_OK != - eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles()) - { - RMW_SET_ERROR_MSG("__create_participant failed to load FastDDS XML configuration file"); - } - ///// // Create Participant - // Create Participant Listener mask so it only uses the domain - // participant listener non inheritated callbacks - // Without this mask, callback of DataReaders on_data_available is never called - // Publisher and subscriber dont need masks as they do not have listeners + // As the participant listener is only used for discovery related callbacks, which are + // Fast DDS extensions to the DDS standard DomainParticipantListener interface, an empty + // mask should be used to let child entities handle standard DDS events. eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); participant_info->participant_ = @@ -222,7 +216,7 @@ rmw_fastrtps_shared_cpp::create_participant( // Add SHM transport if available auto shm_transport = std::make_shared(); - domainParticipantQos.transport().user_transports.push_back(udp_transport); + domainParticipantQos.transport().user_transports.push_back(shm_transport); } size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 69913bcb8..8cd4748f6 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" - #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -25,9 +22,6 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; - namespace rmw_fastrtps_shared_cpp { rmw_ret_t @@ -52,7 +46,7 @@ destroy_publisher( // Delete DataWriter ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -64,7 +58,7 @@ destroy_publisher( // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); - // Delete PublisherInfo structure + // Delete CustomPublisherInfo structure delete info; } else { ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index faf08afa9..1bc603f8c 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -16,10 +16,9 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" -#include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" -#include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" #include "fastdds/rtps/common/Time_t.h" #include "rmw/error_handling.h" @@ -56,7 +55,8 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) return result; } -// Private function to encapsulate DataReader and DataWriter together with TopicQos fill entities +// Private function to encapsulate DataReader and DataWriter, together with Topic, filling +// entities DDS QoS from the RMW QoS profile. template bool fill_entity_qos_from_profile( const rmw_qos_profile_t & qos_policies, @@ -64,10 +64,10 @@ bool fill_entity_qos_from_profile( { switch (qos_policies.history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - entity_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - entity_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: break; @@ -78,10 +78,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.durability) { case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - entity_qos.durability().kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_VOLATILE: - entity_qos.durability().kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: break; @@ -92,10 +92,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.reliability) { case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - entity_qos.reliability().kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - entity_qos.reliability().kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: break; @@ -105,7 +105,7 @@ bool fill_entity_qos_from_profile( } // ensure the history depth is at least the requested queue size - assert(entity_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? + assert(entity_qos.history().depth >= 0); if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && static_cast(entity_qos.history().depth) < qos_policies.depth) @@ -128,10 +128,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.liveliness) { case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: - entity_qos.liveliness().kind = eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: - entity_qos.liveliness().kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: break; @@ -175,35 +175,7 @@ get_topic_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastdds::dds::TopicQos & topic_qos) { - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - topic_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - topic_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - // ensure the history depth is at least the requested queue size - assert(topic_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? - if ( - qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(topic_qos.history().depth) < qos_policies.depth) - { - if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - topic_qos.history().depth = static_cast(qos_policies.depth); - } - - return true; + return fill_entity_qos_from_profile(qos_policies, topic_qos);; } bool diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index ba1ba8e14..1a5046c57 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -90,7 +90,7 @@ __rmw_destroy_client( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -103,7 +103,7 @@ __rmw_destroy_client( remove_topic_and_type(participant_info, request_topic, info->request_type_support_); remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - // Delete ClientInfo structure + // Delete CustomClientInfo structure delete info; } else { final_ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp index 60e974894..db6b86a74 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp @@ -23,11 +23,12 @@ namespace rmw_fastrtps_shared_cpp { -using eprosima::fastdds::dds::Log; rmw_ret_t __rmw_set_log_severity(rmw_log_severity_t severity) { + using eprosima::fastdds::dds::Log; + Log::Kind log_kind; switch (severity) { @@ -49,7 +50,7 @@ __rmw_set_log_severity(rmw_log_severity_t severity) return RMW_RET_ERROR; } - eprosima::fastdds::dds::Log::SetVerbosity(log_kind); + Log::SetVerbosity(log_kind); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index b16195269..8a1bd6a0d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -21,7 +21,7 @@ #include "rmw/rmw.h" #include "fastdds/dds/publisher/DataWriter.hpp" -#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" @@ -125,9 +125,8 @@ __rmw_publisher_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(publisher->data); - eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->data_writer_; - const eprosima::fastdds::dds::DataWriterQos & dds_qos = - fastrtps_pub->get_qos(); + eprosima::fastdds::dds::DataWriter * fastdds_dw = info->data_writer_; + const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastdds_dw->get_qos(); dds_qos_to_rmw_qos(dds_qos, qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index c89bea3c9..73ce6c534 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -17,6 +17,8 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" +#include "fastdds/rtps/common/WriteParams.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/impl/cpp/macros.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index e913c354d..1766e4bbd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -16,7 +16,7 @@ #include "fastcdr/Cdr.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/rtps/common/WriteParams.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp index 339d70ba8..0d09c442a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp @@ -12,20 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" + #include #include #include #include +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + #include "fastrtps/config.h" + #include "rcutils/filesystem.h" #include "rcutils/get_env.h" + #include "rmw/error_handling.h" #include "rmw/qos_profiles.h" #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" - #if HAVE_SECURITY namespace @@ -44,7 +49,7 @@ const char verbosity_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.logging const char distribute_enable_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.distribute"; -// Fast RTPS supports the following verbosities: +// Fast DDS supports the following verbosities: // - EMERGENCY_LEVEL // - ALERT_LEVEL // - CRITICAL_LEVEL @@ -211,8 +216,8 @@ bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPoli #else (void)policy; RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); + "This Fast DDS version doesn't have the security libraries\n" + "Please compile Fast DDS using the -DSECURITY=ON CMake option"); return false; #endif } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 32f85ff7a..e38d780e2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -103,7 +103,7 @@ __rmw_destroy_service( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->response_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -116,7 +116,7 @@ __rmw_destroy_service( remove_topic_and_type(participant_info, request_topic, info->request_type_support_); remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - // Delete ServiceInfo structure + // Delete CustomServiceInfo structure delete info; } else { final_ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index ab9bf5416..98f42159f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -21,7 +21,7 @@ #include "rmw/rmw.h" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -99,9 +99,8 @@ __rmw_subscription_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader * fastrtps_sub = info->data_reader_; - const eprosima::fastdds::dds::DataReaderQos & dds_qos = - fastrtps_sub->get_qos(); + eprosima::fastdds::dds::DataReader * fastdds_dr = info->data_reader_; + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->get_qos(); dds_qos_to_rmw_qos(dds_qos, qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 5fd17b58a..ec75d5f3e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -79,7 +79,7 @@ _take( data.impl = info->type_support_impl_; if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->data_reader_); + info->listener_->update_has_data(info->data_reader_); if (sinfo.valid_data) { if (message_info) { @@ -287,7 +287,7 @@ _take_serialized_message( if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->data_reader_); + info->listener_->update_has_data(info->data_reader_); if (sinfo.valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index df35f97d8..d3cfa24dc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/subscriber/Subscriber.h" - #include "rcutils/macros.h" #include "rmw/error_handling.h" diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index f58acc562..e070c92a5 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -64,7 +64,7 @@ destroy_subscription( // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); - // Delete SubscriberInfo structure + // Delete CustomSubscriberInfo structure delete info; } else { ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index f94ea65b1..31acf8e15 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -16,9 +16,14 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" -#include "rmw/rmw.h" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + #include "fastrtps/types/TypesBase.h" +#include "rmw/rmw.h" + using ReturnCode_t = eprosima::fastrtps::types::ReturnCode_t; namespace rmw_fastrtps_shared_cpp @@ -30,7 +35,7 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) if (code == ReturnCode_t::RETCODE_OK) { return RMW_RET_OK; } else if (code == ReturnCode_t::RETCODE_ERROR) { - // repeats the error to avoid too much if comparations + // repeats the error to avoid too many 'if' comparisons return RMW_RET_ERROR; } else if (code == ReturnCode_t::RETCODE_TIMEOUT) { return RMW_RET_TIMEOUT; diff --git a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp index 588da9ecd..3bba2a29c 100644 --- a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp @@ -14,7 +14,9 @@ #include "gtest/gtest.h" -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/EntityId_t.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/GuidPrefix_t.hpp" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" diff --git a/rmw_fastrtps_shared_cpp/test/test_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_logging.cpp index 73365e05b..e2b158035 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -14,7 +14,7 @@ #include "gtest/gtest.h" -#include "fastrtps/log/Log.h" +#include "fastdds/dds/log/Log.hpp" #include "rmw/rmw.h" #include "rmw/error_handling.h" @@ -23,21 +23,23 @@ TEST(TestLogging, rmw_logging) { + using eprosima::fastdds::dds::Log; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Warning, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_FATAL); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); } TEST(TestLogging, rmw_logging_bad_verbosity) diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 7a554d9b8..b080a1393 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -17,8 +17,9 @@ #include "gtest/gtest.h" -#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" @@ -80,13 +81,13 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_EQ( - eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, subscriber_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, subscriber_qos_.durability().kind); EXPECT_EQ( - eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, subscriber_qos_.liveliness().kind); EXPECT_EQ(0, subscriber_qos_.lifespan().duration.seconds); EXPECT_EQ(500000000u, subscriber_qos_.lifespan().duration.nanosec); @@ -95,7 +96,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { EXPECT_EQ(10, subscriber_qos_.liveliness().lease_duration.seconds); EXPECT_EQ(0u, subscriber_qos_.liveliness().lease_duration.nanosec); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, subscriber_qos_.history().kind); EXPECT_GE(10, subscriber_qos_.history().depth); } @@ -178,13 +179,13 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( - eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, publisher_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, publisher_qos_.durability().kind); EXPECT_EQ( - eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, publisher_qos_.liveliness().kind); EXPECT_EQ(0, publisher_qos_.lifespan().duration.seconds); EXPECT_EQ(500000000u, publisher_qos_.lifespan().duration.nanosec); @@ -193,7 +194,7 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { EXPECT_EQ(10, publisher_qos_.liveliness().lease_duration.seconds); EXPECT_EQ(0u, publisher_qos_.liveliness().lease_duration.nanosec); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, publisher_qos_.history().kind); EXPECT_GE(10, publisher_qos_.history().depth); } @@ -206,7 +207,7 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, publisher_qos_.history().kind); EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); diff --git a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp index 81fb7cbc8..e1188e2cf 100644 --- a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp @@ -16,6 +16,9 @@ #include #include +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + #include "fastrtps/config.h" #include "rcutils/filesystem.h" #include "rmw/error_handling.h" @@ -266,7 +269,7 @@ TEST_F(SecurityLoggingTest, test_apply_logging_fails) eprosima::fastrtps::rtps::PropertyPolicy policy; EXPECT_FALSE(apply_security_logging_configuration(policy)); EXPECT_TRUE(rmw_error_is_set()); - EXPECT_THAT(rmw_get_error_string().str, HasSubstr("Please compile Fast-RTPS")); + EXPECT_THAT(rmw_get_error_string().str, HasSubstr("Please compile Fast DDS")); } #endif From db3a1abaec359e5daa3f8aa44fcc95c730cf1830 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 15 Mar 2021 09:57:25 +0100 Subject: [PATCH 26/60] Addressed internal review comments Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/rmw_client.cpp | 105 +++++++++------ rmw_fastrtps_cpp/src/rmw_service.cpp | 107 +++++++++------ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 125 +++++++++++------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 122 ++++++++++------- .../src/rmw_service_server_is_available.cpp | 6 +- 5 files changed, 286 insertions(+), 179 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c30edc921..9d8a8516f 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -14,15 +14,20 @@ #include + +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/subscriber/Subscriber.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcpputils/scope_exit.hpp" #include "rcutils/error_handling.h" #include "rcutils/logging_macros.h" @@ -86,7 +91,7 @@ rmw_create_client( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); return nullptr; } @@ -153,12 +158,12 @@ rmw_create_client( // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; - eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, request_topic_name, request_type_name, - request_topic, + request_topic_desc, request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -169,12 +174,12 @@ rmw_create_client( // Get response topic and type eprosima::fastdds::dds::TypeSupport response_fastdds_type; - eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, response_topic_name, response_type_name, - response_topic, + response_topic_desc, response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -184,10 +189,10 @@ rmw_create_client( } ///// - // Create the RMW Client struct (info) + // Create the custom Client struct (info) CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate client info"); + RMW_SET_ERROR_MSG("create_client() failed to allocate custom info"); return nullptr; } @@ -208,7 +213,7 @@ rmw_create_client( if (!request_fastdds_type) { auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + RMW_SET_ERROR_MSG("create_client() failed to allocate request typesupport"); return nullptr; } @@ -217,7 +222,7 @@ rmw_create_client( if (!response_fastdds_type) { auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + RMW_SET_ERROR_MSG("create_client() failed to allocate response typesupport"); return nullptr; } @@ -235,7 +240,7 @@ rmw_create_client( }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { - RMW_SET_ERROR_MSG("create_client() failed to register request type"); + RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; @@ -248,7 +253,7 @@ rmw_create_client( // Create Listeners info->listener_ = new (std::nothrow) ClientListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); return nullptr; } @@ -259,7 +264,7 @@ rmw_create_client( info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); return nullptr; } @@ -272,44 +277,66 @@ rmw_create_client( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } // Create response topic - if (!response_topic) { + eprosima::fastdds::dds::Topic * response_topic = nullptr; + if (!response_topic_desc) { response_topic = domainParticipant->create_topic( response_topic_name, response_type_name, topicQos); if (!response_topic) { - RMW_SET_ERROR_MSG("failed to create client response topic"); + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); return nullptr; } + + response_topic_desc = response_topic; } + auto cleanup_response_topic = rcpputils::make_scope_exit( + [domainParticipant, response_topic]() { + if (response_topic) { + domainParticipant->delete_topic(response_topic); + } + }); + // Create request topic - if (!request_topic) { + eprosima::fastdds::dds::Topic * request_topic = nullptr; + bool request_topic_created = false; + if (!request_topic_desc) { request_topic = domainParticipant->create_topic( request_topic_name, request_type_name, topicQos); if (!request_topic) { - RMW_SET_ERROR_MSG("failed to create client request topic"); + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; } + + request_topic_desc = request_topic; + request_topic_created = true; } - - eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(request_topic); - if (!pub_topic) { - RMW_SET_ERROR_MSG("failed, client request topic can only be of class Topic"); - return nullptr; + else { + request_topic = dynamic_cast(request_topic_desc); + if (!request_topic) { + RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); + return nullptr; + } } + auto cleanup_request_topic = rcpputils::make_scope_exit( + [domainParticipant, request_topic, request_topic_created]() { + if (request_topic_created) { + domainParticipant->delete_topic(request_topic); + } + }); + info->request_topic_ = request_topic_name; info->response_topic_ = response_topic_name; @@ -319,9 +346,9 @@ rmw_create_client( ///// // Create response DataReader - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", @@ -339,17 +366,18 @@ rmw_create_client( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } - // Creates DataReader (with subscriber name to not change name policy) + // Creates DataReader info->response_reader_ = subscriber->create_datareader( - response_topic, + response_topic_desc, dataReaderQos, info->listener_); if (!info->response_reader_) { - RMW_SET_ERROR_MSG("failed to create client response data reader"); + RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); return nullptr; } @@ -359,9 +387,9 @@ rmw_create_client( subscriber->delete_datareader(info->response_reader_); }); - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", @@ -386,17 +414,18 @@ rmw_create_client( } if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->request_writer_ = publisher->create_datawriter( - pub_topic, + request_topic, dataWriterQos, info->pub_listener_); if (!info->request_writer_) { - RMW_SET_ERROR_MSG("failed to create client request data writer"); + RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); return nullptr; } @@ -426,7 +455,7 @@ rmw_create_client( rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { - RMW_SET_ERROR_MSG("failed to allocate memory for client"); + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for rmw_client"); return nullptr; } auto cleanup_rmw_client = rcpputils::make_scope_exit( @@ -440,7 +469,7 @@ rmw_create_client( rmw_client->service_name = reinterpret_cast( rmw_allocate(strlen(service_name) + 1)); if (!rmw_client->service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for client name"); + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for service name"); return nullptr; } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); @@ -487,6 +516,8 @@ rmw_create_client( cleanup_rmw_client.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); + cleanup_request_topic.cancel(); + cleanup_response_topic.cancel(); cleanup_pub_listener.cancel(); cleanup_listener.cancel(); cleanup_type_support_response.cancel(); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 961e525ba..506fe0a7a 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -96,7 +96,7 @@ rmw_create_service( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); return nullptr; } @@ -163,12 +163,12 @@ rmw_create_service( // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; - eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, request_topic_name, request_type_name, - request_topic, + request_topic_desc, request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -179,12 +179,12 @@ rmw_create_service( // Get response topic and type eprosima::fastdds::dds::TypeSupport response_fastdds_type; - eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, response_topic_name, response_type_name, - response_topic, + response_topic_desc, response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -194,10 +194,10 @@ rmw_create_service( } ///// - // Create the RMW Service struct (info) + // Create the custom Service struct (info) CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate service info"); + RMW_SET_ERROR_MSG("create_service() failed to allocate custom info"); return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( @@ -215,7 +215,7 @@ rmw_create_service( if (!request_fastdds_type) { auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + RMW_SET_ERROR_MSG("create_service() failed to allocate request typesupport"); return nullptr; } @@ -224,7 +224,7 @@ rmw_create_service( if (!response_fastdds_type) { auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + RMW_SET_ERROR_MSG("create_service() failed to allocate response typesupport"); return nullptr; } @@ -242,7 +242,7 @@ rmw_create_service( }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { - RMW_SET_ERROR_MSG("create_service() failed to register request type"); + RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; @@ -255,22 +255,22 @@ rmw_create_service( // Create Listeners info->listener_ = new (std::nothrow) ServiceListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create service response subscriber listener"); + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); return nullptr; } - auto cleanup_type_support_listener = rcpputils::make_scope_exit( + auto cleanup_listener = rcpputils::make_scope_exit( [info]() { delete info->listener_; }); info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create service request publisher listener"); + RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); return nullptr; } - auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + auto cleanup_pub_listener = rcpputils::make_scope_exit( [info]() { delete info->pub_listener_; }); @@ -279,43 +279,64 @@ rmw_create_service( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } // Create request topic - if (!request_topic) { + eprosima::fastdds::dds::Topic * request_topic = nullptr; + if (!request_topic_desc) { request_topic = domainParticipant->create_topic( request_topic_name, request_type_name, topicQos); if (!request_topic) { - RMW_SET_ERROR_MSG("failed to create service request topic"); + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; } + + request_topic_desc = request_topic; } + auto cleanup_request_topic = rcpputils::make_scope_exit( + [domainParticipant, request_topic]() { + if (request_topic) { + domainParticipant->delete_topic(request_topic); + } + }); + // Create response topic - if (!response_topic) { + eprosima::fastdds::dds::Topic * response_topic = nullptr; + bool response_topic_created = false; + if (!response_topic_desc) { response_topic = domainParticipant->create_topic( response_topic_name, response_type_name, topicQos); if (!response_topic) { - RMW_SET_ERROR_MSG("failed to create service response topic"); + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; + } + + response_topic_desc = response_topic; + response_topic_created = true; + } else { + response_topic = dynamic_cast(response_topic_desc); + if (!response_topic) { + RMW_SET_ERROR_MSG("create_service() called with response topic not of class Topic"); return nullptr; } } - eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(response_topic); - if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, service response topic can only be of class Topic"); - return nullptr; - } + auto cleanup_response_topic = rcpputils::make_scope_exit( + [domainParticipant, response_topic, response_topic_created]() { + if (response_topic_created) { + domainParticipant->delete_topic(response_topic); + } + }); // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; @@ -323,9 +344,9 @@ rmw_create_service( ///// // Create request DataReader - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", @@ -343,17 +364,18 @@ rmw_create_service( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } - // Creates DataReader (with subscriber name to not change name policy) + // Creates DataReader info->request_reader_ = subscriber->create_datareader( - request_topic, + request_topic_desc, dataReaderQos, info->listener_); if (!info->request_reader_) { - RMW_SET_ERROR_MSG("failed to create service request data reader"); + RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); return nullptr; } @@ -367,9 +389,9 @@ rmw_create_service( ///// // Create response DataWriter - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", @@ -394,17 +416,18 @@ rmw_create_service( } if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->response_writer_ = publisher->create_datawriter( - pub_topic, + response_topic, dataWriterQos, info->pub_listener_); if (!info->response_writer_) { - RMW_SET_ERROR_MSG("failed to create service request data writer"); + RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); return nullptr; } @@ -431,7 +454,7 @@ rmw_create_service( rmw_service_t * rmw_service = rmw_service_allocate(); if (!rmw_service) { - RMW_SET_ERROR_MSG("failed to allocate memory for service"); + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for rmw_service"); return nullptr; } auto cleanup_rmw_service = rcpputils::make_scope_exit( @@ -445,7 +468,7 @@ rmw_create_service( rmw_service->service_name = reinterpret_cast( rmw_allocate(strlen(service_name) + 1)); if (!rmw_service->service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for service name"); + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for service name"); return nullptr; } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); @@ -491,8 +514,10 @@ rmw_create_service( cleanup_rmw_service.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_type_support_pub_listener.cancel(); - cleanup_type_support_listener.cancel(); + cleanup_response_topic.cancel(); + cleanup_request_topic.cancel(); + cleanup_pub_listener.cancel(); + cleanup_listener.cancel(); cleanup_type_support_response.cancel(); cleanup_type_support_request.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 97bf568bc..d03a51d64 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -14,14 +14,17 @@ #include +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/subscriber/Subscriber.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" #include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" @@ -90,7 +93,7 @@ rmw_create_client( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); return nullptr; } @@ -157,12 +160,12 @@ rmw_create_client( // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; - eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, request_topic_name, request_type_name, - request_topic, + request_topic_desc, request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -173,12 +176,12 @@ rmw_create_client( // Get response topic and type eprosima::fastdds::dds::TypeSupport response_fastdds_type; - eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, response_topic_name, response_type_name, - response_topic, + response_topic_desc, response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -188,10 +191,10 @@ rmw_create_client( } ///// - // Create the RMW Client struct (info) + // Create the custom Client struct (info) CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate client info"); + RMW_SET_ERROR_MSG("create_client() failed to allocate custom info"); return nullptr; } @@ -209,7 +212,7 @@ rmw_create_client( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { - RMW_SET_ERROR_MSG("failed to allocate request type support"); + RMW_SET_ERROR_MSG("create_client() failed to get request_type_support"); return nullptr; } auto return_request_type_support = rcpputils::make_scope_exit( @@ -219,7 +222,7 @@ rmw_create_client( auto response_type_impl = type_registry.get_response_type_support(type_support); if (!response_type_impl) { - RMW_SET_ERROR_MSG("failed to allocate response type support"); + RMW_SET_ERROR_MSG("create_client() failed to allocate response type support"); return nullptr; } auto return_response_type_support = rcpputils::make_scope_exit( @@ -234,7 +237,7 @@ rmw_create_client( auto tsupport = new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + RMW_SET_ERROR_MSG("create_client() failed to allocate request TypeSupportProxy"); return nullptr; } @@ -245,7 +248,7 @@ rmw_create_client( auto tsupport = new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + RMW_SET_ERROR_MSG("create_client() failed to allocate response TypeSupportProxy"); return nullptr; } @@ -276,22 +279,22 @@ rmw_create_client( // Create Listeners info->listener_ = new (std::nothrow) ClientListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create client response subscriber listener"); + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); return nullptr; } - auto cleanup_type_support_listener = rcpputils::make_scope_exit( + auto cleanup_listener = rcpputils::make_scope_exit( [info]() { delete info->listener_; }); info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create client request publisher listener"); + RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); return nullptr; } - auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + auto cleanup_pub_listener = rcpputils::make_scope_exit( [info]() { delete info->pub_listener_; }); @@ -300,44 +303,66 @@ rmw_create_client( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } // Create response topic - if (!response_topic) { + eprosima::fastdds::dds::Topic * response_topic = nullptr; + if (!response_topic_desc) { response_topic = domainParticipant->create_topic( response_topic_name, response_type_name, topicQos); if (!response_topic) { - RMW_SET_ERROR_MSG("failed to create client response topic"); + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); return nullptr; } + + response_topic_desc = response_topic; } + auto cleanup_response_topic = rcpputils::make_scope_exit( + [domainParticipant, response_topic]() { + if (response_topic) { + domainParticipant->delete_topic(response_topic); + } + }); + // Create request topic - if (!request_topic) { + eprosima::fastdds::dds::Topic * request_topic = nullptr; + bool request_topic_created = false; + if (!request_topic_desc) { request_topic = domainParticipant->create_topic( request_topic_name, request_type_name, topicQos); if (!request_topic) { - RMW_SET_ERROR_MSG("failed to create client request topic"); + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; } + + request_topic_desc = request_topic; + request_topic_created = true; } - - eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(request_topic); - if (!pub_topic) { - RMW_SET_ERROR_MSG("failed, client request topic can only be of class Topic"); - return nullptr; + else { + request_topic = dynamic_cast(request_topic_desc); + if (!request_topic) { + RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); + return nullptr; + } } + auto cleanup_request_topic = rcpputils::make_scope_exit( + [domainParticipant, request_topic, request_topic_created]() { + if (request_topic_created) { + domainParticipant->delete_topic(request_topic); + } + }); + info->request_topic_ = request_topic_name; info->response_topic_ = response_topic_name; @@ -347,9 +372,9 @@ rmw_create_client( ///// // Create request DataReader - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", @@ -367,17 +392,18 @@ rmw_create_client( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } - // Creates DataReader (with subscriber name to not change name policy) + // Creates DataReader info->response_reader_ = subscriber->create_datareader( - response_topic, + response_topic_desc, dataReaderQos, info->listener_); if (!info->response_reader_) { - RMW_SET_ERROR_MSG("failed to create client response data reader"); + RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); return nullptr; } @@ -387,9 +413,9 @@ rmw_create_client( subscriber->delete_datareader(info->response_reader_); }); - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "client" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", @@ -414,17 +440,18 @@ rmw_create_client( } if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->request_writer_ = publisher->create_datawriter( - pub_topic, + request_topic, dataWriterQos, info->pub_listener_); if (!info->request_writer_) { - RMW_SET_ERROR_MSG("failed to create client request data writer"); + RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); return nullptr; } @@ -453,7 +480,7 @@ rmw_create_client( rmw_client_t * rmw_client = rmw_client_allocate(); if (!rmw_client) { - RMW_SET_ERROR_MSG("failed to allocate memory for client"); + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for rmw_client"); return nullptr; } auto cleanup_rmw_client = rcpputils::make_scope_exit( @@ -467,7 +494,7 @@ rmw_create_client( rmw_client->service_name = reinterpret_cast( rmw_allocate(strlen(service_name) + 1)); if (!rmw_client->service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for client name"); + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for service name"); return nullptr; } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); @@ -475,19 +502,19 @@ rmw_create_client( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->request_writer_->guid()); common_context->graph_cache.associate_writer( - gid, + request_publisher_gid, common_context->gid, node->name, node->namespace_); - rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( + rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_reader_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( - gid_response, common_context->gid, node->name, node->namespace_); + response_subscriber_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, common_context->pub, @@ -495,12 +522,12 @@ rmw_create_client( nullptr); if (RMW_RET_OK != rmw_ret) { common_context->graph_cache.dissociate_reader( - gid_response, + response_subscriber_gid, common_context->gid, node->name, node->namespace_); common_context->graph_cache.dissociate_writer( - gid, + request_publisher_gid, common_context->gid, node->name, node->namespace_); @@ -511,8 +538,10 @@ rmw_create_client( cleanup_rmw_client.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_type_support_pub_listener.cancel(); - cleanup_type_support_listener.cancel(); + cleanup_request_topic.cancel(); + cleanup_response_topic.cancel(); + cleanup_pub_listener.cancel(); + cleanup_listener.cancel(); cleanup_type_support_response.cancel(); cleanup_type_support_request.cancel(); return_response_type_support.cancel(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index fb33de137..0f3b48b31 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -102,7 +102,7 @@ rmw_create_service( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); return nullptr; } @@ -169,12 +169,12 @@ rmw_create_service( // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; - eprosima::fastdds::dds::TopicDescription * request_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, request_topic_name, request_type_name, - request_topic, + request_topic_desc, request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -185,12 +185,12 @@ rmw_create_service( // Get response topic and type eprosima::fastdds::dds::TypeSupport response_fastdds_type; - eprosima::fastdds::dds::TopicDescription * response_topic = nullptr; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( participant_info, response_topic_name, response_type_name, - response_topic, + response_topic_desc, response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -203,7 +203,7 @@ rmw_create_service( // Create the RMW Service struct (info) CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); if (!info) { - RMW_SET_ERROR_MSG("failed to allocate service info"); + RMW_SET_ERROR_MSG("create_service() failed to allocate custom info"); return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( @@ -218,8 +218,7 @@ rmw_create_service( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto request_type_impl = type_registry.get_request_type_support(type_support); if (!request_type_impl) { - delete info; - RMW_SET_ERROR_MSG("failed to allocate request type support"); + RMW_SET_ERROR_MSG("create_service() failed to get request_type_support"); return nullptr; } auto return_request_type_support = rcpputils::make_scope_exit( @@ -229,7 +228,7 @@ rmw_create_service( auto response_type_impl = type_registry.get_response_type_support(type_support); if (!response_type_impl) { - RMW_SET_ERROR_MSG("failed to allocate response type support"); + RMW_SET_ERROR_MSG("create_service() failed to get response_type_support"); return nullptr; } auto return_response_type_support = rcpputils::make_scope_exit( @@ -244,7 +243,7 @@ rmw_create_service( auto tsupport = new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate request typesupport"); + RMW_SET_ERROR_MSG("create_service() failed to allocate request TypeSupportProxy"); return nullptr; } @@ -255,7 +254,7 @@ rmw_create_service( auto tsupport = new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); if (!tsupport) { - RMW_SET_ERROR_MSG("failed to allocate response typesupport"); + RMW_SET_ERROR_MSG("create_service() failed to allocate response TypeSupportProxy"); return nullptr; } @@ -273,7 +272,7 @@ rmw_create_service( }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { - RMW_SET_ERROR_MSG("create_service() failed to register request type"); + RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; @@ -286,22 +285,22 @@ rmw_create_service( // Create Listeners info->listener_ = new (std::nothrow) ServiceListener(info); if (!info->listener_) { - RMW_SET_ERROR_MSG("failed to create service response subscriber listener"); + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); return nullptr; } - auto cleanup_type_support_listener = rcpputils::make_scope_exit( + auto cleanup_listener = rcpputils::make_scope_exit( [info]() { delete info->listener_; }); info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { - RMW_SET_ERROR_MSG("failed to create service request publisher listener"); + RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); return nullptr; } - auto cleanup_type_support_pub_listener = rcpputils::make_scope_exit( + auto cleanup_pub_listener = rcpputils::make_scope_exit( [info]() { delete info->pub_listener_; }); @@ -310,43 +309,64 @@ rmw_create_service( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } // Create request topic - if (!request_topic) { + eprosima::fastdds::dds::Topic * request_topic = nullptr; + if (!request_topic_desc) { request_topic = domainParticipant->create_topic( request_topic_name, request_type_name, topicQos); if (!request_topic) { - RMW_SET_ERROR_MSG("failed to create service request topic"); + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; } + + request_topic_desc = request_topic; } + auto cleanup_request_topic = rcpputils::make_scope_exit( + [domainParticipant, request_topic]() { + if (request_topic) { + domainParticipant->delete_topic(request_topic); + } + }); + // Create response topic - if (!response_topic) { + eprosima::fastdds::dds::Topic * response_topic = nullptr; + bool response_topic_created = false; + if (!response_topic_desc) { response_topic = domainParticipant->create_topic( response_topic_name, response_type_name, topicQos); if (!response_topic) { - RMW_SET_ERROR_MSG("failed to create service response topic"); + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; + } + + response_topic_desc = response_topic; + response_topic_created = true; + } else { + response_topic = dynamic_cast(response_topic_desc); + if (!response_topic) { + RMW_SET_ERROR_MSG("create_service() called with response topic not of class Topic"); return nullptr; } } - eprosima::fastdds::dds::Topic * pub_topic = - dynamic_cast(response_topic); - if (pub_topic == nullptr) { - RMW_SET_ERROR_MSG("failed, service response topic can only be of class Topic"); - return nullptr; - } + auto cleanup_response_topic = rcpputils::make_scope_exit( + [domainParticipant, response_topic, response_topic_created]() { + if (response_topic_created) { + domainParticipant->delete_topic(response_topic); + } + }); // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; @@ -354,9 +374,9 @@ rmw_create_service( ///// // Create request DataReader - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill subscriber attributes with a subscriber profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", @@ -374,17 +394,18 @@ rmw_create_service( } if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } - // Creates DataReader (with subscriber name to not change name policy) + // Creates DataReader info->request_reader_ = subscriber->create_datareader( - request_topic, + request_topic_desc, dataReaderQos, info->listener_); if (!info->request_reader_) { - RMW_SET_ERROR_MSG("failed to create service request data reader"); + RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); return nullptr; } @@ -398,9 +419,9 @@ rmw_create_service( ///// // Create response DataWriter - // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill publisher attributes with a publisher profile - // located based of topic name defined by _create_topic_name(). If no profile is found, a search - // with profile_name "service" is attempted. Else, use the default attributes. + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", @@ -425,17 +446,18 @@ rmw_create_service( } if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->response_writer_ = publisher->create_datawriter( - pub_topic, + response_topic, dataWriterQos, info->pub_listener_); if (!info->response_writer_) { - RMW_SET_ERROR_MSG("failed to create service request data writer"); + RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); return nullptr; } @@ -461,7 +483,7 @@ rmw_create_service( rmw_service_t * rmw_service = rmw_service_allocate(); if (!rmw_service) { - RMW_SET_ERROR_MSG("failed to allocate memory for service"); + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for rmw_service"); return nullptr; } auto cleanup_rmw_service = rcpputils::make_scope_exit( @@ -475,7 +497,7 @@ rmw_create_service( rmw_service->service_name = reinterpret_cast( rmw_allocate(strlen(service_name) + 1)); if (!rmw_service->service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for service name"); + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for service name"); return nullptr; } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); @@ -483,19 +505,19 @@ rmw_create_service( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + rmw_gid_t request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->request_reader_->guid()); common_context->graph_cache.associate_reader( - gid, + request_subscriber_gid, common_context->gid, node->name, node->namespace_); - rmw_gid_t gid_response = rmw_fastrtps_shared_cpp::create_rmw_gid( + rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_writer_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( - gid, common_context->gid, node->name, node->namespace_); + response_publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, common_context->pub, @@ -503,12 +525,12 @@ rmw_create_service( nullptr); if (RMW_RET_OK != rmw_ret) { common_context->graph_cache.dissociate_writer( - gid_response, + response_publisher_gid, common_context->gid, node->name, node->namespace_); common_context->graph_cache.dissociate_reader( - gid, + request_subscriber_gid, common_context->gid, node->name, node->namespace_); @@ -519,8 +541,10 @@ rmw_create_service( cleanup_rmw_service.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_type_support_pub_listener.cancel(); - cleanup_type_support_listener.cancel(); + cleanup_response_topic.cancel(); + cleanup_request_topic.cancel(); + cleanup_pub_listener.cancel(); + cleanup_listener.cancel(); cleanup_type_support_response.cancel(); cleanup_type_support_request.cancel(); return_response_type_support.cancel(); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp index 74201087e..4ef97841a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp @@ -72,11 +72,9 @@ __rmw_service_server_is_available( return RMW_RET_ERROR; } - auto pub_topic_name = - client_info->request_topic_; + auto pub_topic_name = client_info->request_topic_; - auto sub_topic_name = - client_info->response_topic_; + auto sub_topic_name = client_info->response_topic_; *is_available = false; auto common_context = static_cast(node->context->impl->common); From 41afcf3f95ab08d63e02d9bcf8bc02de131bf04e Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 15 Mar 2021 10:19:06 +0100 Subject: [PATCH 27/60] Linters Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 11 +++++------ rmw_fastrtps_cpp/src/rmw_client.cpp | 5 ++--- rmw_fastrtps_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 2 +- rmw_fastrtps_shared_cpp/src/qos.cpp | 4 ++-- rmw_fastrtps_shared_cpp/test/test_logging.cpp | 2 +- 7 files changed, 14 insertions(+), 16 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 7920dd1aa..935fd4129 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -148,7 +148,7 @@ rmw_fastrtps_cpp::create_publisher( topic_name_mangled.c_str(), type_name.c_str()); return nullptr; } - + ///// // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; @@ -179,7 +179,7 @@ rmw_fastrtps_cpp::create_publisher( // Transfer ownership to fastdds_type fastdds_type.reset(tsupport); } - + if (keyed && !fastdds_type->m_isGetKeyDefined) { RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); return nullptr; @@ -234,15 +234,14 @@ rmw_fastrtps_cpp::create_publisher( RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } - + topic_created = true; - } - else { + } else { topic = dynamic_cast(des_topic); if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); return nullptr; - } + } } auto cleanup_topic = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 9d8a8516f..825bdbf6c 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -318,11 +318,10 @@ rmw_create_client( RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; } - + request_topic_desc = request_topic; request_topic_created = true; - } - else { + } else { request_topic = dynamic_cast(request_topic_desc); if (!request_topic) { RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 506fe0a7a..10a068fac 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -296,7 +296,7 @@ rmw_create_service( RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; } - + request_topic_desc = request_topic; } @@ -320,7 +320,7 @@ rmw_create_service( RMW_SET_ERROR_MSG("create_service() failed to create response topic"); return nullptr; } - + response_topic_desc = response_topic; response_topic_created = true; } else { diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 474a1eab8..097a3ce0b 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -237,7 +237,7 @@ create_subscription( RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } - + des_topic = topic; } diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index dbb8031c4..be8f4879a 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -121,7 +121,7 @@ __create_participant( // As the participant listener is only used for discovery related callbacks, which are // Fast DDS extensions to the DDS standard DomainParticipantListener interface, an empty - // mask should be used to let child entities handle standard DDS events. + // mask should be used to let child entities handle standard DDS events. eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); participant_info->participant_ = diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 1bc603f8c..129c3be8b 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -55,7 +55,7 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) return result; } -// Private function to encapsulate DataReader and DataWriter, together with Topic, filling +// Private function to encapsulate DataReader and DataWriter, together with Topic, filling // entities DDS QoS from the RMW QoS profile. template bool fill_entity_qos_from_profile( @@ -175,7 +175,7 @@ get_topic_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastdds::dds::TopicQos & topic_qos) { - return fill_entity_qos_from_profile(qos_policies, topic_qos);; + return fill_entity_qos_from_profile(qos_policies, topic_qos); } bool diff --git a/rmw_fastrtps_shared_cpp/test/test_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_logging.cpp index e2b158035..93969fa37 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -24,7 +24,7 @@ TEST(TestLogging, rmw_logging) { using eprosima::fastdds::dds::Log; - + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); From 36c04d1682e13f1f0345ee50380b07d2001df68a Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 16 Mar 2021 07:40:00 +0100 Subject: [PATCH 28/60] Addressed internal review comments Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_cpp/src/subscription.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 8 ++++---- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 6 +++--- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 6 ++++-- .../rmw_fastrtps_shared_cpp/custom_client_info.hpp | 2 -- .../rmw_fastrtps_shared_cpp/custom_publisher_info.hpp | 1 - .../rmw_fastrtps_shared_cpp/custom_service_info.hpp | 1 - .../rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp | 1 - rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 2 -- 11 files changed, 17 insertions(+), 22 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 825bdbf6c..6dbd53982 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -351,7 +351,7 @@ rmw_create_client( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", - // if it does not exist it tryes with the response topic name + // if it does not exist it tries with the response topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, @@ -392,7 +392,7 @@ rmw_create_client( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 10a068fac..1c3f0e1e4 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -350,7 +350,7 @@ rmw_create_service( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, @@ -395,7 +395,7 @@ rmw_create_service( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 097a3ce0b..030f8055f 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -154,7 +154,7 @@ create_subscription( } ///// - // Create the RMW Subscriber struct (info) + // Create the custom Subscriber struct (info) CustomSubscriberInfo * info = nullptr; info = new (std::nothrow) CustomSubscriberInfo(); @@ -169,7 +169,7 @@ create_subscription( }); info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = callbacks; ///// // Create the Type Support struct diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index d03a51d64..c5716f6c1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -266,7 +266,7 @@ rmw_create_client( }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { - RMW_SET_ERROR_MSG("create_client() failed to register request type"); + RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; @@ -370,7 +370,7 @@ rmw_create_client( std::string topic_name_fallback = "client"; ///// - // Create request DataReader + // Create response DataReader // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search @@ -378,7 +378,7 @@ rmw_create_client( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the response topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, @@ -419,7 +419,7 @@ rmw_create_client( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 0f3b48b31..a8e095335 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -200,7 +200,7 @@ rmw_create_service( } ///// - // Create the RMW Service struct (info) + // Create the custom Service struct (info) CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); if (!info) { RMW_SET_ERROR_MSG("create_service() failed to allocate custom info"); @@ -380,7 +380,7 @@ rmw_create_service( eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, @@ -425,7 +425,7 @@ rmw_create_service( eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", - // if it does not exist it tryes with the request topic name + // if it does not exist it tries with the request topic name // It does not need to check the return code, as if any of the profile does not exist, // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 863d60c7c..caa643335 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -65,6 +65,8 @@ create_subscription( bool keyed, bool create_subscription_listener) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + ///// // Check input parameters RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); @@ -150,13 +152,13 @@ create_subscription( fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_publisher() called with existing topic name %s with incompatible type %s", + "create_subscription() called with existing topic name %s with incompatible type %s", topic_name_mangled.c_str(), type_name.c_str()); return nullptr; } ///// - // Create the RMW Subscriber struct (info) + // Create the custom Subscriber struct (info) CustomSubscriberInfo * info = nullptr; info = new (std::nothrow) CustomSubscriberInfo(); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index dad3c01e3..4edba50de 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -41,8 +41,6 @@ #include "rcpputils/thread_safety_annotations.hpp" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - class ClientListener; class ClientPubListener; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index b3492fb96..71ae6981c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -34,7 +34,6 @@ #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index ea17ef4d9..0f90645ee 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -40,7 +40,6 @@ #include "rcpputils/thread_safety_annotations.hpp" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" class ServiceListener; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 379a42703..6ee7d47aa 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -35,7 +35,6 @@ #include "rmw/impl/cpp/macros.hpp" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index ec75d5f3e..6cde76a8c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/serialized_message.h" From f634b96de02ea80bca192eb37259799724899f51 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 16 Mar 2021 13:26:54 +0100 Subject: [PATCH 29/60] Addressed internal review comments Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 8 ++++---- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 6 +++--- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 14 ++++++-------- rmw_fastrtps_dynamic_cpp/test/test_logging.cpp | 2 +- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 3 +++ .../rmw_fastrtps_shared_cpp/custom_client_info.hpp | 2 ++ .../custom_publisher_info.hpp | 6 +++--- .../custom_service_info.hpp | 2 +- .../custom_subscriber_info.hpp | 4 ++-- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 5 ++++- rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp | 2 ++ rmw_fastrtps_shared_cpp/src/participant.cpp | 4 ++-- 14 files changed, 36 insertions(+), 28 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 935fd4129..c80f5a637 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -221,7 +221,7 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS for publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index df8a2c471..3f7f7c8c7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -15,6 +15,7 @@ #include +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" @@ -234,7 +235,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } @@ -247,13 +248,12 @@ rmw_fastrtps_dynamic_cpp::create_publisher( RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } - } - else { + } else { topic = dynamic_cast(des_topic); if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); return nullptr; - } + } } auto cleanup_topic = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index c5716f6c1..c2997223a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -24,6 +24,7 @@ #include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" + #include "fastdds/rtps/resources/ResourceManagement.h" #include "rcpputils/scope_exit.hpp" @@ -344,11 +345,10 @@ rmw_create_client( RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; } - + request_topic_desc = request_topic; request_topic_created = true; - } - else { + } else { request_topic = dynamic_cast(request_topic_desc); if (!request_topic) { RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index a8e095335..a55ec2c00 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -326,7 +326,7 @@ rmw_create_service( RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; } - + request_topic_desc = request_topic; } @@ -350,7 +350,7 @@ rmw_create_service( RMW_SET_ERROR_MSG("create_service() failed to create response topic"); return nullptr; } - + response_topic_desc = response_topic; response_topic_created = true; } else { diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index caa643335..386043045 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -91,8 +91,6 @@ create_subscription( } } RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - (void)keyed; - (void)create_subscription_listener; ///// // Check RMW QoS @@ -177,7 +175,7 @@ create_subscription( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_support_impl = type_registry.get_message_type_support(type_support); if (!type_support_impl) { - RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); return nullptr; } auto return_type_support = rcpputils::make_scope_exit( @@ -220,11 +218,11 @@ create_subscription( info->listener_ = nullptr; if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); - } - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } } auto cleanup_listener = rcpputils::make_scope_exit( @@ -253,7 +251,7 @@ create_subscription( RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; } - + des_topic = topic; } diff --git a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp index 1892537e5..f0ed4037e 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp @@ -22,7 +22,7 @@ TEST(TestLogging, rmw_logging) { using Log = eprosima::fastdds::dds::Log; - + rmw_ret_t ret = rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index cc3f2548d..b31469318 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -20,6 +20,9 @@ #include "fastdds/dds/topic/TopicDataType.hpp" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SerializedPayload.h" + #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 4edba50de..dad3c01e3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -41,6 +41,8 @@ #include "rcpputils/thread_safety_annotations.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + class ClientListener; class ClientPubListener; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 71ae6981c..ebc37ca4a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -131,15 +131,15 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds mutable std::mutex internalMutex_; std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool deadline_changes_; eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 0f90645ee..f66cbdf60 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -30,7 +30,6 @@ #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" -#include "fastdds/dds/subscriber/InstanceState.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" @@ -41,6 +40,7 @@ #include "rcpputils/thread_safety_annotations.hpp" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" class ServiceListener; class ServicePubListener; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 6ee7d47aa..105a71ad1 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -163,11 +163,11 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds std::atomic_bool deadline_changes_; eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index cb41265d0..f1288fddd 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -16,9 +16,12 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ +#include #include #include -#include +#include + +#include #include "rmw/rmw.h" diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index ce1b14929..1baf926fb 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -16,6 +16,8 @@ #include #include +#include "fastdds/rtps/common/SerializedPayload.h" + #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index be8f4879a..5a0795c95 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -316,8 +316,8 @@ rmw_fastrtps_shared_cpp::create_participant( } #else RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); + "This Fast DDS version doesn't have the security libraries\n" + "Please compile Fast DDS using the -DSECURITY=ON CMake option"); return nullptr; #endif } From 9a0382c43226051b7bbfb7a282144f8e6ed5ff68 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 09:07:40 +0100 Subject: [PATCH 30/60] Improvements on destroy_participant. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 27 +++++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 5a0795c95..960a1eeda 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -338,17 +338,28 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant return RMW_RET_ERROR; } + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info->participant_, RMW_RET_ERROR); + + // Make the participant stop listening to discovery + participant_info->participant_->set_listener(nullptr); + + ReturnCode_t ret = ReturnCode_t::RETCODE_OK; + // Remove publisher and subcriber from participant - ReturnCode_t ret = participant_info->participant_->delete_publisher(participant_info->publisher_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete dds publisher from participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (participant_info->publisher_) { + ret = participant_info->participant_->delete_publisher(participant_info->publisher_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete dds publisher from participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } } - ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete dds subscriber from participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (participant_info->subscriber_) { + ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG("Fail in delete dds subscriber from participant"); + return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + } } // Delete Domain Participant From 5c7503162b527f799c7457c5cad20b79113dab46 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 09:39:15 +0100 Subject: [PATCH 31/60] Correctly cleanup when deleting participant. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 28 ++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 960a1eeda..7822b8c36 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -19,7 +19,11 @@ #include "fastdds/dds/core/status/StatusMask.hpp" #include "fastdds/dds/domain/DomainParticipantFactory.hpp" #include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/PublisherQos.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" #include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" #include "fastdds/rtps/attributes/PropertyPolicy.h" #include "fastdds/rtps/common/Property.h" @@ -345,8 +349,17 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant ReturnCode_t ret = ReturnCode_t::RETCODE_OK; - // Remove publisher and subcriber from participant + // Collect topics that should be deleted + std::vector topics_to_remove; + + // Remove datawriters and publisher from participant if (participant_info->publisher_) { + std::vector writers; + participant_info->publisher_->get_datawriters(writers); + for (auto writer : writers) { + topics_to_remove.push_back(writer->get_topic()); + participant_info->publisher_->delete_datawriter(writer); + } ret = participant_info->participant_->delete_publisher(participant_info->publisher_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete dds publisher from participant"); @@ -354,7 +367,14 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant } } + // Remove datareaders and subscriber from participant if (participant_info->subscriber_) { + std::vector readers; + participant_info->subscriber_->get_datareaders(readers); + for (auto reader : readers) { + topics_to_remove.push_back(reader->get_topicdescription()); + participant_info->subscriber_->delete_datareader(reader); + } ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); if (ret != ReturnCode_t::RETCODE_OK) { RMW_SET_ERROR_MSG("Fail in delete dds subscriber from participant"); @@ -362,6 +382,12 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant } } + // Remove topics + eprosima::fastdds::dds::TypeSupport dummy_type; + for (auto topic : topics_to_remove) { + remove_topic_and_type(participant_info, topic, dummy_type); + } + // Delete Domain Participant ret = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( From 9e7c92f8a41f396207890f4292fa5b0030e4b686 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 10:36:50 +0100 Subject: [PATCH 32/60] Linters Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 7822b8c36..262c88351 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "fastdds/dds/core/status/StatusMask.hpp" #include "fastdds/dds/domain/DomainParticipantFactory.hpp" @@ -372,8 +373,8 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant std::vector readers; participant_info->subscriber_->get_datareaders(readers); for (auto reader : readers) { - topics_to_remove.push_back(reader->get_topicdescription()); - participant_info->subscriber_->delete_datareader(reader); + topics_to_remove.push_back(reader->get_topicdescription()); + participant_info->subscriber_->delete_datareader(reader); } ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); if (ret != ReturnCode_t::RETCODE_OK) { From a3bfff826ffcac822a691063ee02a2d03b338f25 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 11:52:20 +0100 Subject: [PATCH 33/60] Fixed duration conversions on rtps_qos_to_rmw_qos Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index f1288fddd..dac20c454 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -158,11 +158,8 @@ rtps_qos_to_rmw_qos( break; } - qos->deadline.sec = rtps_qos.m_deadline.period.seconds; - qos->deadline.nsec = rtps_qos.m_deadline.period.nanosec; - - qos->lifespan.sec = rtps_qos.m_lifespan.duration.seconds; - qos->lifespan.nsec = rtps_qos.m_lifespan.duration.nanosec; + qos->deadline = dds_duration_to_rmw(rtps_qos.m_deadline.period); + qos->lifespan = dds_duration_to_rmw(rtps_qos.m_lifespan.duration); switch (rtps_qos.m_liveliness.kind) { case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: From 4d9c19c8e5df276cdd5c5ffe7585e77fa9750a91 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 11:53:49 +0100 Subject: [PATCH 34/60] Added test for dds_qos_to_rmw_qos Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/test/CMakeLists.txt | 6 + .../test/test_dds_qos_to_rmw_qos.cpp | 161 ++++++++++++++++++ 2 files changed, 167 insertions(+) create mode 100644 rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp diff --git a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt index ef660b2fe..66478cdc6 100644 --- a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt @@ -7,6 +7,12 @@ if(TARGET test_dds_attributes_to_rmw_qos) target_link_libraries(test_dds_attributes_to_rmw_qos ${PROJECT_NAME}) endif() +ament_add_gtest(test_dds_qos_to_rmw_qos test_dds_qos_to_rmw_qos.cpp) +if(TARGET test_dds_qos_to_rmw_qos) + ament_target_dependencies(test_dds_qos_to_rmw_qos) + target_link_libraries(test_dds_qos_to_rmw_qos ${PROJECT_NAME}) +endif() + ament_add_gtest(test_rmw_qos_to_dds_attributes test_rmw_qos_to_dds_attributes.cpp) if(TARGET test_rmw_qos_to_dds_attributes) target_link_libraries(test_rmw_qos_to_dds_attributes ${PROJECT_NAME}) diff --git a/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp new file mode 100644 index 000000000..0df38d319 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp @@ -0,0 +1,161 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +#include + +#include "gtest/gtest.h" + +#include "fastdds/dds/core/policy/QosPolicies.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" + +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +using eprosima::fastdds::dds::DataReaderQos; +using eprosima::fastdds::dds::DataWriterQos; +static const eprosima::fastrtps::Duration_t InfiniteDuration = + eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + +class DDSQosToRMWQosTest : public ::testing::Test +{ +protected: + rmw_qos_profile_t qos_profile_ {}; + DataWriterQos writer_qos_ {}; + DataReaderQos reader_qos_ {}; +}; + + +TEST_F(DDSQosToRMWQosTest, test_publisher_depth_conversion) { + writer_qos_.history().depth = 0; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 0u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_history_conversion) { + writer_qos_.history().kind = eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_ALL); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_durability_conversion) { + writer_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_DURABILITY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_UNKNOWN); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_reliability_conversion) { + writer_qos_.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_liveliness_conversion) { + writer_qos_.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_liveliness_lease_duration_conversion) { + writer_qos_.liveliness().lease_duration = {8, 78901234}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 8u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 78901234u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_deadline_conversion) { + writer_qos_.deadline().period = {12, 1234}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 12u); + EXPECT_EQ(qos_profile_.deadline.nsec, 1234u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_lifespan_conversion) { + writer_qos_.lifespan().duration = {19, 5432}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 19u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 5432u); +} + + +TEST_F(DDSQosToRMWQosTest, test_subscriber_depth_conversion) { + reader_qos_.history().depth = 1; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 1u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_history_conversion) { + reader_qos_.history().kind = eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_durability_conversion) { + reader_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_reliability_conversion) { + reader_qos_.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_liveliness_conversion) { + reader_qos_.liveliness().kind = + eprosima::fastdds::dds::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_UNKNOWN); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_liveliness_lease_duration_conversion) { + reader_qos_.liveliness().lease_duration = {80, 34567}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 80u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 34567u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_deadline_conversion) { + reader_qos_.deadline().period = {1, 3324}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 1u); + EXPECT_EQ(qos_profile_.deadline.nsec, 3324u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_lifespan_conversion) { + reader_qos_.lifespan().duration = {9, 432}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 9u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 432u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_infinite_duration_conversions) { + reader_qos_.lifespan().duration = InfiniteDuration; + reader_qos_.deadline().period = InfiniteDuration; + reader_qos_.liveliness().lease_duration = InfiniteDuration; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_infinite_duration_conversions) { + writer_qos_.lifespan().duration = InfiniteDuration; + writer_qos_.deadline().period = InfiniteDuration; + writer_qos_.liveliness().lease_duration = InfiniteDuration; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} From 9cefd7edde53ba045a1dc54e6ec1d163920c88bc Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 12:25:34 +0100 Subject: [PATCH 35/60] Improvements on destroy_client Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 42 ++++++++++++++++------ 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 1a5046c57..87966dd5c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -40,8 +40,21 @@ __rmw_destroy_client( { rmw_ret_t final_ret = RMW_RET_OK; auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); auto info = static_cast(client->data); - { + bool info_is_valid = true; + if (nullptr == info || nullptr == info->request_writer_ || nullptr == info->response_reader_) { + RMW_SET_ERROR_MSG("destroy_client() called with invalid info struct"); + final_ret = RMW_RET_INVALID_ARGUMENT; + info_is_valid = false; + } else if (nullptr == participant_info || nullptr == common_context) { + RMW_SET_ERROR_MSG("destroy_client() called on invalid context"); + final_ret = RMW_RET_INVALID_ARGUMENT; + info_is_valid = false; + } + + if (info_is_valid) { // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -63,12 +76,17 @@ __rmw_destroy_client( nullptr); } + auto show_previous_error = [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + ///// // Delete DataWriter and DataReader - auto participant_info = - static_cast(node->context->impl->participant_info); - - if (nullptr != info) { + if (info_is_valid) { std::lock_guard lck(participant_info->entity_creation_mutex_); // Keep pointers to topics, so we can remove them later @@ -78,8 +96,10 @@ __rmw_destroy_client( // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + info->response_reader_->set_listener(nullptr); } // Delete DataReader listener @@ -90,8 +110,10 @@ __rmw_destroy_client( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datawriter"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + info->request_writer_->set_listener(nullptr); } // Delete DataWriter listener @@ -105,8 +127,6 @@ __rmw_destroy_client( // Delete CustomClientInfo structure delete info; - } else { - final_ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(client->service_name)); From 6e1c82f18c3e113e8ea5c84493b2f5fb3418365c Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 13:05:13 +0100 Subject: [PATCH 36/60] Improvements on destroy_service Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 38 ++++++++++++++++----- 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index e38d780e2..3f8110c08 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -53,8 +53,21 @@ __rmw_destroy_service( rmw_ret_t final_ret = RMW_RET_OK; auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); auto info = static_cast(service->data); - { + bool info_is_valid = true; + if (nullptr == info || nullptr == info->request_reader_ || nullptr == info->response_writer_) { + RMW_SET_ERROR_MSG("destroy_service() called with invalid info struct"); + final_ret = RMW_RET_INVALID_ARGUMENT; + info_is_valid = false; + } else if (nullptr == participant_info || nullptr == common_context) { + RMW_SET_ERROR_MSG("destroy_service() called on invalid context"); + final_ret = RMW_RET_INVALID_ARGUMENT; + info_is_valid = false; + } + + if (info_is_valid) { // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -76,12 +89,17 @@ __rmw_destroy_service( nullptr); } + auto show_previous_error = [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + ///// // Delete DataWriter and DataReader - auto participant_info = - static_cast(node->context->impl->participant_info); - - if (nullptr != info) { + if (info_is_valid) { std::lock_guard lck(participant_info->entity_creation_mutex_); // Keep pointers to topics, so we can remove them later @@ -91,8 +109,10 @@ __rmw_destroy_service( // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datareader"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + info->request_reader_->set_listener(nullptr); } // Delete DataReader listener @@ -103,8 +123,10 @@ __rmw_destroy_service( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->response_writer_); if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datawriter"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + info->response_writer_->set_listener(nullptr); } // Delete DataWriter listener @@ -118,8 +140,6 @@ __rmw_destroy_service( // Delete CustomServiceInfo structure delete info; - } else { - final_ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(service->service_name)); From f6dd571185cd541ced4125fbe22b89688b162d79 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 18 Mar 2021 16:32:52 +0100 Subject: [PATCH 37/60] Linters Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 15 ++++++++------- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 15 ++++++++------- .../test/test_dds_qos_to_rmw_qos.cpp | 2 +- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 87966dd5c..6cbf42f2d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -76,13 +76,14 @@ __rmw_destroy_client( nullptr); } - auto show_previous_error = [&final_ret]() { - if (RMW_RET_OK != final_ret) { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; + auto show_previous_error = + [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; ///// // Delete DataWriter and DataReader diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 3f8110c08..d8816b17c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -89,13 +89,14 @@ __rmw_destroy_service( nullptr); } - auto show_previous_error = [&final_ret]() { - if (RMW_RET_OK != final_ret) { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; + auto show_previous_error = + [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; ///// // Delete DataWriter and DataReader diff --git a/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp index 0df38d319..88aef4e5d 100644 --- a/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 732d8146a98fe7e4931bd50a187e05526c8b06ce Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 22 Mar 2021 10:07:16 +0100 Subject: [PATCH 38/60] Avoid API break on native getters. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_cpp/get_client.hpp | 28 ++++- .../rmw_fastrtps_cpp/get_participant.hpp | 17 ++- .../rmw_fastrtps_cpp/get_publisher.hpp | 15 ++- .../include/rmw_fastrtps_cpp/get_service.hpp | 28 ++++- .../rmw_fastrtps_cpp/get_subscriber.hpp | 15 ++- rmw_fastrtps_cpp/src/get_client.cpp | 18 ++- rmw_fastrtps_cpp/src/get_participant.cpp | 9 +- rmw_fastrtps_cpp/src/get_publisher.cpp | 9 +- rmw_fastrtps_cpp/src/get_service.cpp | 18 ++- rmw_fastrtps_cpp/src/get_subscriber.cpp | 9 +- .../test/test_get_native_entities.cpp | 118 ++++++++++++++++-- .../rmw_fastrtps_dynamic_cpp/get_client.hpp | 27 +++- .../get_participant.hpp | 14 ++- .../get_publisher.hpp | 14 ++- .../rmw_fastrtps_dynamic_cpp/get_service.hpp | 27 +++- .../get_subscriber.hpp | 14 ++- rmw_fastrtps_dynamic_cpp/src/get_client.cpp | 18 ++- .../src/get_participant.cpp | 9 +- .../src/get_publisher.cpp | 9 +- rmw_fastrtps_dynamic_cpp/src/get_service.cpp | 18 ++- .../src/get_subscriber.cpp | 9 +- .../test/test_get_native_entities.cpp | 118 ++++++++++++++++-- 22 files changed, 518 insertions(+), 43 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp index ab429826d..b4f4aa2e2 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp @@ -17,12 +17,36 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" + +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { +/// Return a native FastRTPS publisher handle for the request. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_request_publisher(rmw_client_t * client); + +/// Return a native FastRTPS subscriber handle for the response. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client); + /// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or @@ -32,7 +56,7 @@ namespace rmw_fastrtps_cpp */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_request_publisher(rmw_client_t * client); +get_request_datawriter(rmw_client_t * client); /// Return a native Fast DDS DataReader handle for the response. /** @@ -43,7 +67,7 @@ get_request_publisher(rmw_client_t * client); */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_response_subscriber(rmw_client_t * client); +get_response_datareader(rmw_client_t * client); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp index 2de0a844b..70a070064 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp @@ -16,22 +16,35 @@ #define RMW_FASTRTPS_CPP__GET_PARTICIPANT_HPP_ #include "fastdds/dds/domain/DomainParticipant.hpp" + +#include "fastrtps/participant/Participant.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { +/// Return a native Fast RTPS participant handle. +/** + * This function function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Participant * +get_participant(rmw_node_t * node); + /// Return a native Fast DDS DomainParticipant handle. /** - * The function returns `NULL` when either the node handle is `NULL` or when the + * This function returns `NULL` when either the node handle is `NULL` or when the * node handle is from a different rmw implementation. * * \return native Fast DDS DomainParticipant handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DomainParticipant * -get_participant(rmw_node_t * node); +get_domain_participant(rmw_node_t * node); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp index cee30beae..3d449b78b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp @@ -16,12 +16,25 @@ #define RMW_FASTRTPS_CPP__GET_PUBLISHER_HPP_ #include "fastdds/dds/publisher/DataWriter.hpp" + +#include "fastrtps/publisher/Publisher.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { +/// Return a native FastRTPS publisher handle. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_publisher(rmw_publisher_t * publisher); + /// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or @@ -31,7 +44,7 @@ namespace rmw_fastrtps_cpp */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_publisher(rmw_publisher_t * publisher); +get_datawriter(rmw_publisher_t * publisher); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp index 26360875a..804967a90 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp @@ -17,12 +17,36 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" + +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { +/// Return a native FastRTPS subscriber handle for the request. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_request_subscriber(rmw_service_t * service); + +/// Return a native FastRTPS publisher handle for the response. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service); + /// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or @@ -32,7 +56,7 @@ namespace rmw_fastrtps_cpp */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_request_subscriber(rmw_service_t * service); +get_request_datareader(rmw_service_t * service); /// Return a native Fast DDS DataWriter handle for the response. /** @@ -43,7 +67,7 @@ get_request_subscriber(rmw_service_t * service); */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_response_publisher(rmw_service_t * service); +get_response_datawriter(rmw_service_t * service); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp index 69af229a2..963f24a34 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -16,12 +16,25 @@ #define RMW_FASTRTPS_CPP__GET_SUBSCRIBER_HPP_ #include "fastdds/dds/subscriber/DataReader.hpp" + +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { +/// Return a native FastRTPS subscriber handle. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_subscriber(rmw_subscription_t * subscription); + /// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or @@ -31,7 +44,7 @@ namespace rmw_fastrtps_cpp */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_subscriber(rmw_subscription_t * subscription); +get_datareader(rmw_subscription_t * subscription); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/get_client.cpp b/rmw_fastrtps_cpp/src/get_client.cpp index 76e30662c..c127ffddb 100644 --- a/rmw_fastrtps_cpp/src/get_client.cpp +++ b/rmw_fastrtps_cpp/src/get_client.cpp @@ -23,8 +23,22 @@ namespace rmw_fastrtps_cpp { -eprosima::fastdds::dds::DataWriter * +eprosima::fastrtps::Publisher * get_request_publisher(rmw_client_t * client) +{ + (void)client; + return nullptr; +} + +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client) +{ + (void)client; + return nullptr; +} + +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client) { if (!client) { return nullptr; @@ -37,7 +51,7 @@ get_request_publisher(rmw_client_t * client) } eprosima::fastdds::dds::DataReader * -get_response_subscriber(rmw_client_t * client) +get_response_datareader(rmw_client_t * client) { if (!client) { return nullptr; diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index e6263e1cb..e93ff2516 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -22,8 +22,15 @@ namespace rmw_fastrtps_cpp { -eprosima::fastdds::dds::DomainParticipant * +eprosima::fastrtps::Participant * get_participant(rmw_node_t * node) +{ + (void)node; + return nullptr; +} + +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node) { if (!node) { return nullptr; diff --git a/rmw_fastrtps_cpp/src/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index 71ae970e4..c2a237d37 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -20,8 +20,15 @@ namespace rmw_fastrtps_cpp { -eprosima::fastdds::dds::DataWriter * +eprosima::fastrtps::Publisher * get_publisher(rmw_publisher_t * publisher) +{ + (void)publisher; + return nullptr; +} + +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher) { if (!publisher) { return nullptr; diff --git a/rmw_fastrtps_cpp/src/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index f92e80812..bc4e01dad 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -20,8 +20,22 @@ namespace rmw_fastrtps_cpp { -eprosima::fastdds::dds::DataReader * +eprosima::fastrtps::Subscriber * get_request_subscriber(rmw_service_t * service) +{ + (void)service; + return nullptr; +} + +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service) +{ + (void)service; + return nullptr; +} + +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service) { if (!service) { return nullptr; @@ -34,7 +48,7 @@ get_request_subscriber(rmw_service_t * service) } eprosima::fastdds::dds::DataWriter * -get_response_publisher(rmw_service_t * service) +get_response_datawriter(rmw_service_t * service) { if (!service) { return nullptr; diff --git a/rmw_fastrtps_cpp/src/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 468a732b0..9f8f877ac 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -20,8 +20,15 @@ namespace rmw_fastrtps_cpp { -eprosima::fastdds::dds::DataReader * +eprosima::fastrtps::Subscriber * get_subscriber(rmw_subscription_t * subscription) +{ + (void)subscription; + return nullptr; +} + +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription) { if (!subscription) { return nullptr; diff --git a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp index 532931e47..04070db8d 100644 --- a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp @@ -76,7 +76,18 @@ TEST_F(TestNativeEntities, get_participant) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(node)); node->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_participant(node)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(node)); +} + +TEST_F(TestNativeEntities, get_domain_participant) { + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_domain_participant(nullptr)); + + const char * implementation_identifier = node->implementation_identifier; + node->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_domain_participant(node)); + node->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_domain_participant(node)); } TEST_F(TestNativeEntities, get_publisher) { @@ -95,7 +106,29 @@ TEST_F(TestNativeEntities, get_publisher) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); pub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); + + rmw_ret_t ret = rmw_destroy_publisher(node, pub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_datawriter) { + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datawriter(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_publisher_options_t options = rmw_get_default_publisher_options(); + rmw_publisher_t * pub = rmw_create_publisher(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, pub) << rmw_get_error_string().str; + + const char * implementation_identifier = pub->implementation_identifier; + pub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datawriter(pub)); + pub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_datawriter(pub)); rmw_ret_t ret = rmw_destroy_publisher(node, pub); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -118,7 +151,30 @@ TEST_F(TestNativeEntities, get_subscriber) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); sub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); + + rmw_ret_t ret = rmw_destroy_subscription(node, sub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_datareader) { + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datareader(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_subscription_options_t options = rmw_get_default_subscription_options(); + rmw_subscription_t * sub = + rmw_create_subscription(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, sub) << rmw_get_error_string().str; + + const char * implementation_identifier = sub->implementation_identifier; + sub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datareader(sub)); + sub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_datareader(sub)); rmw_ret_t ret = rmw_destroy_subscription(node, sub); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -141,8 +197,32 @@ TEST_F(TestNativeEntities, get_service) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); srv->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_request_subscriber(srv)); - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_subscriber(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); + + rmw_ret_t ret = rmw_destroy_service(node, srv); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_service_dds) { + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datareader(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datawriter(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_service_t * srv = rmw_create_service(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, srv) << rmw_get_error_string().str; + + const char * implementation_identifier = srv->implementation_identifier; + srv->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datareader(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datawriter(srv)); + srv->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_request_datareader(srv)); + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_datawriter(srv)); rmw_ret_t ret = rmw_destroy_service(node, srv); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -165,8 +245,32 @@ TEST_F(TestNativeEntities, get_client) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); client->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_request_publisher(client)); - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_publisher(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); + + rmw_ret_t ret = rmw_destroy_client(node, client); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_client_dds) { + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datawriter(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datareader(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_client_t * client = rmw_create_client(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, client) << rmw_get_error_string().str; + + const char * implementation_identifier = client->implementation_identifier; + client->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datawriter(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datareader(client)); + client->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_request_datawriter(client)); + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_datareader(client)); rmw_ret_t ret = rmw_destroy_client(node, client); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp index c86087b84..3a86ea1e1 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp @@ -18,12 +18,35 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { +/// Return a native FastRTPS publisher handle for the request. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_request_publisher(rmw_client_t * client); + +/// Return a native FastRTPS subscriber handle for the response. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client); + /// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or @@ -33,7 +56,7 @@ namespace rmw_fastrtps_dynamic_cpp */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_request_publisher(rmw_client_t * client); +get_request_datawriter(rmw_client_t * client); /// Return a native Fast DDS DataReader handle for the response. /** @@ -44,7 +67,7 @@ get_request_publisher(rmw_client_t * client); */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_response_subscriber(rmw_client_t * client); +get_response_datareader(rmw_client_t * client); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp index 017605269..629712d09 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp @@ -17,12 +17,24 @@ #include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastrtps/participant/Participant.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { +/// Return a native FastRTPS participant handle. +/** + * This function function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Participant * +get_participant(rmw_node_t * node); + /// Return a native Fast DDS DomainParticipant handle. /** * The function returns `NULL` when either the node handle is `NULL` or when the @@ -32,7 +44,7 @@ namespace rmw_fastrtps_dynamic_cpp */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DomainParticipant * -get_participant(rmw_node_t * node); +get_domain_participant(rmw_node_t * node); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp index f563c3606..adbde1bb9 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp @@ -17,12 +17,24 @@ #include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastrtps/publisher/Publisher.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { +/// Return a native FastRTPS publisher handle. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_publisher(rmw_publisher_t * publisher); + /// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or @@ -32,7 +44,7 @@ namespace rmw_fastrtps_dynamic_cpp */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_publisher(rmw_publisher_t * publisher); +get_datawriter(rmw_publisher_t * publisher); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp index 59974dc49..dc7a35a6a 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp @@ -18,12 +18,35 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { +/// Return a native FastRTPS subscriber handle for the request. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_request_subscriber(rmw_service_t * service); + +/// Return a native FastRTPS publisher handle for the response. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service); + /// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or @@ -33,7 +56,7 @@ namespace rmw_fastrtps_dynamic_cpp */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_request_subscriber(rmw_service_t * service); +get_request_datareader(rmw_service_t * service); /// Return a native Fast DDS DataWriter handle for the response. /** @@ -44,7 +67,7 @@ get_request_subscriber(rmw_service_t * service); */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataWriter * -get_response_publisher(rmw_service_t * service); +get_response_datawriter(rmw_service_t * service); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp index 34812e883..93485029d 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -17,12 +17,24 @@ #include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastrtps/subscriber/Subscriber.h" + #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { +/// Return a native FastRTPS subscriber handle. +/** + * This function has been deprecated and always returns `NULL`. + * + * \return `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_subscriber(rmw_subscription_t * subscription); + /// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or @@ -32,7 +44,7 @@ namespace rmw_fastrtps_dynamic_cpp */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataReader * -get_subscriber(rmw_subscription_t * subscription); +get_datareader(rmw_subscription_t * subscription); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp index e25b657d9..f9c56d86f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -20,8 +20,22 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastdds::dds::DataWriter * +eprosima::fastrtps::Publisher * get_request_publisher(rmw_client_t * client) +{ + (void)client; + return nullptr; +} + +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client) +{ + (void)client; + return nullptr; +} + +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client) { if (!client) { return nullptr; @@ -34,7 +48,7 @@ get_request_publisher(rmw_client_t * client) } eprosima::fastdds::dds::DataReader * -get_response_subscriber(rmw_client_t * client) +get_response_datareader(rmw_client_t * client) { if (!client) { return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index 34c046c01..a0451038e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -23,8 +23,15 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastdds::dds::DomainParticipant * +eprosima::fastrtps::Participant * get_participant(rmw_node_t * node) +{ + (void)node; + return nullptr; +} + +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node) { if (!node) { return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp index 1651dce5e..ae00672d8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -20,8 +20,15 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastdds::dds::DataWriter * +eprosima::fastrtps::Publisher * get_publisher(rmw_publisher_t * publisher) +{ + (void)publisher; + return nullptr; +} + +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher) { if (!publisher) { return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp index 68d8a75c2..c62260681 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -20,8 +20,22 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastdds::dds::DataReader * +eprosima::fastrtps::Subscriber * get_request_subscriber(rmw_service_t * service) +{ + (void)service; + return nullptr; +} + +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service) +{ + (void)service; + return nullptr; +} + +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service) { if (!service) { return nullptr; @@ -34,7 +48,7 @@ get_request_subscriber(rmw_service_t * service) } eprosima::fastdds::dds::DataWriter * -get_response_publisher(rmw_service_t * service) +get_response_datawriter(rmw_service_t * service) { if (!service) { return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp index e8111fccc..88fd70573 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -20,8 +20,15 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastdds::dds::DataReader * +eprosima::fastrtps::Subscriber * get_subscriber(rmw_subscription_t * subscription) +{ + (void)subscription; + return nullptr; +} + +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription) { if (!subscription) { return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp index 21289bd5e..c042a583d 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp @@ -76,7 +76,18 @@ TEST_F(TestNativeEntities, get_participant) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); node->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); +} + +TEST_F(TestNativeEntities, get_domain_participant) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(nullptr)); + + const char * implementation_identifier = node->implementation_identifier; + node->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); + node->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); } TEST_F(TestNativeEntities, get_publisher) { @@ -95,7 +106,29 @@ TEST_F(TestNativeEntities, get_publisher) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); pub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); + + rmw_ret_t ret = rmw_destroy_publisher(node, pub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_datawriter) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_publisher_options_t options = rmw_get_default_publisher_options(); + rmw_publisher_t * pub = rmw_create_publisher(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, pub) << rmw_get_error_string().str; + + const char * implementation_identifier = pub->implementation_identifier; + pub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(pub)); + pub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(pub)); rmw_ret_t ret = rmw_destroy_publisher(node, pub); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -118,7 +151,30 @@ TEST_F(TestNativeEntities, get_subscriber) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); sub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); + + rmw_ret_t ret = rmw_destroy_subscription(node, sub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_datareader) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_subscription_options_t options = rmw_get_default_subscription_options(); + rmw_subscription_t * sub = + rmw_create_subscription(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, sub) << rmw_get_error_string().str; + + const char * implementation_identifier = sub->implementation_identifier; + sub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(sub)); + sub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(sub)); rmw_ret_t ret = rmw_destroy_subscription(node, sub); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -141,8 +197,32 @@ TEST_F(TestNativeEntities, get_service) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); srv->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_subscriber(srv)); - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_subscriber(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); + + rmw_ret_t ret = rmw_destroy_service(node, srv); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_service_dds) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_service_t * srv = rmw_create_service(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, srv) << rmw_get_error_string().str; + + const char * implementation_identifier = srv->implementation_identifier; + srv->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(srv)); + srv->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(srv)); + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(srv)); rmw_ret_t ret = rmw_destroy_service(node, srv); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; @@ -165,8 +245,32 @@ TEST_F(TestNativeEntities, get_client) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); client->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_publisher(client)); - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_publisher(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); + + rmw_ret_t ret = rmw_destroy_client(node, client); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_client_dds) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_client_t * client = rmw_create_client(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, client) << rmw_get_error_string().str; + + const char * implementation_identifier = client->implementation_identifier; + client->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(client)); + client->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(client)); + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(client)); rmw_ret_t ret = rmw_destroy_client(node, client); EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; From c2f68997366eef04c8389703d5f130af58c70d54 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 09:00:04 +0200 Subject: [PATCH 39/60] Removed old native getters API. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_cpp/get_client.hpp | 23 ---- .../rmw_fastrtps_cpp/get_participant.hpp | 12 -- .../rmw_fastrtps_cpp/get_publisher.hpp | 12 -- .../include/rmw_fastrtps_cpp/get_service.hpp | 23 ---- .../rmw_fastrtps_cpp/get_subscriber.hpp | 12 -- rmw_fastrtps_cpp/src/get_client.cpp | 14 --- rmw_fastrtps_cpp/src/get_participant.cpp | 7 -- rmw_fastrtps_cpp/src/get_publisher.cpp | 7 -- rmw_fastrtps_cpp/src/get_service.cpp | 14 --- rmw_fastrtps_cpp/src/get_subscriber.cpp | 7 -- .../test/test_get_native_entities.cpp | 104 ------------------ .../rmw_fastrtps_dynamic_cpp/get_client.hpp | 23 ---- .../get_participant.hpp | 12 -- .../get_publisher.hpp | 12 -- .../rmw_fastrtps_dynamic_cpp/get_service.hpp | 23 ---- .../get_subscriber.hpp | 12 -- rmw_fastrtps_dynamic_cpp/src/get_client.cpp | 14 --- .../src/get_participant.cpp | 7 -- .../src/get_publisher.cpp | 7 -- rmw_fastrtps_dynamic_cpp/src/get_service.cpp | 14 --- .../src/get_subscriber.cpp | 7 -- .../test/test_get_native_entities.cpp | 104 ------------------ 22 files changed, 470 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp index b4f4aa2e2..51af2dd9b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp @@ -18,35 +18,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS publisher handle for the request. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client); - -/// Return a native FastRTPS subscriber handle for the response. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client); - /// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp index 70a070064..cbd76bfbc 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/domain/DomainParticipant.hpp" -#include "fastrtps/participant/Participant.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native Fast RTPS participant handle. -/** - * This function function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node); - /// Return a native Fast DDS DomainParticipant handle. /** * This function returns `NULL` when either the node handle is `NULL` or when the diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp index 3d449b78b..3f09df5ed 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" -#include "fastrtps/publisher/Publisher.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS publisher handle. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher); - /// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp index 804967a90..edcc2c19b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp @@ -18,35 +18,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS subscriber handle for the request. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service); - -/// Return a native FastRTPS publisher handle for the response. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service); - /// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp index 963f24a34..82449bf99 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/visibility_control.h" namespace rmw_fastrtps_cpp { -/// Return a native FastRTPS subscriber handle. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription); - /// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or diff --git a/rmw_fastrtps_cpp/src/get_client.cpp b/rmw_fastrtps_cpp/src/get_client.cpp index c127ffddb..3e29ae368 100644 --- a/rmw_fastrtps_cpp/src/get_client.cpp +++ b/rmw_fastrtps_cpp/src/get_client.cpp @@ -23,20 +23,6 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client) -{ - (void)client; - return nullptr; -} - -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client) -{ - (void)client; - return nullptr; -} - eprosima::fastdds::dds::DataWriter * get_request_datawriter(rmw_client_t * client) { diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index e93ff2516..372671200 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -22,13 +22,6 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node) -{ - (void)node; - return nullptr; -} - eprosima::fastdds::dds::DomainParticipant * get_domain_participant(rmw_node_t * node) { diff --git a/rmw_fastrtps_cpp/src/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index c2a237d37..fae5aee70 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -20,13 +20,6 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher) -{ - (void)publisher; - return nullptr; -} - eprosima::fastdds::dds::DataWriter * get_datawriter(rmw_publisher_t * publisher) { diff --git a/rmw_fastrtps_cpp/src/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index bc4e01dad..bfa407f5c 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -20,20 +20,6 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service) -{ - (void)service; - return nullptr; -} - -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service) -{ - (void)service; - return nullptr; -} - eprosima::fastdds::dds::DataReader * get_request_datareader(rmw_service_t * service) { diff --git a/rmw_fastrtps_cpp/src/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 9f8f877ac..08f4baed3 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -20,13 +20,6 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription) -{ - (void)subscription; - return nullptr; -} - eprosima::fastdds::dds::DataReader * get_datareader(rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp index 04070db8d..54dc13ac8 100644 --- a/rmw_fastrtps_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_cpp/test/test_get_native_entities.cpp @@ -68,17 +68,6 @@ class TestNativeEntities : public ::testing::Test rmw_node_t * node{nullptr}; }; -TEST_F(TestNativeEntities, get_participant) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(nullptr)); - - const char * implementation_identifier = node->implementation_identifier; - node->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(node)); - node->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(node)); -} - TEST_F(TestNativeEntities, get_domain_participant) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_domain_participant(nullptr)); @@ -90,28 +79,6 @@ TEST_F(TestNativeEntities, get_domain_participant) { EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_domain_participant(node)); } -TEST_F(TestNativeEntities, get_publisher) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(nullptr)); - - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - constexpr char topic_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_publisher_options_t options = rmw_get_default_publisher_options(); - rmw_publisher_t * pub = rmw_create_publisher(node, ts, topic_name, &qos_profile, &options); - ASSERT_NE(nullptr, pub) << rmw_get_error_string().str; - - const char * implementation_identifier = pub->implementation_identifier; - pub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); - pub->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); - - rmw_ret_t ret = rmw_destroy_publisher(node, pub); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - TEST_F(TestNativeEntities, get_datawriter) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datawriter(nullptr)); @@ -134,29 +101,6 @@ TEST_F(TestNativeEntities, get_datawriter) { EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; } -TEST_F(TestNativeEntities, get_subscriber) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(nullptr)); - - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - constexpr char topic_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_subscription_options_t options = rmw_get_default_subscription_options(); - rmw_subscription_t * sub = - rmw_create_subscription(node, ts, topic_name, &qos_profile, &options); - ASSERT_NE(nullptr, sub) << rmw_get_error_string().str; - - const char * implementation_identifier = sub->implementation_identifier; - sub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); - sub->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); - - rmw_ret_t ret = rmw_destroy_subscription(node, sub); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - TEST_F(TestNativeEntities, get_datareader) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datareader(nullptr)); @@ -181,30 +125,6 @@ TEST_F(TestNativeEntities, get_datareader) { } TEST_F(TestNativeEntities, get_service) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_subscriber(nullptr)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_publisher(nullptr)); - - const rosidl_service_type_support_t * ts = - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); - constexpr char service_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_service_t * srv = rmw_create_service(node, ts, service_name, &qos_profile); - ASSERT_NE(nullptr, srv) << rmw_get_error_string().str; - - const char * implementation_identifier = srv->implementation_identifier; - srv->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_subscriber(srv)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); - srv->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_subscriber(srv)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); - - rmw_ret_t ret = rmw_destroy_service(node, srv); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - -TEST_F(TestNativeEntities, get_service_dds) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datareader(nullptr)); EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datawriter(nullptr)); @@ -229,30 +149,6 @@ TEST_F(TestNativeEntities, get_service_dds) { } TEST_F(TestNativeEntities, get_client) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_publisher(nullptr)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_subscriber(nullptr)); - - const rosidl_service_type_support_t * ts = - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); - constexpr char service_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_client_t * client = rmw_create_client(node, ts, service_name, &qos_profile); - ASSERT_NE(nullptr, client) << rmw_get_error_string().str; - - const char * implementation_identifier = client->implementation_identifier; - client->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_publisher(client)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); - client->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_publisher(client)); - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); - - rmw_ret_t ret = rmw_destroy_client(node, client); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - -TEST_F(TestNativeEntities, get_client_dds) { EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_request_datawriter(nullptr)); EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_response_datareader(nullptr)); diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp index 3a86ea1e1..b4357dec9 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp @@ -18,35 +18,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS publisher handle for the request. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client); - -/// Return a native FastRTPS subscriber handle for the response. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client); - /// Return a native Fast DDS DataWriter handle for the request. /** * The function returns `NULL` when either the client handle is `NULL` or diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp index 629712d09..53c228954 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/domain/DomainParticipant.hpp" -#include "fastrtps/participant/Participant.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS participant handle. -/** - * This function function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node); - /// Return a native Fast DDS DomainParticipant handle. /** * The function returns `NULL` when either the node handle is `NULL` or when the diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp index adbde1bb9..28817bc93 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" -#include "fastrtps/publisher/Publisher.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS publisher handle. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher); - /// Return a native Fast DDS DataWriter handle. /** * The function returns `NULL` when either the publisher handle is `NULL` or diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp index dc7a35a6a..753a74b44 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp @@ -18,35 +18,12 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS subscriber handle for the request. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service); - -/// Return a native FastRTPS publisher handle for the response. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service); - /// Return a native Fast DDS DataReader handle for the request. /** * The function returns `NULL` when either the service handle is `NULL` or diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp index 93485029d..a61b2bca2 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -17,24 +17,12 @@ #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS subscriber handle. -/** - * This function has been deprecated and always returns `NULL`. - * - * \return `NULL` - */ -RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription); - /// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or diff --git a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp index f9c56d86f..69a7af466 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -20,20 +20,6 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client) -{ - (void)client; - return nullptr; -} - -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client) -{ - (void)client; - return nullptr; -} - eprosima::fastdds::dds::DataWriter * get_request_datawriter(rmw_client_t * client) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index a0451038e..23a2c56b2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -23,13 +23,6 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node) -{ - (void)node; - return nullptr; -} - eprosima::fastdds::dds::DomainParticipant * get_domain_participant(rmw_node_t * node) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp index ae00672d8..2b5b5dadd 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -20,13 +20,6 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher) -{ - (void)publisher; - return nullptr; -} - eprosima::fastdds::dds::DataWriter * get_datawriter(rmw_publisher_t * publisher) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp index c62260681..b93ea26df 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -20,20 +20,6 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service) -{ - (void)service; - return nullptr; -} - -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service) -{ - (void)service; - return nullptr; -} - eprosima::fastdds::dds::DataReader * get_request_datareader(rmw_service_t * service) { diff --git a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp index 88fd70573..c3c51fa58 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -20,13 +20,6 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription) -{ - (void)subscription; - return nullptr; -} - eprosima::fastdds::dds::DataReader * get_datareader(rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp index c042a583d..89ac01a9e 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp @@ -68,17 +68,6 @@ class TestNativeEntities : public ::testing::Test rmw_node_t * node{nullptr}; }; -TEST_F(TestNativeEntities, get_participant) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(nullptr)); - - const char * implementation_identifier = node->implementation_identifier; - node->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); - node->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); -} - TEST_F(TestNativeEntities, get_domain_participant) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(nullptr)); @@ -90,28 +79,6 @@ TEST_F(TestNativeEntities, get_domain_participant) { EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); } -TEST_F(TestNativeEntities, get_publisher) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(nullptr)); - - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - constexpr char topic_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_publisher_options_t options = rmw_get_default_publisher_options(); - rmw_publisher_t * pub = rmw_create_publisher(node, ts, topic_name, &qos_profile, &options); - ASSERT_NE(nullptr, pub) << rmw_get_error_string().str; - - const char * implementation_identifier = pub->implementation_identifier; - pub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); - pub->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); - - rmw_ret_t ret = rmw_destroy_publisher(node, pub); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - TEST_F(TestNativeEntities, get_datawriter) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(nullptr)); @@ -134,29 +101,6 @@ TEST_F(TestNativeEntities, get_datawriter) { EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; } -TEST_F(TestNativeEntities, get_subscriber) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(nullptr)); - - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - constexpr char topic_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_subscription_options_t options = rmw_get_default_subscription_options(); - rmw_subscription_t * sub = - rmw_create_subscription(node, ts, topic_name, &qos_profile, &options); - ASSERT_NE(nullptr, sub) << rmw_get_error_string().str; - - const char * implementation_identifier = sub->implementation_identifier; - sub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); - sub->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); - - rmw_ret_t ret = rmw_destroy_subscription(node, sub); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - TEST_F(TestNativeEntities, get_datareader) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(nullptr)); @@ -181,30 +125,6 @@ TEST_F(TestNativeEntities, get_datareader) { } TEST_F(TestNativeEntities, get_service) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_subscriber(nullptr)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(nullptr)); - - const rosidl_service_type_support_t * ts = - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); - constexpr char service_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_service_t * srv = rmw_create_service(node, ts, service_name, &qos_profile); - ASSERT_NE(nullptr, srv) << rmw_get_error_string().str; - - const char * implementation_identifier = srv->implementation_identifier; - srv->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_subscriber(srv)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); - srv->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_subscriber(srv)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); - - rmw_ret_t ret = rmw_destroy_service(node, srv); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - -TEST_F(TestNativeEntities, get_service_dds) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(nullptr)); EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(nullptr)); @@ -229,30 +149,6 @@ TEST_F(TestNativeEntities, get_service_dds) { } TEST_F(TestNativeEntities, get_client) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_publisher(nullptr)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(nullptr)); - - const rosidl_service_type_support_t * ts = - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); - constexpr char service_name[] = "/test"; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - rmw_client_t * client = rmw_create_client(node, ts, service_name, &qos_profile); - ASSERT_NE(nullptr, client) << rmw_get_error_string().str; - - const char * implementation_identifier = client->implementation_identifier; - client->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_publisher(client)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); - client->implementation_identifier = implementation_identifier; - - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_publisher(client)); - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); - - rmw_ret_t ret = rmw_destroy_client(node, client); - EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; -} - -TEST_F(TestNativeEntities, get_client_dds) { EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(nullptr)); EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(nullptr)); From 7ad1f99777d2a032b2356298db23317b5176ea74 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 10:09:27 +0200 Subject: [PATCH 40/60] Use pointers on output arguments. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_client.cpp | 8 ++++---- rmw_fastrtps_cpp/src/rmw_service.cpp | 8 ++++---- rmw_fastrtps_cpp/src/subscription.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 8 ++++---- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 8 ++++---- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 4 ++-- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 4 ++-- rmw_fastrtps_shared_cpp/src/utils.cpp | 12 ++++++------ 10 files changed, 32 insertions(+), 32 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c80f5a637..81dccad96 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -140,8 +140,8 @@ rmw_fastrtps_cpp::create_publisher( participant_info, topic_name_mangled, type_name, - des_topic, - fastdds_type)) + &des_topic, + &fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_publisher() called for existing topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 6dbd53982..452e0b3a4 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -163,8 +163,8 @@ rmw_create_client( participant_info, request_topic_name, request_type_name, - request_topic_desc, - request_fastdds_type)) + &request_topic_desc, + &request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing request topic name %s with incompatible type %s", @@ -179,8 +179,8 @@ rmw_create_client( participant_info, response_topic_name, response_type_name, - response_topic_desc, - response_fastdds_type)) + &response_topic_desc, + &response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing response topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 1c3f0e1e4..df0aafdba 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -168,8 +168,8 @@ rmw_create_service( participant_info, request_topic_name, request_type_name, - request_topic_desc, - request_fastdds_type)) + &request_topic_desc, + &request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing request topic name %s with incompatible type %s", @@ -184,8 +184,8 @@ rmw_create_service( participant_info, response_topic_name, response_type_name, - response_topic_desc, - response_fastdds_type)) + &response_topic_desc, + &response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing response topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 030f8055f..ed4ace8a3 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -144,8 +144,8 @@ create_subscription( participant_info, topic_name_mangled, type_name, - des_topic, - fastdds_type)) + &des_topic, + &fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_subscription() called for existing topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 3f7f7c8c7..575a8cf03 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -142,8 +142,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( participant_info, topic_name_mangled, type_name, - des_topic, - fastdds_type)) + &des_topic, + &fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_publisher() called with existing topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index c2997223a..e944df883 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -166,8 +166,8 @@ rmw_create_client( participant_info, request_topic_name, request_type_name, - request_topic_desc, - request_fastdds_type)) + &request_topic_desc, + &request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing request topic name %s with incompatible type %s", @@ -182,8 +182,8 @@ rmw_create_client( participant_info, response_topic_name, response_type_name, - response_topic_desc, - response_fastdds_type)) + &response_topic_desc, + &response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_client() called for existing response topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index a55ec2c00..71b74e0d8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -174,8 +174,8 @@ rmw_create_service( participant_info, request_topic_name, request_type_name, - request_topic_desc, - request_fastdds_type)) + &request_topic_desc, + &request_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing request topic name %s with incompatible type %s", @@ -190,8 +190,8 @@ rmw_create_service( participant_info, response_topic_name, response_type_name, - response_topic_desc, - response_fastdds_type)) + &response_topic_desc, + &response_fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_service() called for existing response topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 386043045..9ee9f070c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -146,8 +146,8 @@ create_subscription( participant_info, topic_name_mangled, type_name, - des_topic, - fastdds_type)) + &des_topic, + &fastdds_type)) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "create_subscription() called with existing topic name %s with incompatible type %s", diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 6b12c0bce..0c2d697dc 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -42,8 +42,8 @@ find_and_check_topic_and_type( const CustomParticipantInfo * participant_info, const std::string & topic_name, const std::string & type_name, - eprosima::fastdds::dds::TopicDescription * & returned_topic, - eprosima::fastdds::dds::TypeSupport & returned_type); + eprosima::fastdds::dds::TopicDescription ** returned_topic, + eprosima::fastdds::dds::TypeSupport * returned_type); RMW_FASTRTPS_SHARED_CPP_PUBLIC void diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 31acf8e15..2318fa6fc 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -56,18 +56,18 @@ find_and_check_topic_and_type( const CustomParticipantInfo * participant_info, const std::string & topic_name, const std::string & type_name, - eprosima::fastdds::dds::TopicDescription * & returned_topic, - eprosima::fastdds::dds::TypeSupport & returned_type) + eprosima::fastdds::dds::TopicDescription ** returned_topic, + eprosima::fastdds::dds::TypeSupport * returned_type) { // Searchs for an already existing topic - returned_topic = participant_info->participant_->lookup_topicdescription(topic_name); - if (nullptr != returned_topic) { - if (returned_topic->get_type_name() != type_name) { + *returned_topic = participant_info->participant_->lookup_topicdescription(topic_name); + if (nullptr != *returned_topic) { + if ((*returned_topic)->get_type_name() != type_name) { return false; } } - returned_type = participant_info->participant_->find_type(type_name); + *returned_type = participant_info->participant_->find_type(type_name); return true; } From c7a514964631ea8a71f8994d34d1188ef8ea884e Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 10:15:55 +0200 Subject: [PATCH 41/60] Flip operands on equal comparisons. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/utils.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 2318fa6fc..53cfb48a6 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -32,18 +32,18 @@ namespace rmw_fastrtps_shared_cpp rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) { // not switch because it is not an enum class - if (code == ReturnCode_t::RETCODE_OK) { + if (ReturnCode_t::RETCODE_OK == code) { return RMW_RET_OK; - } else if (code == ReturnCode_t::RETCODE_ERROR) { + } else if (ReturnCode_t::RETCODE_ERROR == code) { // repeats the error to avoid too many 'if' comparisons return RMW_RET_ERROR; - } else if (code == ReturnCode_t::RETCODE_TIMEOUT) { + } else if (ReturnCode_t::RETCODE_TIMEOUT == code) { return RMW_RET_TIMEOUT; - } else if (code == ReturnCode_t::RETCODE_UNSUPPORTED) { + } else if (ReturnCode_t::RETCODE_UNSUPPORTED == code) { return RMW_RET_UNSUPPORTED; - } else if (code == ReturnCode_t::RETCODE_BAD_PARAMETER) { + } else if (ReturnCode_t::RETCODE_BAD_PARAMETER == code) { return RMW_RET_INVALID_ARGUMENT; - } else if (code == ReturnCode_t::RETCODE_OUT_OF_RESOURCES) { + } else if (ReturnCode_t::RETCODE_OUT_OF_RESOURCES == code) { // Could be that out of resources comes from a different source than a bad allocation return RMW_RET_BAD_ALLOC; } else { From 30a462bcc5ff9f375c944b094b6eb37df8617bc3 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 11:16:28 +0200 Subject: [PATCH 42/60] Apply suggestions on destroy_service and destroy_client. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 18 ++++-------------- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 19 ++++--------------- 2 files changed, 8 insertions(+), 29 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 6cbf42f2d..d3fde8d9c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -43,18 +43,8 @@ __rmw_destroy_client( auto participant_info = static_cast(node->context->impl->participant_info); auto info = static_cast(client->data); - bool info_is_valid = true; - if (nullptr == info || nullptr == info->request_writer_ || nullptr == info->response_reader_) { - RMW_SET_ERROR_MSG("destroy_client() called with invalid info struct"); - final_ret = RMW_RET_INVALID_ARGUMENT; - info_is_valid = false; - } else if (nullptr == participant_info || nullptr == common_context) { - RMW_SET_ERROR_MSG("destroy_client() called on invalid context"); - final_ret = RMW_RET_INVALID_ARGUMENT; - info_is_valid = false; - } - if (info_is_valid) { + { // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -87,7 +77,7 @@ __rmw_destroy_client( ///// // Delete DataWriter and DataReader - if (info_is_valid) { + { std::lock_guard lck(participant_info->entity_creation_mutex_); // Keep pointers to topics, so we can remove them later @@ -99,7 +89,7 @@ __rmw_destroy_client( if (ret != ReturnCode_t::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); - final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = RMW_RET_ERROR; info->response_reader_->set_listener(nullptr); } @@ -113,7 +103,7 @@ __rmw_destroy_client( if (ret != ReturnCode_t::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); - final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = RMW_RET_ERROR; info->request_writer_->set_listener(nullptr); } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index d8816b17c..1eba8692c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -56,18 +56,7 @@ __rmw_destroy_service( auto participant_info = static_cast(node->context->impl->participant_info); auto info = static_cast(service->data); - bool info_is_valid = true; - if (nullptr == info || nullptr == info->request_reader_ || nullptr == info->response_writer_) { - RMW_SET_ERROR_MSG("destroy_service() called with invalid info struct"); - final_ret = RMW_RET_INVALID_ARGUMENT; - info_is_valid = false; - } else if (nullptr == participant_info || nullptr == common_context) { - RMW_SET_ERROR_MSG("destroy_service() called on invalid context"); - final_ret = RMW_RET_INVALID_ARGUMENT; - info_is_valid = false; - } - - if (info_is_valid) { + { // Update graph std::lock_guard guard(common_context->node_update_mutex); rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -100,7 +89,7 @@ __rmw_destroy_service( ///// // Delete DataWriter and DataReader - if (info_is_valid) { + { std::lock_guard lck(participant_info->entity_creation_mutex_); // Keep pointers to topics, so we can remove them later @@ -112,7 +101,7 @@ __rmw_destroy_service( if (ret != ReturnCode_t::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datareader"); - final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = RMW_RET_ERROR; info->request_reader_->set_listener(nullptr); } @@ -126,7 +115,7 @@ __rmw_destroy_service( if (ret != ReturnCode_t::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datawriter"); - final_ret = rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + final_ret = RMW_RET_ERROR; info->response_writer_->set_listener(nullptr); } From 0c988c70557f5ce77adc40f0c384b2286ea50e6d Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 13:13:11 +0200 Subject: [PATCH 43/60] Added topic holder utility. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 27 ++++++++++++++ rmw_fastrtps_shared_cpp/src/utils.cpp | 37 +++++++++++++++++++ 2 files changed, 64 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 0c2d697dc..0935c532b 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -32,10 +32,37 @@ namespace rmw_fastrtps_shared_cpp { +/// Auxiliary struct to cleanup a topic created during entity creation +struct TopicHolder +{ + ~TopicHolder() + { + if (should_be_deleted) { + participant->delete_topic(topic); + } + } + + eprosima::fastdds::dds::DomainParticipant * participant = nullptr; + eprosima::fastdds::dds::TopicDescription * desc = nullptr; + eprosima::fastdds::dds::Topic * topic = nullptr; + bool should_be_deleted = false; +}; + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +cast_or_create_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * desc, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & topic_qos, + bool is_writer_topic, + TopicHolder * topic_holder); + RMW_FASTRTPS_SHARED_CPP_PUBLIC bool find_and_check_topic_and_type( diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 53cfb48a6..ce541c244 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -51,6 +51,43 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) } } +bool +cast_or_create_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * desc, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & topic_qos, + bool is_writer_topic, + TopicHolder * topic_holder) +{ + topic_holder->should_be_deleted = false; + topic_holder->participant = participant; + topic_holder->desc = desc; + topic_holder->topic = nullptr; + + if (nullptr == desc) { + topic_holder->topic = participant->create_topic( + topic_name, + type_name, + topic_qos); + + if (!topic_holder->topic) { + return false; + } + + topic_holder->desc = topic_holder->topic; + topic_holder->should_be_deleted = true; + } else { + if (is_writer_topic) { + topic_holder->topic = dynamic_cast(desc); + assert(nullptr != topic_holder->topic); + } + } + + return true; +} + bool find_and_check_topic_and_type( const CustomParticipantInfo * participant_info, From 745e152b319336c58328c7c8fd6421b5b7538b88 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 13:42:56 +0200 Subject: [PATCH 44/60] Apply suggestions on publisher creation. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 83 +++++++--------------- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 77 +++++++------------- 2 files changed, 50 insertions(+), 110 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 81dccad96..2e82857bd 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -93,14 +93,6 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - ///// - // Get Participant and Publisher - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - ///// // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( @@ -149,6 +141,11 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + ///// // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; @@ -160,7 +157,11 @@ rmw_fastrtps_cpp::create_publisher( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->listener_; + if (info->type_support_) { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + } delete info; }); @@ -190,10 +191,6 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } info->type_support_ = fastdds_type; - auto cleanup_type_support = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->type_support_.get_type_name()); - }); ///// // Create Listener @@ -207,49 +204,22 @@ rmw_fastrtps_cpp::create_publisher( } } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - ///// // Create and register Topic - bool topic_created = false; - eprosima::fastdds::dds::Topic * topic = nullptr; - if (!des_topic) { - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; - } - - topic = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); - - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); - return nullptr; - } - - topic_created = true; - } else { - topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; - } + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, des_topic, + topic_name_mangled, type_name, topicQos, true, &topic)) + { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; } - - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, topic, topic_created]() { - if (topic_created) { - domainParticipant->delete_topic(topic); - } - }); ///// // Create DataWriter @@ -284,7 +254,7 @@ rmw_fastrtps_cpp::create_publisher( // Creates DataWriter info->data_writer_ = publisher->create_datawriter( - topic, + topic.topic, dataWriterQos, info->listener_); @@ -330,11 +300,10 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher->options = *publisher_options; + topic.should_be_deleted = false; cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); - cleanup_topic.cancel(); - cleanup_listener.cancel(); - cleanup_type_support.cancel(); cleanup_info.cancel(); + return rmw_publisher; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 575a8cf03..a63c5da56 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -95,14 +95,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - ///// - // Get Participant and Publisher - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - ///// // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( @@ -151,6 +143,11 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + ///// // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; @@ -162,7 +159,11 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->listener_; + if (info->type_support_) { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + } delete info; }); @@ -204,10 +205,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } info->type_support_ = fastdds_type; - auto cleanup_type_support = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->type_support_.get_type_name()); - }); ///// // Create Listener @@ -221,47 +218,23 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - ///// // Create and register Topic - bool topic_created = false; - eprosima::fastdds::dds::Topic * topic = nullptr; - if (!des_topic) { - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; - } + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - topic = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); - - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); - return nullptr; - } - } else { - topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; - } + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; } - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, topic, topic_created]() { - if (topic_created) { - domainParticipant->delete_topic(topic); - } - }); + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, des_topic, + topic_name_mangled, type_name, topicQos, true, &topic)) + { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; + } ///// // Create DataWriter @@ -296,7 +269,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // Creates DataWriter (with publisher name to not change name policy) info->data_writer_ = publisher->create_datawriter( - topic, + topic.topic, dataWriterQos, info->listener_); @@ -342,11 +315,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->options = *publisher_options; + topic.should_be_deleted = false; cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); - cleanup_topic.cancel(); - cleanup_listener.cancel(); - cleanup_type_support.cancel(); return_type_support.cancel(); cleanup_info.cancel(); return rmw_publisher; From 7898d5e4123ba7629870b72a41f80abf9aa1808d Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 15:54:26 +0200 Subject: [PATCH 45/60] Apply suggestions on subscription creation. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/subscription.cpp | 73 +++++++------------ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 73 +++++++------------ 2 files changed, 50 insertions(+), 96 deletions(-) diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index ed4ace8a3..975fe86b8 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -97,14 +97,6 @@ create_subscription( return nullptr; } - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); - ///// // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( @@ -153,6 +145,11 @@ create_subscription( return nullptr; } + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + ///// // Create the custom Subscriber struct (info) CustomSubscriberInfo * info = nullptr; @@ -164,7 +161,11 @@ create_subscription( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->listener_; + if (info->type_support_) { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + } delete info; }); @@ -194,10 +195,6 @@ create_subscription( return nullptr; } info->type_support_ = fastdds_type; - auto cleanup_type_support = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->type_support_.get_type_name()); - }); ///// // Create Listener @@ -211,42 +208,24 @@ create_subscription( } } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - ///// // Create and register Topic - eprosima::fastdds::dds::Topic * topic = nullptr; - if (!des_topic) { - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("create_subscription() failed setting topic QoS"); - return nullptr; - } - - topic = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); - - if (!topic) { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - des_topic = topic; + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, des_topic, + topic_name_mangled, type_name, topicQos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; } - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, topic]() { - if (topic) { - domainParticipant->delete_topic(topic); - } - }); + des_topic = topic.desc; ///// // Create DataReader @@ -318,11 +297,9 @@ create_subscription( rmw_subscription->options = *subscription_options; rmw_subscription->can_loan_messages = false; + topic.should_be_deleted = false; cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); - cleanup_topic.cancel(); - cleanup_listener.cancel(); - cleanup_type_support.cancel(); cleanup_info.cancel(); return rmw_subscription; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 9ee9f070c..a3179095c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -99,14 +99,6 @@ create_subscription( return nullptr; } - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); - ///// // Get RMW Type Support const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( @@ -155,6 +147,11 @@ create_subscription( return nullptr; } + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + ///// // Create the custom Subscriber struct (info) CustomSubscriberInfo * info = nullptr; @@ -166,7 +163,11 @@ create_subscription( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->listener_; + if (info->type_support_) { + domainParticipant->unregister_type(info->type_support_.get_type_name()); + } delete info; }); @@ -208,10 +209,6 @@ create_subscription( } info->type_support_ = fastdds_type; - auto cleanup_type_support = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->type_support_.get_type_name()); - }); ///// // Create Listener @@ -225,42 +222,24 @@ create_subscription( } } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - ///// // Create and register Topic - eprosima::fastdds::dds::Topic * topic = nullptr; - if (!des_topic) { - // Use Topic Qos Default - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - - if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("create_subscription() failed setting topic QoS"); - return nullptr; - } - - topic = domainParticipant->create_topic( - topic_name_mangled, - type_name, - topicQos); - - if (!topic) { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - des_topic = topic; + eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topicQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, des_topic, + topic_name_mangled, type_name, topicQos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; } - auto cleanup_topic = rcpputils::make_scope_exit( - [domainParticipant, topic]() { - if (topic) { - domainParticipant->delete_topic(topic); - } - }); + des_topic = topic.desc; ///// // Create DataReader @@ -332,11 +311,9 @@ create_subscription( rmw_subscription->options = *subscription_options; rmw_subscription->can_loan_messages = false; + topic.should_be_deleted = false; cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); - cleanup_topic.cancel(); - cleanup_listener.cancel(); - cleanup_type_support.cancel(); return_type_support.cancel(); cleanup_info.cancel(); return rmw_subscription; From f1d4621155c24ca70f67192e1e79c197abb81e01 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 16:14:40 +0200 Subject: [PATCH 46/60] Apply suggestions on client creation. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/rmw_client.cpp | 105 +++++--------------- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 105 +++++--------------- 2 files changed, 54 insertions(+), 156 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 452e0b3a4..fd73541ff 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -102,13 +102,8 @@ rmw_create_client( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -197,7 +192,15 @@ rmw_create_client( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + } delete info; }); @@ -234,20 +237,12 @@ rmw_create_client( return nullptr; } info->request_type_support_ = request_fastdds_type; - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); - }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); - }); ///// // Create Listeners @@ -257,22 +252,12 @@ rmw_create_client( return nullptr; } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); return nullptr; } - auto cleanup_pub_listener = rcpputils::make_scope_exit( - [info]() { - delete info->pub_listener_; - }); - ///// // Create and register Topics // Same default topic QoS for both topics @@ -283,59 +268,27 @@ rmw_create_client( } // Create response topic - eprosima::fastdds::dds::Topic * response_topic = nullptr; - if (!response_topic_desc) { - response_topic = domainParticipant->create_topic( - response_topic_name, - response_type_name, - topicQos); - - if (!response_topic) { - RMW_SET_ERROR_MSG("create_client() failed to create response topic"); - return nullptr; - } - - response_topic_desc = response_topic; + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, response_topic_desc, + response_topic_name, response_type_name, topicQos, false, &response_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); + return nullptr; } - auto cleanup_response_topic = rcpputils::make_scope_exit( - [domainParticipant, response_topic]() { - if (response_topic) { - domainParticipant->delete_topic(response_topic); - } - }); + response_topic_desc = response_topic.desc; // Create request topic - eprosima::fastdds::dds::Topic * request_topic = nullptr; - bool request_topic_created = false; - if (!request_topic_desc) { - request_topic = domainParticipant->create_topic( - request_topic_name, - request_type_name, - topicQos); - - if (!request_topic) { - RMW_SET_ERROR_MSG("create_client() failed to create request topic"); - return nullptr; - } - - request_topic_desc = request_topic; - request_topic_created = true; - } else { - request_topic = dynamic_cast(request_topic_desc); - if (!request_topic) { - RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); - return nullptr; - } + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, request_topic_desc, + request_topic_name, request_type_name, topicQos, true, &request_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); + return nullptr; } - auto cleanup_request_topic = rcpputils::make_scope_exit( - [domainParticipant, request_topic, request_topic_created]() { - if (request_topic_created) { - domainParticipant->delete_topic(request_topic); - } - }); - info->request_topic_ = request_topic_name; info->response_topic_ = response_topic_name; @@ -419,7 +372,7 @@ rmw_create_client( // Creates DataWriter info->request_writer_ = publisher->create_datawriter( - request_topic, + request_topic.topic, dataWriterQos, info->pub_listener_); @@ -512,15 +465,11 @@ rmw_create_client( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_client.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_request_topic.cancel(); - cleanup_response_topic.cancel(); - cleanup_pub_listener.cancel(); - cleanup_listener.cancel(); - cleanup_type_support_response.cancel(); - cleanup_type_support_request.cancel(); cleanup_info.cancel(); return rmw_client; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index e944df883..913b5b9fe 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -105,13 +105,8 @@ rmw_create_client( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -200,7 +195,15 @@ rmw_create_client( } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + } delete info; }); @@ -261,20 +264,12 @@ rmw_create_client( return nullptr; } info->request_type_support_ = request_fastdds_type; - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); - }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); - }); ///// // Create Listeners @@ -284,22 +279,12 @@ rmw_create_client( return nullptr; } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); return nullptr; } - auto cleanup_pub_listener = rcpputils::make_scope_exit( - [info]() { - delete info->pub_listener_; - }); - ///// // Create and register Topics // Same default topic QoS for both topics @@ -310,59 +295,27 @@ rmw_create_client( } // Create response topic - eprosima::fastdds::dds::Topic * response_topic = nullptr; - if (!response_topic_desc) { - response_topic = domainParticipant->create_topic( - response_topic_name, - response_type_name, - topicQos); - - if (!response_topic) { - RMW_SET_ERROR_MSG("create_client() failed to create response topic"); - return nullptr; - } - - response_topic_desc = response_topic; + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, response_topic_desc, + response_topic_name, response_type_name, topicQos, false, &response_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); + return nullptr; } - auto cleanup_response_topic = rcpputils::make_scope_exit( - [domainParticipant, response_topic]() { - if (response_topic) { - domainParticipant->delete_topic(response_topic); - } - }); + response_topic_desc = response_topic.desc; // Create request topic - eprosima::fastdds::dds::Topic * request_topic = nullptr; - bool request_topic_created = false; - if (!request_topic_desc) { - request_topic = domainParticipant->create_topic( - request_topic_name, - request_type_name, - topicQos); - - if (!request_topic) { - RMW_SET_ERROR_MSG("create_client() failed to create request topic"); - return nullptr; - } - - request_topic_desc = request_topic; - request_topic_created = true; - } else { - request_topic = dynamic_cast(request_topic_desc); - if (!request_topic) { - RMW_SET_ERROR_MSG("create_client() called with request topic not of class Topic"); - return nullptr; - } + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, request_topic_desc, + request_topic_name, request_type_name, topicQos, true, &request_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); + return nullptr; } - auto cleanup_request_topic = rcpputils::make_scope_exit( - [domainParticipant, request_topic, request_topic_created]() { - if (request_topic_created) { - domainParticipant->delete_topic(request_topic); - } - }); - info->request_topic_ = request_topic_name; info->response_topic_ = response_topic_name; @@ -446,7 +399,7 @@ rmw_create_client( // Creates DataWriter info->request_writer_ = publisher->create_datawriter( - request_topic, + request_topic.topic, dataWriterQos, info->pub_listener_); @@ -535,15 +488,11 @@ rmw_create_client( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_client.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_request_topic.cancel(); - cleanup_response_topic.cancel(); - cleanup_pub_listener.cancel(); - cleanup_listener.cancel(); - cleanup_type_support_response.cancel(); - cleanup_type_support_request.cancel(); return_response_type_support.cancel(); return_request_type_support.cancel(); cleanup_info.cancel(); From 7ed04a17971190896fea24b26d51d4e83fe046a6 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 16:28:17 +0200 Subject: [PATCH 47/60] Apply suggestions on service creation. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/rmw_service.cpp | 105 +++++-------------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 105 +++++-------------- 2 files changed, 54 insertions(+), 156 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index df0aafdba..48f662d61 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -107,13 +107,8 @@ rmw_create_service( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -201,7 +196,15 @@ rmw_create_service( return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + } delete info; }); @@ -236,20 +239,12 @@ rmw_create_service( return nullptr; } info->request_type_support_ = request_fastdds_type; - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); - }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); - }); ///// // Create Listeners @@ -259,22 +254,12 @@ rmw_create_service( return nullptr; } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); return nullptr; } - auto cleanup_pub_listener = rcpputils::make_scope_exit( - [info]() { - delete info->pub_listener_; - }); - ///// // Create and register Topics // Same default topic QoS for both topics @@ -285,59 +270,27 @@ rmw_create_service( } // Create request topic - eprosima::fastdds::dds::Topic * request_topic = nullptr; - if (!request_topic_desc) { - request_topic = domainParticipant->create_topic( - request_topic_name, - request_type_name, - topicQos); - - if (!request_topic) { - RMW_SET_ERROR_MSG("create_service() failed to create request topic"); - return nullptr; - } - - request_topic_desc = request_topic; + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, request_topic_desc, + request_topic_name, request_type_name, topicQos, false, &request_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); + return nullptr; } - auto cleanup_request_topic = rcpputils::make_scope_exit( - [domainParticipant, request_topic]() { - if (request_topic) { - domainParticipant->delete_topic(request_topic); - } - }); + request_topic_desc = request_topic.desc; // Create response topic - eprosima::fastdds::dds::Topic * response_topic = nullptr; - bool response_topic_created = false; - if (!response_topic_desc) { - response_topic = domainParticipant->create_topic( - response_topic_name, - response_type_name, - topicQos); - - if (!response_topic) { - RMW_SET_ERROR_MSG("create_service() failed to create response topic"); - return nullptr; - } - - response_topic_desc = response_topic; - response_topic_created = true; - } else { - response_topic = dynamic_cast(response_topic_desc); - if (!response_topic) { - RMW_SET_ERROR_MSG("create_service() called with response topic not of class Topic"); - return nullptr; - } + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, response_topic_desc, + response_topic_name, response_type_name, topicQos, true, &response_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; } - auto cleanup_response_topic = rcpputils::make_scope_exit( - [domainParticipant, response_topic, response_topic_created]() { - if (response_topic_created) { - domainParticipant->delete_topic(response_topic); - } - }); - // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; @@ -422,7 +375,7 @@ rmw_create_service( // Creates DataWriter info->response_writer_ = publisher->create_datawriter( - response_topic, + response_topic.topic, dataWriterQos, info->pub_listener_); @@ -511,15 +464,11 @@ rmw_create_service( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_service.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_response_topic.cancel(); - cleanup_request_topic.cancel(); - cleanup_pub_listener.cancel(); - cleanup_listener.cancel(); - cleanup_type_support_response.cancel(); - cleanup_type_support_request.cancel(); cleanup_info.cancel(); return rmw_service; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 71b74e0d8..84d8b7ba7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -113,13 +113,8 @@ rmw_create_service( static_cast(node->context->impl->participant_info); eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; - RMW_CHECK_FOR_NULL_WITH_MSG(domainParticipant, "participant handle is null", return nullptr); - eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", return nullptr); - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - RMW_CHECK_FOR_NULL_WITH_MSG(subscriber, "subscriber handle is null", return nullptr); ///// // Get RMW Type Support @@ -207,7 +202,15 @@ rmw_create_service( return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( - [info]() { + [info, domainParticipant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + } delete info; }); @@ -266,20 +269,12 @@ rmw_create_service( return nullptr; } info->request_type_support_ = request_fastdds_type; - auto cleanup_type_support_request = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); - }); if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } info->response_type_support_ = response_fastdds_type; - auto cleanup_type_support_response = rcpputils::make_scope_exit( - [info, domainParticipant]() { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); - }); ///// // Create Listeners @@ -289,22 +284,12 @@ rmw_create_service( return nullptr; } - auto cleanup_listener = rcpputils::make_scope_exit( - [info]() { - delete info->listener_; - }); - info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); return nullptr; } - auto cleanup_pub_listener = rcpputils::make_scope_exit( - [info]() { - delete info->pub_listener_; - }); - ///// // Create and register Topics // Same default topic QoS for both topics @@ -315,59 +300,27 @@ rmw_create_service( } // Create request topic - eprosima::fastdds::dds::Topic * request_topic = nullptr; - if (!request_topic_desc) { - request_topic = domainParticipant->create_topic( - request_topic_name, - request_type_name, - topicQos); - - if (!request_topic) { - RMW_SET_ERROR_MSG("create_service() failed to create request topic"); - return nullptr; - } - - request_topic_desc = request_topic; + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, request_topic_desc, + request_topic_name, request_type_name, topicQos, false, &request_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); + return nullptr; } - auto cleanup_request_topic = rcpputils::make_scope_exit( - [domainParticipant, request_topic]() { - if (request_topic) { - domainParticipant->delete_topic(request_topic); - } - }); + request_topic_desc = request_topic.desc; // Create response topic - eprosima::fastdds::dds::Topic * response_topic = nullptr; - bool response_topic_created = false; - if (!response_topic_desc) { - response_topic = domainParticipant->create_topic( - response_topic_name, - response_type_name, - topicQos); - - if (!response_topic) { - RMW_SET_ERROR_MSG("create_service() failed to create response topic"); - return nullptr; - } - - response_topic_desc = response_topic; - response_topic_created = true; - } else { - response_topic = dynamic_cast(response_topic_desc); - if (!response_topic) { - RMW_SET_ERROR_MSG("create_service() called with response topic not of class Topic"); - return nullptr; - } + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + domainParticipant, response_topic_desc, + response_topic_name, response_type_name, topicQos, true, &response_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; } - auto cleanup_response_topic = rcpputils::make_scope_exit( - [domainParticipant, response_topic, response_topic_created]() { - if (response_topic_created) { - domainParticipant->delete_topic(response_topic); - } - }); - // Keyword to find DataWrtier and DataReader QoS std::string topic_name_fallback = "service"; @@ -452,7 +405,7 @@ rmw_create_service( // Creates DataWriter info->response_writer_ = publisher->create_datawriter( - response_topic, + response_topic.topic, dataWriterQos, info->pub_listener_); @@ -538,15 +491,11 @@ rmw_create_service( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_service.cancel(); cleanup_datawriter.cancel(); cleanup_datareader.cancel(); - cleanup_response_topic.cancel(); - cleanup_request_topic.cancel(); - cleanup_pub_listener.cancel(); - cleanup_listener.cancel(); - cleanup_type_support_response.cancel(); - cleanup_type_support_request.cancel(); return_response_type_support.cancel(); return_request_type_support.cancel(); cleanup_info.cancel(); From 940ac4a5ddb72cd6555d8e36d8058373829a644a Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 16:43:20 +0200 Subject: [PATCH 48/60] Linters Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 2 +- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 2e82857bd..7ed9c0057 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -211,7 +211,7 @@ rmw_fastrtps_cpp::create_publisher( RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } - + rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( domainParticipant, des_topic, diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 975fe86b8..7d9158012 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -215,7 +215,7 @@ create_subscription( RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } - + rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( domainParticipant, des_topic, diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index a3179095c..587abafa9 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -229,7 +229,7 @@ create_subscription( RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } - + rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( domainParticipant, des_topic, From 88d6ac04b6df002915a7e7c26afdd386c36b0a7c Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 20:40:13 +0200 Subject: [PATCH 49/60] Using auto where possible. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/rmw_client.cpp | 10 +++------- rmw_fastrtps_cpp/src/rmw_service.cpp | 10 +++------- rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp | 6 ++---- rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 6 ++---- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 6 +++--- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 10 +++++----- 6 files changed, 18 insertions(+), 30 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index fd73541ff..8197786f5 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -133,14 +133,10 @@ rmw_create_client( // Find and check existing topics and types // Create Topic and Type names - const service_type_support_callbacks_t * service_members; - const message_type_support_callbacks_t * request_members; - const message_type_support_callbacks_t * response_members; - - service_members = static_cast(type_support->data); - request_members = static_cast( + auto service_members = static_cast(type_support->data); + auto request_members = static_cast( service_members->request_members_->data); - response_members = static_cast( + auto response_members = static_cast( service_members->response_members_->data); std::string request_type_name = _create_type_name(request_members); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 48f662d61..5450adc56 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -138,14 +138,10 @@ rmw_create_service( // Find and check existing topics and types // Create Topic and Type names - const service_type_support_callbacks_t * service_members; - const message_type_support_callbacks_t * request_members; - const message_type_support_callbacks_t * response_members; - - service_members = static_cast(type_support->data); - request_members = static_cast( + auto service_members = static_cast(type_support->data); + auto request_members = static_cast( service_members->request_members_->data); - response_members = static_cast( + auto response_members = static_cast( service_members->response_members_->data); std::string request_type_name = _create_type_name(request_members); diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 33e6c13f8..840d2e830 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -81,8 +81,7 @@ bool PubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) switch (event_type) { case RMW_EVENT_LIVELINESS_LOST: { - rmw_liveliness_lost_status_t * rmw_data = - static_cast(event_info); + auto rmw_data = static_cast(event_info); rmw_data->total_count = liveliness_lost_status_.total_count; rmw_data->total_count_change = liveliness_lost_status_.total_count_change; liveliness_lost_status_.total_count_change = 0; @@ -91,8 +90,7 @@ bool PubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) break; case RMW_EVENT_OFFERED_DEADLINE_MISSED: { - rmw_offered_deadline_missed_status_t * rmw_data = - static_cast(event_info); + auto rmw_data = static_cast(event_info); rmw_data->total_count = offered_deadline_missed_status_.total_count; rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; offered_deadline_missed_status_.total_count_change = 0; diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index d9eb011d1..17da1fa79 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -85,8 +85,7 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) switch (event_type) { case RMW_EVENT_LIVELINESS_CHANGED: { - rmw_liveliness_changed_status_t * rmw_data = - static_cast(event_info); + auto rmw_data = static_cast(event_info); rmw_data->alive_count = liveliness_changed_status_.alive_count; rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; @@ -98,8 +97,7 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) break; case RMW_EVENT_REQUESTED_DEADLINE_MISSED: { - rmw_requested_deadline_missed_status_t * rmw_data = - static_cast(event_info); + auto rmw_data = static_cast(event_info); rmw_data->total_count = requested_deadline_missed_status_.total_count; rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; requested_deadline_missed_status_.total_count_change = 0; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 6cde76a8c..f5e442acc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -65,7 +65,7 @@ _take( subscription->implementation_identifier, identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - CustomSubscriberInfo * info = static_cast(subscription->data); + auto info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); eprosima::fastdds::dds::SampleInfo sinfo; @@ -109,7 +109,7 @@ _take_sequence( subscription->implementation_identifier, identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - CustomSubscriberInfo * info = static_cast(subscription->data); + auto info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); // Limit the upper bound of reads to the number unread at the beginning. @@ -272,7 +272,7 @@ _take_serialized_message( subscription->implementation_identifier, identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - CustomSubscriberInfo * info = static_cast(subscription->data); + auto info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); eprosima::fastcdr::FastBuffer buffer; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index d3cfa24dc..4275e4ffc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -47,7 +47,7 @@ check_wait_set_for_data( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); + auto custom_client_info = static_cast(data); if (custom_client_info && custom_client_info->listener_->hasData()) { return true; } @@ -57,7 +57,7 @@ check_wait_set_for_data( if (services) { for (size_t i = 0; i < services->service_count; ++i) { void * data = services->services[i]; - CustomServiceInfo * custom_service_info = static_cast(data); + auto custom_service_info = static_cast(data); if (custom_service_info && custom_service_info->listener_->hasData()) { return true; } @@ -114,7 +114,7 @@ __rmw_wait( // error. // - Heap is corrupt. // In all three cases, it's better if this crashes soon enough. - CustomWaitsetInfo * wait_set_info = static_cast(wait_set->data); + auto wait_set_info = static_cast(wait_set->data); std::mutex * conditionMutex = &wait_set_info->condition_mutex; std::condition_variable * conditionVariable = &wait_set_info->condition; @@ -129,7 +129,7 @@ __rmw_wait( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); + auto custom_client_info = static_cast(data); custom_client_info->listener_->attachCondition(conditionMutex, conditionVariable); } } @@ -205,7 +205,7 @@ __rmw_wait( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); + auto custom_client_info = static_cast(data); custom_client_info->listener_->detachCondition(); if (!custom_client_info->listener_->hasData()) { clients->clients[i] = 0; From f4e24ec8d512b73aef21ac467356659e67f6a675 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 21:30:13 +0200 Subject: [PATCH 50/60] Documentation on utils.hpp Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 70 ++++++++++++++++++- 1 file changed, 67 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 0935c532b..5cd19b165 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -32,7 +32,20 @@ namespace rmw_fastrtps_shared_cpp { -/// Auxiliary struct to cleanup a topic created during entity creation +/** +* Auxiliary struct to cleanup a topic created during entity creation. +* It is similar to a unique_ptr and its custom deleter at the same time. +* +* The creation process of an entity should be as follows: +* - find_and_check_topic_and_type is called +* - If the type is not found, it is created and registered +* - cast_or_create_topic is called on a stack-allocated TopicHolder +* - An early return will delete the topic if necessary +* - create_datawriter or create_datareader is called +* - Rest of the initialization process is performed +* - Before correctly returning the created entity, field should_be_deleted is set to false +* to avoid deletion of the topic +*/ struct TopicHolder { ~TopicHolder() @@ -48,10 +61,38 @@ struct TopicHolder bool should_be_deleted = false; }; +/** +* Convert a Fast DDS return code into the corresponding rmw_ret_t +* \param code The Fast DDS return code to be translated +* \return the corresponding rmw_ret_t value +*/ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t - cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t); - + cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); + +/** +* Auxiliary method to reuse or create a topic during the creation of an entity. +* +* Input: +* \param participant DomainParticipant where the topic will be created. +* \param desc TopicDescription returned by find_and_check_topic_and_type. +* \param topic_name Name of the topic. +* \param type_name Name of the type. +* \param topic_qos QoS with which to create the topic. +* \param is_writer_topic Whether the resulting topic will be used on a DataWriter. +* +* Output: +* \param topic_holder Will hold the pointer to the topic along with the necessary +* information for its deletion. +* When is_writer_topic is true, topic_holder->topic can be +* directly used on a create_datawriter call. +* When is_writer_topic is false, topic_holder->desc can be +* directly used on a create_datareader call. +* +* \return true when the topic was reused (topic_holder->should_be_deleted will be false) +* \return true when the topic was created (topic_holder->should_be_deleted will be true) +* \return false when the topic could not be created +*/ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool cast_or_create_topic( @@ -63,6 +104,22 @@ cast_or_create_topic( bool is_writer_topic, TopicHolder * topic_holder); +/** +* Tries to find already registered topic and type. +* +* Input: +* \param participant_info CustomParticipantInfo associated to the context. +* \param topic_name Name of the topic for the entity being created. +* \param type_name Name of the type for the entity being created. +* +* Output: +* \param returned_topic TopicDescription for topic_name +* \param returned_type TypeSupport for type_name +* +* \return false if `topic_name` was previously created with a different type name. +* \return true when there is no such conflict. Returned topic and type may be null +* if they were not previously registered on the participant. +*/ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool find_and_check_topic_and_type( @@ -72,6 +129,13 @@ find_and_check_topic_and_type( eprosima::fastdds::dds::TopicDescription ** returned_topic, eprosima::fastdds::dds::TypeSupport * returned_type); +/** +* Performs removal of associated topic and type. +* +* \param participant_info CustomParticipantInfo associated to the context. +* \param topic Topic of the entity being deleted. +* \param type TypeSupport of the entity being deleted. +*/ RMW_FASTRTPS_SHARED_CPP_PUBLIC void remove_topic_and_type( From 87b2371ac1567eb5075c8ae607401cfb1e686e69 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 31 Mar 2021 21:36:32 +0200 Subject: [PATCH 51/60] Linters Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 5cd19b165..938df27c2 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -68,7 +68,7 @@ struct TopicHolder */ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t - cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); +cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); /** * Auxiliary method to reuse or create a topic during the creation of an entity. From 6ac968a15d2c85a8e42441a2b5da5a92c3996240 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 16:57:06 +0200 Subject: [PATCH 52/60] Fixed some basic review comments. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 4 +--- rmw_fastrtps_cpp/src/rmw_client.cpp | 2 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 2 +- rmw_fastrtps_cpp/src/subscription.cpp | 4 +--- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 4 +--- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 9 +++------ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 4 +--- 8 files changed, 10 insertions(+), 21 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 7ed9c0057..f1605b9cd 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -148,9 +148,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create the custom Publisher struct (info) - CustomPublisherInfo * info = nullptr; - - info = new (std::nothrow) CustomPublisherInfo(); + auto info = new (std::nothrow) CustomPublisherInfo(); if (!info) { RMW_SET_ERROR_MSG("create_publisher() failed to allocate CustomPublisherInfo"); return nullptr; diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 8197786f5..a12a6ece8 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -289,7 +289,7 @@ rmw_create_client( info->response_topic_ = response_topic_name; // Keyword to find DataWriter and DataReader QoS - std::string topic_name_fallback = "client"; + const std::string topic_name_fallback = "client"; ///// // Create response DataReader diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 5450adc56..fa243ca00 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -288,7 +288,7 @@ rmw_create_service( } // Keyword to find DataWrtier and DataReader QoS - std::string topic_name_fallback = "service"; + const std::string topic_name_fallback = "service"; ///// // Create request DataReader diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 7d9158012..2f85e9868 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -152,9 +152,7 @@ create_subscription( ///// // Create the custom Subscriber struct (info) - CustomSubscriberInfo * info = nullptr; - - info = new (std::nothrow) CustomSubscriberInfo(); + auto info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index a63c5da56..1b8dc5773 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -150,9 +150,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create the custom Publisher struct (info) - CustomPublisherInfo * info = nullptr; - - info = new (std::nothrow) CustomPublisherInfo(); + auto info = new (std::nothrow) CustomPublisherInfo(); if (!info) { RMW_SET_ERROR_MSG("create_publisher() failed to allocate CustomPublisherInfo"); return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 913b5b9fe..3a3b46f8c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -320,7 +320,7 @@ rmw_create_client( info->response_topic_ = response_topic_name; // Keyword to find DataWrtier and DataReader QoS - std::string topic_name_fallback = "client"; + const std::string topic_name_fallback = "client"; ///// // Create response DataReader diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 84d8b7ba7..fd69b5bfc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -144,12 +144,9 @@ rmw_create_service( // Find and check existing topics and types // Create Topic and Type names - const void * untyped_request_members; - const void * untyped_response_members; - - untyped_request_members = get_request_ptr( + const void * untyped_request_members = get_request_ptr( type_support->data, type_support->typesupport_identifier); - untyped_response_members = get_response_ptr( + const void * untyped_response_members = get_response_ptr( type_support->data, type_support->typesupport_identifier); std::string request_type_name = _create_type_name( @@ -322,7 +319,7 @@ rmw_create_service( } // Keyword to find DataWrtier and DataReader QoS - std::string topic_name_fallback = "service"; + const std::string topic_name_fallback = "service"; ///// // Create request DataReader diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 587abafa9..f5a584557 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -154,9 +154,7 @@ create_subscription( ///// // Create the custom Subscriber struct (info) - CustomSubscriberInfo * info = nullptr; - - info = new (std::nothrow) CustomSubscriberInfo(); + auto info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); return nullptr; From 1e8f025aff1f7c41128b093605fd8ec33059b0c9 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 16:58:37 +0200 Subject: [PATCH 53/60] Removed redundant initialization of listeners. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 1 - rmw_fastrtps_cpp/src/subscription.cpp | 1 - rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 1 - rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 1 - 4 files changed, 4 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index f1605b9cd..44eaba3db 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -192,7 +192,6 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create Listener - info->listener_ = nullptr; if (create_publisher_listener) { info->listener_ = new (std::nothrow) PubListener(info); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 2f85e9868..df8340053 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -196,7 +196,6 @@ create_subscription( ///// // Create Listener - info->listener_ = nullptr; if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 1b8dc5773..623d5ae7c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -206,7 +206,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create Listener - info->listener_ = nullptr; if (create_publisher_listener) { info->listener_ = new (std::nothrow) PubListener(info); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index f5a584557..e0dbde542 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -210,7 +210,6 @@ create_subscription( ///// // Create Listener - info->listener_ = nullptr; if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); From 2f8229591ef7534718124c846da7942c494421b4 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 17:07:18 +0200 Subject: [PATCH 54/60] Fixed doxygen Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 48 ++++++++----------- 1 file changed, 21 insertions(+), 27 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 938df27c2..8b6910e64 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -63,7 +63,7 @@ struct TopicHolder /** * Convert a Fast DDS return code into the corresponding rmw_ret_t -* \param code The Fast DDS return code to be translated +* \param[in] code The Fast DDS return code to be translated * \return the corresponding rmw_ret_t value */ RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -73,21 +73,18 @@ cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); /** * Auxiliary method to reuse or create a topic during the creation of an entity. * -* Input: -* \param participant DomainParticipant where the topic will be created. -* \param desc TopicDescription returned by find_and_check_topic_and_type. -* \param topic_name Name of the topic. -* \param type_name Name of the type. -* \param topic_qos QoS with which to create the topic. -* \param is_writer_topic Whether the resulting topic will be used on a DataWriter. -* -* Output: -* \param topic_holder Will hold the pointer to the topic along with the necessary -* information for its deletion. -* When is_writer_topic is true, topic_holder->topic can be -* directly used on a create_datawriter call. -* When is_writer_topic is false, topic_holder->desc can be -* directly used on a create_datareader call. +* \param[in] participant DomainParticipant where the topic will be created. +* \param[in] desc TopicDescription returned by find_and_check_topic_and_type. +* \param[in] topic_name Name of the topic. +* \param[in] type_name Name of the type. +* \param[in] topic_qos QoS with which to create the topic. +* \param[in] is_writer_topic Whether the resulting topic will be used on a DataWriter. +* \param[out] topic_holder Will hold the pointer to the topic along with the necessary +* information for its deletion. +* When is_writer_topic is true, topic_holder->topic can be +* directly used on a create_datawriter call. +* When is_writer_topic is false, topic_holder->desc can be +* directly used on a create_datareader call. * * \return true when the topic was reused (topic_holder->should_be_deleted will be false) * \return true when the topic was created (topic_holder->should_be_deleted will be true) @@ -107,14 +104,11 @@ cast_or_create_topic( /** * Tries to find already registered topic and type. * -* Input: -* \param participant_info CustomParticipantInfo associated to the context. -* \param topic_name Name of the topic for the entity being created. -* \param type_name Name of the type for the entity being created. -* -* Output: -* \param returned_topic TopicDescription for topic_name -* \param returned_type TypeSupport for type_name +* \param[in] participant_info CustomParticipantInfo associated to the context. +* \param[in] topic_name Name of the topic for the entity being created. +* \param[in] type_name Name of the type for the entity being created. +* \param[out] returned_topic TopicDescription for topic_name +* \param[out] returned_type TypeSupport for type_name * * \return false if `topic_name` was previously created with a different type name. * \return true when there is no such conflict. Returned topic and type may be null @@ -132,9 +126,9 @@ find_and_check_topic_and_type( /** * Performs removal of associated topic and type. * -* \param participant_info CustomParticipantInfo associated to the context. -* \param topic Topic of the entity being deleted. -* \param type TypeSupport of the entity being deleted. +* \param[in] participant_info CustomParticipantInfo associated to the context. +* \param[in] topic Topic of the entity being deleted. +* \param[in] type TypeSupport of the entity being deleted. */ RMW_FASTRTPS_SHARED_CPP_PUBLIC void From 5823fd30097945e7637ddf3ec807ca9f28b2b421 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 18:40:13 +0200 Subject: [PATCH 55/60] Use snake_case variable names. Signed-off-by: Miguel Company --- rmw_fastrtps_cpp/src/publisher.cpp | 30 +++++------ rmw_fastrtps_cpp/src/rmw_client.cpp | 52 +++++++++---------- rmw_fastrtps_cpp/src/rmw_service.cpp | 52 +++++++++---------- rmw_fastrtps_cpp/src/subscription.cpp | 26 +++++----- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 30 +++++------ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 52 +++++++++---------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 52 +++++++++---------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 26 +++++----- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 6 +-- 9 files changed, 163 insertions(+), 163 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 44eaba3db..431392120 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -143,7 +143,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Get Participant and Publisher - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; ///// @@ -155,10 +155,10 @@ rmw_fastrtps_cpp::create_publisher( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->listener_; if (info->type_support_) { - domainParticipant->unregister_type(info->type_support_.get_type_name()); + dds_participant->unregister_type(info->type_support_.get_type_name()); } delete info; }); @@ -184,7 +184,7 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } @@ -203,16 +203,16 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, des_topic, - topic_name_mangled, type_name, topicQos, true, &topic)) + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, true, &topic)) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; @@ -224,27 +224,27 @@ rmw_fastrtps_cpp::create_publisher( // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name // It does not need to check the return code, as if the profile does not exist, // the QoS is already the default - publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } @@ -252,7 +252,7 @@ rmw_fastrtps_cpp::create_publisher( // Creates DataWriter info->data_writer_ = publisher->create_datawriter( topic.topic, - dataWriterQos, + writer_qos, info->listener_); if (!info->data_writer_) { diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index a12a6ece8..171fbdc50 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -101,7 +101,7 @@ rmw_create_client( auto participant_info = static_cast(node->context->impl->participant_info); - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; @@ -188,14 +188,14 @@ rmw_create_client( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->pub_listener_; delete info->listener_; if (info->response_type_support_) { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); } if (info->request_type_support_) { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); @@ -228,13 +228,13 @@ rmw_create_client( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } info->request_type_support_ = request_fastdds_type; - if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } @@ -257,8 +257,8 @@ rmw_create_client( ///// // Create and register Topics // Same default topic QoS for both topics - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } @@ -266,8 +266,8 @@ rmw_create_client( // Create response topic rmw_fastrtps_shared_cpp::TopicHolder response_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, response_topic_desc, - response_topic_name, response_type_name, topicQos, false, &response_topic)) + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, false, &response_topic)) { RMW_SET_ERROR_MSG("create_client() failed to create response topic"); return nullptr; @@ -278,8 +278,8 @@ rmw_create_client( // Create request topic rmw_fastrtps_shared_cpp::TopicHolder request_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, request_topic_desc, - request_topic_name, request_type_name, topicQos, true, &request_topic)) + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, true, &request_topic)) { RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; @@ -297,7 +297,7 @@ rmw_create_client( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", // if it does not exist it tries with the response topic name @@ -305,15 +305,15 @@ rmw_create_client( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(response_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(response_topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -321,7 +321,7 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - dataReaderQos, + reader_qos, info->listener_); if (!info->response_reader_) { @@ -338,7 +338,7 @@ rmw_create_client( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", // if it does not exist it tries with the request topic name @@ -346,22 +346,22 @@ rmw_create_client( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(request_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(request_topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } @@ -369,7 +369,7 @@ rmw_create_client( // Creates DataWriter info->request_writer_ = publisher->create_datawriter( request_topic.topic, - dataWriterQos, + writer_qos, info->pub_listener_); if (!info->request_writer_) { diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index fa243ca00..2345b71a6 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -106,7 +106,7 @@ rmw_create_service( auto participant_info = static_cast(node->context->impl->participant_info); - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; @@ -192,14 +192,14 @@ rmw_create_service( return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->pub_listener_; delete info->listener_; if (info->response_type_support_) { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); } if (info->request_type_support_) { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); @@ -230,13 +230,13 @@ rmw_create_service( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } info->request_type_support_ = request_fastdds_type; - if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } @@ -259,8 +259,8 @@ rmw_create_service( ///// // Create and register Topics // Same default topic QoS for both topics - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } @@ -268,8 +268,8 @@ rmw_create_service( // Create request topic rmw_fastrtps_shared_cpp::TopicHolder request_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, request_topic_desc, - request_topic_name, request_type_name, topicQos, false, &request_topic)) + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, false, &request_topic)) { RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; @@ -280,8 +280,8 @@ rmw_create_service( // Create response topic rmw_fastrtps_shared_cpp::TopicHolder response_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, response_topic_desc, - response_topic_name, response_type_name, topicQos, true, &response_topic)) + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, true, &response_topic)) { RMW_SET_ERROR_MSG("create_service() failed to create response topic"); return nullptr; @@ -296,7 +296,7 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", // if it does not exist it tries with the request topic name @@ -304,15 +304,15 @@ rmw_create_service( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(request_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(request_topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -320,7 +320,7 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - dataReaderQos, + reader_qos, info->listener_); if (!info->request_reader_) { @@ -341,7 +341,7 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", // if it does not exist it tries with the request topic name @@ -349,22 +349,22 @@ rmw_create_service( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(response_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(response_topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } @@ -372,7 +372,7 @@ rmw_create_service( // Creates DataWriter info->response_writer_ = publisher->create_datawriter( response_topic.topic, - dataWriterQos, + writer_qos, info->pub_listener_); if (!info->response_writer_) { diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index df8340053..a24cb3d86 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -147,7 +147,7 @@ create_subscription( ///// // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; ///// @@ -159,10 +159,10 @@ create_subscription( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->listener_; if (info->type_support_) { - domainParticipant->unregister_type(info->type_support_.get_type_name()); + dds_participant->unregister_type(info->type_support_.get_type_name()); } delete info; }); @@ -188,7 +188,7 @@ create_subscription( return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } @@ -207,16 +207,16 @@ create_subscription( ///// // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, des_topic, - topic_name_mangled, type_name, topicQos, false, &topic)) + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; @@ -230,19 +230,19 @@ create_subscription( // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datareader which profile name matches with topic_name. If such profile does not exist, // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name // It does not need to check the return code, as if the profile does not exist, // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } @@ -250,7 +250,7 @@ create_subscription( // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, - dataReaderQos, + reader_qos, info->listener_); if (!info->data_reader_) { diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 623d5ae7c..f5dafd4b6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -145,7 +145,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Get Participant and Publisher - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; ///// @@ -157,10 +157,10 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->listener_; if (info->type_support_) { - domainParticipant->unregister_type(info->type_support_.get_type_name()); + dds_participant->unregister_type(info->type_support_.get_type_name()); } delete info; }); @@ -197,7 +197,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } @@ -217,17 +217,17 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, des_topic, - topic_name_mangled, type_name, topicQos, true, &topic)) + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, true, &topic)) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; @@ -239,27 +239,27 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name // It does not need to check the return code, as if the profile does not exist, // the QoS is already the default - publisher->get_datawriter_qos_from_profile(topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } @@ -267,7 +267,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // Creates DataWriter (with publisher name to not change name policy) info->data_writer_ = publisher->create_datawriter( topic.topic, - dataWriterQos, + writer_qos, info->listener_); if (!info->data_writer_) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 3a3b46f8c..696dab61b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -104,7 +104,7 @@ rmw_create_client( auto participant_info = static_cast(node->context->impl->participant_info); - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; @@ -195,14 +195,14 @@ rmw_create_client( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->pub_listener_; delete info->listener_; if (info->response_type_support_) { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); } if (info->request_type_support_) { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); @@ -259,13 +259,13 @@ rmw_create_client( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register request type"); return nullptr; } info->request_type_support_ = request_fastdds_type; - if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } @@ -288,8 +288,8 @@ rmw_create_client( ///// // Create and register Topics // Same default topic QoS for both topics - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } @@ -297,8 +297,8 @@ rmw_create_client( // Create response topic rmw_fastrtps_shared_cpp::TopicHolder response_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, response_topic_desc, - response_topic_name, response_type_name, topicQos, false, &response_topic)) + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, false, &response_topic)) { RMW_SET_ERROR_MSG("create_client() failed to create response topic"); return nullptr; @@ -309,8 +309,8 @@ rmw_create_client( // Create request topic rmw_fastrtps_shared_cpp::TopicHolder request_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, request_topic_desc, - request_topic_name, request_type_name, topicQos, true, &request_topic)) + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, true, &request_topic)) { RMW_SET_ERROR_MSG("create_client() failed to create request topic"); return nullptr; @@ -328,7 +328,7 @@ rmw_create_client( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile named "client", // if it does not exist it tries with the response topic name @@ -336,15 +336,15 @@ rmw_create_client( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(response_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(response_topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -352,7 +352,7 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - dataReaderQos, + reader_qos, info->listener_); if (!info->response_reader_) { @@ -369,7 +369,7 @@ rmw_create_client( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile named "client", // if it does not exist it tries with the request topic name @@ -377,22 +377,22 @@ rmw_create_client( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(request_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(request_topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } @@ -400,7 +400,7 @@ rmw_create_client( // Creates DataWriter info->request_writer_ = publisher->create_datawriter( request_topic.topic, - dataWriterQos, + writer_qos, info->pub_listener_); if (!info->request_writer_) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index fd69b5bfc..bb0f7b567 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -112,7 +112,7 @@ rmw_create_service( auto participant_info = static_cast(node->context->impl->participant_info); - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; @@ -199,14 +199,14 @@ rmw_create_service( return nullptr; } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->pub_listener_; delete info->listener_; if (info->response_type_support_) { - domainParticipant->unregister_type(info->response_type_support_.get_type_name()); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); } if (info->request_type_support_) { - domainParticipant->unregister_type(info->request_type_support_.get_type_name()); + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); @@ -261,13 +261,13 @@ rmw_create_service( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register request type"); return nullptr; } info->request_type_support_ = request_fastdds_type; - if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } @@ -290,8 +290,8 @@ rmw_create_service( ///// // Create and register Topics // Same default topic QoS for both topics - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } @@ -299,8 +299,8 @@ rmw_create_service( // Create request topic rmw_fastrtps_shared_cpp::TopicHolder request_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, request_topic_desc, - request_topic_name, request_type_name, topicQos, false, &request_topic)) + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, false, &request_topic)) { RMW_SET_ERROR_MSG("create_service() failed to create request topic"); return nullptr; @@ -311,8 +311,8 @@ rmw_create_service( // Create response topic rmw_fastrtps_shared_cpp::TopicHolder response_topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, response_topic_desc, - response_topic_name, response_type_name, topicQos, true, &response_topic)) + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, true, &response_topic)) { RMW_SET_ERROR_MSG("create_service() failed to create response topic"); return nullptr; @@ -327,7 +327,7 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile named "service", // if it does not exist it tries with the request topic name @@ -335,15 +335,15 @@ rmw_create_service( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - subscriber->get_datareader_qos_from_profile(topic_name_fallback, dataReaderQos); - subscriber->get_datareader_qos_from_profile(request_topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(request_topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -351,7 +351,7 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - dataReaderQos, + reader_qos, info->listener_); if (!info->request_reader_) { @@ -372,7 +372,7 @@ rmw_create_service( // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile // located based on topic name defined by _create_topic_name(). If no profile is found, a search // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. - eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); // Try to load the profile named "service", // if it does not exist it tries with the request topic name @@ -380,22 +380,22 @@ rmw_create_service( // the QoS is already set correctly: // If none exist is default, if only one exists is the one chosen, // if both exist topic name is chosen - publisher->get_datawriter_qos_from_profile(topic_name_fallback, dataWriterQos); - publisher->get_datawriter_qos_from_profile(response_topic_name, dataWriterQos); + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(response_topic_name, writer_qos); // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { - dataWriterQos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } - dataWriterQos.endpoint().history_memory_policy = + writer_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } @@ -403,7 +403,7 @@ rmw_create_service( // Creates DataWriter info->response_writer_ = publisher->create_datawriter( response_topic.topic, - dataWriterQos, + writer_qos, info->pub_listener_); if (!info->response_writer_) { diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index e0dbde542..4f73c70e5 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -149,7 +149,7 @@ create_subscription( ///// // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * domainParticipant = participant_info->participant_; + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; ///// @@ -161,10 +161,10 @@ create_subscription( } auto cleanup_info = rcpputils::make_scope_exit( - [info, domainParticipant]() { + [info, dds_participant]() { delete info->listener_; if (info->type_support_) { - domainParticipant->unregister_type(info->type_support_.get_type_name()); + dds_participant->unregister_type(info->type_support_.get_type_name()); } delete info; }); @@ -201,7 +201,7 @@ create_subscription( return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } @@ -221,16 +221,16 @@ create_subscription( ///// // Create and register Topic - eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topicQos)) { + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); return nullptr; } rmw_fastrtps_shared_cpp::TopicHolder topic; if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - domainParticipant, des_topic, - topic_name_mangled, type_name, topicQos, false, &topic)) + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) { RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); return nullptr; @@ -244,19 +244,19 @@ create_subscription( // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datareader which profile name matches with topic_name. If such profile does not exist, // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos dataReaderQos = subscriber->get_default_datareader_qos(); + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); // Try to load the profile with the topic name // It does not need to check the return code, as if the profile does not exist, // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, dataReaderQos); + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); if (!participant_info->leave_middleware_default_qos) { - dataReaderQos.endpoint().history_memory_policy = + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - if (!get_datareader_qos(*qos_policies, dataReaderQos)) { + if (!get_datareader_qos(*qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } @@ -264,7 +264,7 @@ create_subscription( // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, - dataReaderQos, + reader_qos, info->listener_); if (!info->data_reader_) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index dac20c454..724f6bf7a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -35,19 +35,19 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastdds::dds::DataReaderQos & dataReaderQos); + eprosima::fastdds::dds::DataReaderQos & reader_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastdds::dds::DataWriterQos & dataWriterQos); + eprosima::fastdds::dds::DataWriterQos & writer_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_topic_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastdds::dds::TopicQos & topicQos); + eprosima::fastdds::dds::TopicQos & topic_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_time_t From 7a2a52a46f627e081bb5df74fe4744d6d5428c9d Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 18:51:50 +0200 Subject: [PATCH 56/60] Reduced cleanup lambdas. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 24 ++++----------------- 1 file changed, 4 insertions(+), 20 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 262c88351..a01510ca3 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -103,6 +103,10 @@ __create_participant( // lambda to delete participant info auto cleanup_participant_info = rcpputils::make_scope_exit( [participant_info]() { + participant_info->participant_->delete_publisher(participant_info->publisher_); + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); + delete participant_info->listener_; delete participant_info; }); @@ -115,11 +119,6 @@ __create_participant( RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); return nullptr; } - // lambda to delete listener - auto cleanup_listener = rcpputils::make_scope_exit( - [participant_info]() { - delete participant_info->listener_; - }); ///// // Create Participant @@ -138,12 +137,6 @@ __create_participant( RMW_SET_ERROR_MSG("__create_participant failed to create participant"); return nullptr; } - // lambda to delete participant - auto cleanup_participant = rcpputils::make_scope_exit( - [participant_info]() { - eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( - participant_info->participant_); - }); ///// // Set participant info parameters @@ -162,12 +155,6 @@ __create_participant( return nullptr; } - // lambda to delete publisher - auto cleanup_publisher = rcpputils::make_scope_exit( - [participant_info]() { - participant_info->participant_->delete_publisher(participant_info->publisher_); - }); - ///// // Create Subscriber eprosima::fastdds::dds::SubscriberQos subscriberQos = @@ -180,9 +167,6 @@ __create_participant( return nullptr; } - cleanup_publisher.cancel(); - cleanup_participant.cancel(); - cleanup_listener.cancel(); cleanup_participant_info.cancel(); return participant_info; From 28b3b0f593f36a7dff4e9778aea74869aa94c795 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 19:19:57 +0200 Subject: [PATCH 57/60] Apply suggestions on destroy_participant. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 23 ++++++++------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index a01510ca3..06f4bb581 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -323,12 +323,10 @@ rmw_ret_t rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant_info) { if (!participant_info) { - RMW_SET_ERROR_MSG("participant_info is null"); + RMW_SET_ERROR_MSG("participant_info is null on destroy_participant"); return RMW_RET_ERROR; } - RMW_CHECK_ARGUMENT_FOR_NULL(participant_info->participant_, RMW_RET_ERROR); - // Make the participant stop listening to discovery participant_info->participant_->set_listener(nullptr); @@ -338,7 +336,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant std::vector topics_to_remove; // Remove datawriters and publisher from participant - if (participant_info->publisher_) { + { std::vector writers; participant_info->publisher_->get_datawriters(writers); for (auto writer : writers) { @@ -346,14 +344,13 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant participant_info->publisher_->delete_datawriter(writer); } ret = participant_info->participant_->delete_publisher(participant_info->publisher_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete dds publisher from participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds publisher from participant"); } } // Remove datareaders and subscriber from participant - if (participant_info->subscriber_) { + { std::vector readers; participant_info->subscriber_->get_datareaders(readers); for (auto reader : readers) { @@ -361,9 +358,8 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant participant_info->subscriber_->delete_datareader(reader); } ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete dds subscriber from participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds subscriber from participant"); } } @@ -378,9 +374,8 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( participant_info->participant_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete participant"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete participant"); } // Delete Listener From fd6fe5dc03ebfc3c61ea5f9b6c62cc884d88915a Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 19:34:51 +0200 Subject: [PATCH 58/60] Apply suggestions on destroy_publisher and destroy_subscription. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/publisher.cpp | 25 +++++++++----------- rmw_fastrtps_shared_cpp/src/subscription.cpp | 25 +++++++++----------- 2 files changed, 22 insertions(+), 28 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 8cd4748f6..b0a97c865 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -33,41 +33,38 @@ destroy_publisher( assert(publisher->implementation_identifier == identifier); static_cast(identifier); - // Get RMW Publisher - rmw_ret_t ret = RMW_RET_OK; - auto info = static_cast(publisher->data); - - if (nullptr != info) { + { std::lock_guard lck(participant_info->entity_creation_mutex_); + // Get RMW Publisher + auto info = static_cast(publisher->data); + // Keep pointer to topic, so we can remove it later auto topic = info->data_writer_->get_topic(); // Delete DataWriter ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datawriter"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (ReturnCode_t::RETCODE_OK != ret) { + RMW_SET_ERROR_MSG("Failed to delete datawriter"); + // This is the first failure on this function, and we have not changed state. + // This means it should be safe to return an error + return RMW_RET_ERROR; } // Delete DataWriter listener - if (nullptr != info->listener_) { - delete info->listener_; - } + delete info->listener_; // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); // Delete CustomPublisherInfo structure delete info; - } else { - ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(publisher->topic_name)); rmw_publisher_free(publisher); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return ret; + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index e070c92a5..cd0cdddcf 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -39,41 +39,38 @@ destroy_subscription( assert(subscription->implementation_identifier == identifier); static_cast(identifier); - // Get RMW Subscriber - rmw_ret_t ret = RMW_RET_OK; - auto info = static_cast(subscription->data); - - if (nullptr != info) { + { std::lock_guard lck(participant_info->entity_creation_mutex_); + // Get RMW Subscriber + auto info = static_cast(subscription->data); + // Keep pointer to topic, so we can remove it later auto topic = info->data_reader_->get_topicdescription(); // Delete DataReader ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->data_reader_); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); - return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); + if (ReturnCode_t::RETCODE_OK != ret) { + RMW_SET_ERROR_MSG("Failed to delete datareader"); + // This is the first failure on this function, and we have not changed state. + // This means it should be safe to return an error + return RMW_RET_ERROR; } // Delete DataReader listener - if (nullptr != info->listener_) { - delete info->listener_; - } + delete info->listener_; // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); // Delete CustomSubscriberInfo structure delete info; - } else { - ret = RMW_RET_INVALID_ARGUMENT; } rmw_free(const_cast(subscription->topic_name)); rmw_subscription_free(subscription); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return ret; + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp From e0b0b01541d2e077ef45b5fad33dd21accd14a2b Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 19:16:01 +0200 Subject: [PATCH 59/60] Unique network flows (#502) * Changes required to incorporate the network flows feature Signed-off-by: Miguel Barro Signed-off-by: Miguel Company * Update API names to match rmw updated ones Signed-off-by: Miguel Barro Signed-off-by: Miguel Company * Update to new options interface. Signed-off-by: Miguel Company * Solve windows linkage issues. Signed-off-by: Miguel Company * Remove unique flows support on publishers. Signed-off-by: Miguel Company * Update subscription creation to new Fast DDS interface Signed-off-by: Miguel Company * Update to new locator getters interface. Signed-off-by: Miguel Company * Update to endpoints interface. Signed-off-by: Miguel Company * Fix linting Signed-off-by: EduPonz * Make linters happy Signed-off-by: EduPonz * Use updated rmw interface Signed-off-by: Ananya Muddukrishna * Uncrustify Signed-off-by: Ananya Muddukrishna * Include C++ header before others Signed-off-by: Ananya Muddukrishna * Use updated rmw interface to populate flow endpoint Signed-off-by: Ananya Muddukrishna * Fixed unreferenced local variable warnings Signed-off-by: Miguel Company Co-authored-by: Miguel Barro Co-authored-by: EduPonz Co-authored-by: Ananya Muddukrishna --- rmw_fastrtps_cpp/CMakeLists.txt | 1 + rmw_fastrtps_cpp/src/publisher.cpp | 7 + .../src/rmw_get_endpoint_network_flow.cpp | 46 ++++ rmw_fastrtps_cpp/src/subscription.cpp | 32 +++ rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 1 + rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 7 + .../src/rmw_get_endpoint_network_flow.cpp | 46 ++++ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 32 +++ rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 15 ++ .../src/rmw_get_endpoint_network_flow.cpp | 204 ++++++++++++++++++ 11 files changed, 392 insertions(+) create mode 100644 rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp create mode 100644 rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 744eb7b6e..a479fb531 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(rmw_fastrtps_cpp src/serialization_format.cpp src/subscription.cpp src/type_support_common.cpp + src/rmw_get_endpoint_network_flow.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 431392120..a4b80fa30 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -86,6 +86,13 @@ rmw_fastrtps_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + if (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED == + publisher_options->require_unique_network_flow_endpoints) + { + RMW_SET_ERROR_MSG("Unique network flow endpoints not supported on publishers"); + return nullptr; + } + ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { diff --git a/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp new file mode 100644 index 000000000..371c43d88 --- /dev/null +++ b/rmw_fastrtps_cpp/src/rmw_get_endpoint_network_flow.cpp @@ -0,0 +1,46 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include "rmw/get_network_flow_endpoints.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_get_network_flow_endpoints( + publisher, + allocator, + network_flow_endpoint_array); +} + +rmw_ret_t +rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_network_flow_endpoints( + subscription, + allocator, + network_flow_endpoint_array); +} +} // extern "C" diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index a24cb3d86..4affb8ec1 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -50,6 +50,8 @@ #include "type_support_common.hpp" +using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; + namespace rmw_fastrtps_cpp { @@ -247,11 +249,41 @@ create_subscription( return nullptr; } + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints) { + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, reader_qos, info->listener_); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, + info->listener_); + } if (!info->data_reader_) { RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 9e035a7de..2d747fd37 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -94,6 +94,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/type_support_common.cpp src/type_support_proxy.cpp src/type_support_registry.cpp + src/rmw_get_endpoint_network_flow.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index f5dafd4b6..e5e51dba7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -88,6 +88,13 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + if (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED == + publisher_options->require_unique_network_flow_endpoints) + { + RMW_SET_ERROR_MSG("Unique network flow endpoints not supported on publishers"); + return nullptr; + } + ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp new file mode 100644 index 000000000..7740818e7 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_endpoint_network_flow.cpp @@ -0,0 +1,46 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/get_network_flow_endpoints.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_get_network_flow_endpoints( + publisher, + allocator, + network_flow_endpoint_array); +} + +rmw_ret_t +rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_network_flow_endpoints( + subscription, + allocator, + network_flow_endpoint_array); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 4f73c70e5..86301ff17 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -52,6 +52,8 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" +using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; + namespace rmw_fastrtps_dynamic_cpp { @@ -261,11 +263,41 @@ create_subscription( return nullptr; } + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints) { + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, reader_qos, info->listener_); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, + info->listener_); + } if (!info->data_reader_) { RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index bce4e76db..d74142afa 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -61,6 +61,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp src/rmw_event.cpp + src/rmw_get_endpoint_network_flow.cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 1a9a0148b..082e5629a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -23,6 +23,7 @@ #include "rmw/topic_endpoint_info_array.h" #include "rmw/types.h" #include "rmw/names_and_types.h" +#include "rmw/network_flow_endpoint_array.h" namespace rmw_fastrtps_shared_cpp { @@ -401,6 +402,20 @@ __rmw_qos_profile_check_compatible( char * reason, size_t reason_size); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array); + } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp new file mode 100644 index 000000000..73e6d3769 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp @@ -0,0 +1,204 @@ +// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include + +#include "rmw/get_network_flow_endpoints.h" +#include "rmw/error_handling.h" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "fastrtps/utils/IPLocator.h" + + +namespace rmw_fastrtps_shared_cpp +{ + +using Locator_t = eprosima::fastrtps::rtps::Locator_t; +using LocatorList_t = eprosima::fastrtps::rtps::LocatorList_t; +using IPLocator = eprosima::fastrtps::rtps::IPLocator; + +rmw_ret_t fill_network_flow_endpoint(rmw_network_flow_endpoint_t *, const Locator_t &); + +rmw_ret_t +__rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + rmw_ret_t res = RMW_RET_OK; + + // Retrieve the sender locators + CustomPublisherInfo * data = + static_cast(publisher->data); + LocatorList_t locators; + data->data_writer_->get_sending_locators(locators); + + if (locators.empty()) { + return res; + } + + // It must be a non-initialized array + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) + { + return res; + } + + // Allocate + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_init( + network_flow_endpoint_array, + locators.size(), + allocator))) + { + return res; + } + + // Translate the locators, on error reset the array + try { + auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; + for (const Locator_t & loc : locators) { + if (RMW_RET_OK != + (res = fill_network_flow_endpoint(rmw_nf++, loc))) + { + throw res; + } + } + } catch (rmw_ret_t) { + // clear the array + rmw_network_flow_endpoint_array_fini( + network_flow_endpoint_array); + + // set error message + RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); + } + + return res; +} + +rmw_ret_t +__rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + rmw_ret_t res = RMW_RET_OK; + + // Retrieve the listener locators + CustomSubscriberInfo * data = + static_cast(subscription->data); + LocatorList_t locators; + data->data_reader_->get_listening_locators(locators); + + if (locators.empty()) { + return res; + } + + // It must be a non-initialized array + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) + { + return res; + } + + // Allocate + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_init( + network_flow_endpoint_array, + locators.size(), + allocator))) + { + return res; + } + + // Translate the locators, on error reset the array + try { + auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; + for (const Locator_t & loc : locators) { + if (RMW_RET_OK != + (res = fill_network_flow_endpoint(rmw_nf++, loc))) + { + throw res; + } + } + } catch (rmw_ret_t) { + // clear the array + rmw_network_flow_endpoint_array_fini( + network_flow_endpoint_array); + + // set error message + RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); + } + + return res; +} + +// Ancillary translation methods +rmw_transport_protocol_t +get_transport_protocol(const Locator_t & loc) +{ + if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_UDPv6)) { + return RMW_TRANSPORT_PROTOCOL_UDP; + } else if (loc.kind & (LOCATOR_KIND_TCPv4 | LOCATOR_KIND_TCPv6)) { + return RMW_TRANSPORT_PROTOCOL_TCP; + } + + return RMW_TRANSPORT_PROTOCOL_UNKNOWN; +} + +rmw_internet_protocol_t +get_internet_protocol(const Locator_t & loc) +{ + if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_TCPv4)) { + return RMW_INTERNET_PROTOCOL_IPV4; + } else if (loc.kind & (LOCATOR_KIND_TCPv6 | LOCATOR_KIND_UDPv6)) { + return RMW_INTERNET_PROTOCOL_IPV6; + } + + return RMW_INTERNET_PROTOCOL_UNKNOWN; +} + +rmw_ret_t +fill_network_flow_endpoint( + rmw_network_flow_endpoint_t * network_flow_endpoint, + const Locator_t & locator) +{ + rmw_ret_t res = RMW_RET_OK; + + // Translate transport protocol + network_flow_endpoint->transport_protocol = get_transport_protocol(locator); + + // Translate internet protocol + network_flow_endpoint->internet_protocol = get_internet_protocol(locator); + + // Set the port + network_flow_endpoint->transport_port = IPLocator::getPhysicalPort(locator); + + // Set the address + std::string address = IPLocator::ip_to_string(locator); + + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_set_internet_address( + network_flow_endpoint, + address.c_str(), + address.length()))) + { + return res; + } + + return res; +} + +} // namespace rmw_fastrtps_shared_cpp From 2f4e0cc9b7d29affa73f9e3bca19140e6ea368bb Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 5 Apr 2021 23:28:22 +0200 Subject: [PATCH 60/60] Fix segfault on cleanup lambda Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/participant.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 06f4bb581..90ebed03d 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -103,9 +103,11 @@ __create_participant( // lambda to delete participant info auto cleanup_participant_info = rcpputils::make_scope_exit( [participant_info]() { - participant_info->participant_->delete_publisher(participant_info->publisher_); - eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( - participant_info->participant_); + if (nullptr != participant_info->participant_) { + participant_info->participant_->delete_publisher(participant_info->publisher_); + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); + } delete participant_info->listener_; delete participant_info; });