From 29050bfdb02dda7a05cd87d9311f685762ca5542 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 6 Apr 2021 03:43:14 +0200 Subject: [PATCH] Refactor to use DDS standard API (#518) Signed-off-by: Miguel Company Co-authored-by: Miguel Barro Co-authored-by: EduPonz --- rmw_fastrtps_cpp/CMakeLists.txt | 4 +- .../rmw_fastrtps_cpp/ServiceTypeSupport.hpp | 5 +- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 13 +- .../include/rmw_fastrtps_cpp/get_client.hpp | 21 +- .../rmw_fastrtps_cpp/get_participant.hpp | 13 +- .../rmw_fastrtps_cpp/get_publisher.hpp | 11 +- .../include/rmw_fastrtps_cpp/get_service.hpp | 21 +- .../rmw_fastrtps_cpp/get_subscriber.hpp | 11 +- .../include/rmw_fastrtps_cpp/publisher.hpp | 1 - .../include/rmw_fastrtps_cpp/subscription.hpp | 1 - rmw_fastrtps_cpp/src/get_client.cpp | 15 +- rmw_fastrtps_cpp/src/get_participant.cpp | 6 +- rmw_fastrtps_cpp/src/get_publisher.cpp | 6 +- rmw_fastrtps_cpp/src/get_service.cpp | 12 +- rmw_fastrtps_cpp/src/get_subscriber.cpp | 6 +- .../src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_cpp/src/publisher.cpp | 219 +++++--- rmw_fastrtps_cpp/src/rmw_client.cpp | 418 +++++++++------ .../src/rmw_compare_gids_equal.cpp | 2 - rmw_fastrtps_cpp/src/rmw_service.cpp | 419 +++++++++------ rmw_fastrtps_cpp/src/subscription.cpp | 223 +++++--- rmw_fastrtps_cpp/src/type_support_common.hpp | 11 - .../test/test_get_native_entities.cpp | 50 +- rmw_fastrtps_cpp/test/test_logging.cpp | 16 +- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 4 +- .../MessageTypeSupport.hpp | 6 +- .../MessageTypeSupport_impl.hpp | 6 +- .../ServiceTypeSupport.hpp | 5 +- .../ServiceTypeSupport_impl.hpp | 5 +- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 13 +- .../TypeSupport_impl.hpp | 8 +- .../rmw_fastrtps_dynamic_cpp/get_client.hpp | 21 +- .../get_participant.hpp | 11 +- .../get_publisher.hpp | 11 +- .../rmw_fastrtps_dynamic_cpp/get_service.hpp | 21 +- .../get_subscriber.hpp | 11 +- rmw_fastrtps_dynamic_cpp/src/get_client.cpp | 12 +- .../src/get_participant.cpp | 8 +- .../src/get_publisher.cpp | 6 +- rmw_fastrtps_dynamic_cpp/src/get_service.cpp | 12 +- .../src/get_subscriber.cpp | 6 +- .../src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 232 +++++--- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 507 +++++++++++------- .../src/rmw_compare_gids_equal.cpp | 2 - rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 499 ++++++++++------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 234 +++++--- .../src/type_support_common.cpp | 8 - .../src/type_support_common.hpp | 8 - .../test/test_get_native_entities.cpp | 50 +- .../test/test_logging.cpp | 16 +- rmw_fastrtps_shared_cpp/CMakeLists.txt | 7 +- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 25 +- .../create_rmw_gid.hpp | 2 +- .../custom_client_info.hpp | 77 +-- .../custom_event_info.hpp | 7 - .../custom_participant_info.hpp | 42 +- .../custom_publisher_info.hpp | 49 +- .../custom_service_info.hpp | 84 +-- .../custom_subscriber_info.hpp | 68 +-- .../rmw_fastrtps_shared_cpp/guid_utils.hpp | 4 +- .../rmw_fastrtps_shared_cpp/participant.hpp | 6 + .../include/rmw_fastrtps_shared_cpp/qos.hpp | 130 ++++- .../rmw_security_logging.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 142 +++++ .../src/TypeSupport_impl.cpp | 7 +- .../src/create_rmw_gid.cpp | 2 +- .../src/custom_publisher_info.cpp | 16 +- .../src/custom_subscriber_info.cpp | 18 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 252 ++++++--- rmw_fastrtps_shared_cpp/src/publisher.cpp | 38 +- rmw_fastrtps_shared_cpp/src/qos.cpp | 72 ++- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 78 ++- .../src/rmw_compare_gids_equal.cpp | 2 +- .../src/rmw_get_endpoint_network_flow.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_logging.cpp | 7 +- .../src/rmw_node_names.cpp | 4 - rmw_fastrtps_shared_cpp/src/rmw_publish.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 12 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 12 +- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 12 +- .../src/rmw_security_logging.cpp | 15 +- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 77 ++- .../src/rmw_service_server_is_available.cpp | 6 +- .../src/rmw_subscription.cpp | 10 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 40 +- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 12 +- rmw_fastrtps_shared_cpp/src/subscription.cpp | 48 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 130 +++++ rmw_fastrtps_shared_cpp/test/CMakeLists.txt | 6 + .../test/test_dds_qos_to_rmw_qos.cpp | 161 ++++++ .../test/test_guid_utils.cpp | 4 +- rmw_fastrtps_shared_cpp/test/test_logging.cpp | 16 +- .../test/test_rmw_qos_to_dds_attributes.cpp | 145 ++--- .../test/test_security_logging.cpp | 5 +- 95 files changed, 3328 insertions(+), 1762 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 create mode 100644 rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 35ef66d0e..a479fb531 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/ServiceTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp index 49ec62900..7284bd4f5 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp @@ -15,10 +15,11 @@ #ifndef RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ -#include -#include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #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 5c9964a5a..1a9a52bf7 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -15,16 +15,15 @@ #ifndef RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ -#include -#include - -#include - -#include -#include #include #include +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" 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..51af2dd9b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_client.hpp @@ -15,35 +15,36 @@ #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 * -get_request_publisher(rmw_client_t * client); +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(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 * -get_response_subscriber(rmw_client_t * client); +eprosima::fastdds::dds::DataReader * +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 6b287ce98..cbd76bfbc 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_participant.hpp @@ -15,23 +15,24 @@ #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 + * This 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 * -get_participant(rmw_node_t * node); +eprosima::fastdds::dds::DomainParticipant * +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 0f8c94c98..3f09df5ed 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_publisher.hpp @@ -15,23 +15,24 @@ #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 * -get_publisher(rmw_publisher_t * publisher); +eprosima::fastdds::dds::DataWriter * +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 645da26e4..edcc2c19b 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_service.hpp @@ -15,35 +15,36 @@ #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 * -get_request_subscriber(rmw_service_t * service); +eprosima::fastdds::dds::DataReader * +get_request_datareader(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 * -get_response_publisher(rmw_service_t * service); +eprosima::fastdds::dds::DataWriter * +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 403a176f4..82449bf99 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -15,23 +15,24 @@ #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 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_CPP_PUBLIC -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription); +eprosima::fastdds::dds::DataReader * +get_datareader(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..037930f55 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -30,7 +30,6 @@ create_publisher( const rmw_publisher_options_t * publisher_options, bool keyed, 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..3413cea4c 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -32,7 +32,6 @@ create_subscription( const rmw_subscription_options_t * subscription_options, bool keyed, 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..3e29ae368 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,8 +23,8 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client) +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client) { if (!client) { return nullptr; @@ -30,11 +33,11 @@ get_request_publisher(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->request_publisher_; + return impl->request_writer_; } -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client) +eprosima::fastdds::dds::DataReader * +get_response_datareader(rmw_client_t * client) { if (!client) { return nullptr; @@ -43,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/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index 120086460..372671200 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -22,8 +22,8 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node) +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node) { if (!node) { return nullptr; @@ -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/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index bac263c6c..fae5aee70 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher) +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher) { if (!publisher) { return nullptr; @@ -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/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index a7245a83b..bfa407f5c 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service) +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service) { if (!service) { return nullptr; @@ -30,11 +30,11 @@ get_request_subscriber(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->request_subscriber_; + return impl->request_reader_; } -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service) +eprosima::fastdds::dds::DataWriter * +get_response_datawriter(rmw_service_t * service) { if (!service) { return nullptr; @@ -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/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 9eb7eae8b..08f4baed3 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_cpp { -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription) +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription) { if (!subscription) { return nullptr; @@ -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/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index f32f783ed..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->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 41df66259..a4b80fa30 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -15,9 +15,15 @@ #include -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/xmlparser/XMLProfileManager.h" +#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" @@ -36,17 +42,13 @@ #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" #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, @@ -57,13 +59,15 @@ rmw_fastrtps_cpp::create_publisher( bool keyed, bool create_publisher_listener) { + ///// + // Check input parameters 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); @@ -75,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; } } @@ -88,9 +93,15 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - Participant * participant = participant_info->participant; - RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); + 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) { @@ -111,102 +122,167 @@ rmw_fastrtps_cpp::create_publisher( } } - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; + std::lock_guard lck(participant_info->entity_creation_mutex_); - if (!is_valid_qos(*qos_policies)) { + ///// + // 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; } - // 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); + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - info = new (std::nothrow) CustomPublisherInfo(); + ///// + // Create the custom Publisher struct (info) + auto 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]() { + [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { - _unregister_type(participant, info->type_support_); + dds_participant->unregister_type(info->type_support_.get_type_name()); } - delete info->listener_; delete info; }); + info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = callbacks; - 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"); + ///// + // Create the Type Support struct + 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; } - _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; - } + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - 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); + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; - info->listener_ = nullptr; + ///// + // Create Listener 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; } } - info->publisher_ = Domain::createPublisher( - participant, - publisherParam, + ///// + // Create and register Topic + 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( + 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; + } + + ///// + // 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 Fast DDS 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, writer_qos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + // Get QoS from RMW + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); + return nullptr; + } + + // Creates DataWriter + info->data_writer_ = publisher->create_datawriter( + topic.topic, + writer_qos, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + + if (!info->data_writer_) { + 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->data_writer_); }); + ///// + // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->getGuid()); + eprosima_fastrtps_identifier, info->data_writer_->guid()); - rmw_publisher = rmw_publisher_allocate(); + ///// + // 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( @@ -215,20 +291,23 @@ 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); rmw_publisher->options = *publisher_options; + topic.should_be_deleted = false; cleanup_rmw_publisher.cancel(); - cleanup_publisher.cancel(); + cleanup_datawriter.cancel(); cleanup_info.cancel(); + return rmw_publisher; } diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 18f364c84..171fbdc50 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -14,6 +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" @@ -31,27 +45,23 @@ #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 * 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 RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -78,11 +88,25 @@ rmw_create_client( } } + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); + 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 * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // 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,196 +127,283 @@ rmw_create_client( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + auto service_members = static_cast(type_support->data); + auto request_members = static_cast( + service_members->request_members_->data); + auto 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &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", + 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &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", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // 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; } - 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_); - } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); - info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; - 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("create_client() failed to allocate request typesupport"); + return nullptr; + } - 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"); + request_fastdds_type.reset(tsupport); + } + if (!response_fastdds_type) { + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_client() failed to allocate response typesupport"); return nullptr; } - _register_type(participant, info->request_type_support_); + + response_fastdds_type.reset(tsupport); + } + + 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(dds_participant)) { + RMW_SET_ERROR_MSG("create_client() failed to register response type"); + return nullptr; + } + info->response_type_support_ = response_fastdds_type; + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); + return nullptr; + } + + 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; } - if ( - !Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) + ///// + // Create and register Topics + // Same default topic QoS for both topics + 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; + } + + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, false, &response_topic)) { - 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_); + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); + return nullptr; } - // 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); - - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) + response_topic_desc = response_topic.desc; + + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, true, &request_topic)) { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); + return nullptr; } + info->request_topic_ = request_topic_name; + info->response_topic_ = response_topic_name; + + // Keyword to find DataWriter and DataReader QoS + const std::string topic_name_fallback = "client"; + + ///// + // 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 + // with profile_name "client" is attempted. Else, use the default Fast DDS 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 + // 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, reader_qos); + subscriber->get_datareader_qos_from_profile(response_topic_name, reader_qos); + if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = + reader_qos.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, reader_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); + return nullptr; + } - // 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); + // Creates DataReader + info->response_reader_ = subscriber->create_datareader( + response_topic_desc, + reader_qos, + info->listener_); - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + if (!info->response_reader_) { + RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); + return nullptr; } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->response_reader_); + }); + + // 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 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 + // 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, writer_qos); + publisher->get_datawriter_qos_from_profile(request_topic_name, writer_qos); + + // 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; + writer_qos.publish_mode().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; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); + return nullptr; + } + + // Creates DataWriter + info->request_writer_ = publisher->create_datawriter( + request_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->request_writer_) { + RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); + 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_writer_); + }); + + ///// + // Create client + // Debug info RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", "************ Client Details *********"); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Sub Topic %s", subscriberParam.topic.topicName.c_str()); + "Sub Topic %s", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "***********"); - // Create Client Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - 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"); - 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"); - return nullptr; - } - - // Create Client Publisher and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - 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_; - } - }); - 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; - } - info->request_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); - if (!info->request_publisher_) { - RMW_SET_ERROR_MSG("failed to create client request publisher"); - return nullptr; - } - - info->writer_guid_ = info->request_publisher_->getGuid(); - info->reader_guid_ = info->response_subscriber_->getGuid(); + 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) { - 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( @@ -306,7 +417,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); @@ -315,14 +426,15 @@ 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_writer_->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_reader_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_reader( response_subscriber_gid, @@ -349,10 +461,12 @@ rmw_create_client( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_client.cancel(); - cleanup_response_subscriber.cancel(); - cleanup_request_publisher.cancel(); - cleanup_base_info.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_info.cancel(); return rmw_client; } 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_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index e36e791ee..2345b71a6 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 * @@ -64,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, @@ -90,11 +93,25 @@ rmw_create_service( } } - const CustomParticipantInfo * impl = - static_cast(node->context->impl->participant_info); + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); + 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 * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + ///// + // 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,190 +132,278 @@ rmw_create_service( } } + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + auto service_members = static_cast(type_support->data); + auto request_members = static_cast( + service_members->request_members_->data); + auto 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &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", + 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &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", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // 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_base_info = rcpputils::make_scope_exit( - [info, participant]() { - if (info->request_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); - } + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { - rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); } delete info; }); - info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; - 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 ( - !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"); + if (!request_fastdds_type) { + auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate request typesupport"); return nullptr; } - _register_type(participant, 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"); + request_fastdds_type.reset(tsupport); + } + if (!response_fastdds_type) { + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate response typesupport"); return nullptr; } - _register_type(participant, 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); - - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + response_fastdds_type.reset(tsupport); } - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + 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; } - 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); + info->request_type_support_ = request_fastdds_type; + + 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; } + info->response_type_support_ = response_fastdds_type; - 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; - } + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); + return nullptr; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = response_type_name; - publisherParam.topic.topicName = pub_topic_name; + 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; + } - 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", "***********"); + ///// + // Create and register Topics + // Same default topic QoS for both topics + 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; + } - // Create Service Subscriber and set QoS - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_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; } - 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"); + + request_topic_desc = request_topic.desc; + + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_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; } - info->request_subscriber_ = - Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->request_subscriber_) { - RMW_SET_ERROR_MSG("failed to create service request subscriber"); + + // Keyword to find DataWrtier and DataReader QoS + const std::string topic_name_fallback = "service"; + + ///// + // Create request 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 + // with profile_name "service" is attempted. Else, use the default Fast DDS 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 + // 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, reader_qos); + subscriber->get_datareader_qos_from_profile(request_topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } - // Create Service Publisher and set QoS - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + // Creates DataReader + info->request_reader_ = subscriber->create_datareader( + request_topic_desc, + reader_qos, + info->listener_); + + if (!info->request_reader_) { + RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); 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_; - } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->request_reader_); }); - 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 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 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 + // 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, 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) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } - info->response_publisher_ = - Domain::createPublisher(participant, publisherParam, info->pub_listener_); - if (!info->response_publisher_) { - RMW_SET_ERROR_MSG("failed to create service response publisher"); + + // Creates DataWriter + info->response_writer_ = publisher->create_datawriter( + response_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->response_writer_) { + RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); return nullptr; } + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->response_writer_); + }); + + ///// + // Create Service + + // Debug info + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "************ Service Details *********"); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Sub Topic %s", request_topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_cpp", + "Pub Topic %s", response_topic_name.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"); + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for rmw_service"); return nullptr; } auto cleanup_rmw_service = rcpputils::make_scope_exit( @@ -312,7 +417,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); @@ -321,14 +426,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_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_->getGuid()); + eprosima_fastrtps_identifier, info->response_writer_->guid()); rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.associate_writer( response_publisher_gid, @@ -355,10 +460,12 @@ rmw_create_service( } } + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; cleanup_rmw_service.cancel(); - cleanup_request_subscriber.cancel(); - cleanup_response_publisher.cancel(); - cleanup_base_info.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + cleanup_info.cancel(); return rmw_service; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 30cbf6fb0..4affb8ec1 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -15,6 +15,16 @@ #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/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" @@ -33,21 +43,14 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #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_shared_cpp/utils.hpp" #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 PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; namespace rmw_fastrtps_cpp { @@ -62,13 +65,15 @@ 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); 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); @@ -80,14 +85,22 @@ 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; } } 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); + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + 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) { @@ -107,70 +120,136 @@ create_subscription( return nullptr; } } - 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 + 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; } - // 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); + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); + ///// + // Create the custom Subscriber struct (info) + auto 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; } + auto cleanup_info = rcpputils::make_scope_exit( - [info, participant]() { + [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { - _unregister_type(participant, info->type_support_); + dds_participant->unregister_type(info->type_support_.get_type_name()); } - delete info->listener_; delete info; }); + info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = callbacks; - 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"); + ///// + // Create the Type Support struct + 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; } - _register_type(participant, info->type_support_); - } - if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - 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); + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } + info->type_support_ = fastdds_type; + ///// + // Create Listener 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"); + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); return nullptr; } } - eprosima::fastrtps::SubscriberAttributes originalParam = subscriberParam; + ///// + // Create and register Topic + 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( + 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; + } + + des_topic = topic.desc; + + ///// + // 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 Fast DDS 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, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + 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: @@ -183,46 +262,50 @@ create_subscription( // Ensure we request unique network flow endpoints if (nullptr == PropertyPolicyHelper::find_property( - subscriberParam.properties, + reader_qos.properties(), "fastdds.unique_network_flows")) { - subscriberParam.properties.properties().emplace_back("fastdds.unique_network_flows", ""); + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); } break; } - info->subscriber_ = Domain::createSubscriber( - participant, - subscriberParam, + // Creates DataReader (with subscriber name to not change name policy) + info->data_reader_ = subscriber->create_datareader( + des_topic, + reader_qos, info->listener_); - if (!info->subscriber_ && + if (!info->data_reader_ && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { - info->subscriber_ = Domain::createSubscriber( - participant, - originalParam, + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, info->listener_); } - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + + if (!info->data_reader_) { + RMW_SET_ERROR_MSG("create_subscription() 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->data_reader_); }); + ///// + // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->data_reader_->guid()); + ///// + // 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( @@ -236,14 +319,16 @@ 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; rmw_subscription->can_loan_messages = false; + topic.should_be_deleted = false; cleanup_rmw_subscription.cancel(); - cleanup_subscription.cancel(); + cleanup_datareader.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..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 "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" - #include "rmw/error_handling.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" @@ -61,12 +58,4 @@ _create_type_name( return ss.str(); } -inline void -_register_type( - eprosima::fastrtps::Participant * participant, - rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) -{ - 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..54dc13ac8 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 "gtest/gtest.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" @@ -68,19 +68,19 @@ class TestNativeEntities : public ::testing::Test rmw_node_t * node{nullptr}; }; -TEST_F(TestNativeEntities, get_participant) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_participant(nullptr)); +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_participant(node)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_domain_participant(node)); node->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_participant(node)); + EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_domain_participant(node)); } -TEST_F(TestNativeEntities, get_publisher) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(nullptr)); +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); @@ -92,17 +92,17 @@ TEST_F(TestNativeEntities, get_publisher) { const char * implementation_identifier = pub->implementation_identifier; pub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datawriter(pub)); pub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_publisher(pub)); + 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; } -TEST_F(TestNativeEntities, get_subscriber) { - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(nullptr)); +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); @@ -115,18 +115,18 @@ TEST_F(TestNativeEntities, get_subscriber) { const char * implementation_identifier = sub->implementation_identifier; sub->implementation_identifier = "not-an-rmw-implementation-identifier"; - EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); + EXPECT_EQ(nullptr, rmw_fastrtps_cpp::get_datareader(sub)); sub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_subscriber(sub)); + 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; } 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)); + 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); @@ -137,20 +137,20 @@ TEST_F(TestNativeEntities, get_service) { 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)); + 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_subscriber(srv)); - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_publisher(srv)); + 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; } 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)); + 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); @@ -161,12 +161,12 @@ TEST_F(TestNativeEntities, get_client) { 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)); + 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_publisher(client)); - EXPECT_NE(nullptr, rmw_fastrtps_cpp::get_response_subscriber(client)); + 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_cpp/test/test_logging.cpp b/rmw_fastrtps_cpp/test/test_logging.cpp index cd1068094..7bf706d54 100644 --- a/rmw_fastrtps_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_cpp/test/test_logging.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#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,19 +23,21 @@ TEST(TestLogging, rmw_logging) { 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(eprosima::fastdds::dds::Log::Kind::Info, eprosima::fastdds::dds::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(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::fastrtps::Log::Kind::Warning, eprosima::fastrtps::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::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(eprosima::fastdds::dds::Log::Kind::Error, eprosima::fastdds::dds::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(eprosima::fastdds::dds::Log::Kind::Error, eprosima::fastdds::dds::Log::GetVerbosity()); } TEST(TestLogging, rmw_logging_bad_verbosity) diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index f91079b4d..2d747fd37 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/MessageTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp index b63518852..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 -#include - #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 980888915..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 -#include - #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 607bf86ef..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,10 +15,11 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ -#include -#include #include +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #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 61237fdb9..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 -#include #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 0c09766fd..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,16 +15,15 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ -#include -#include - -#include - -#include -#include #include #include +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + #include "rcutils/logging_macros.h" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" 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..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,14 +15,14 @@ #ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ #define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ -#include -#include -#include - #include #include #include +#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/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp index 9bf08a1f7..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 @@ -15,35 +15,36 @@ #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 * -get_request_publisher(rmw_client_t * client); +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(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 * -get_response_subscriber(rmw_client_t * client); +eprosima::fastdds::dds::DataReader * +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 d32e07fe1..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 @@ -15,23 +15,24 @@ #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 * -get_participant(rmw_node_t * node); +eprosima::fastdds::dds::DomainParticipant * +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 727bb4846..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 @@ -15,23 +15,24 @@ #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 * -get_publisher(rmw_publisher_t * publisher); +eprosima::fastdds::dds::DataWriter * +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 6d9e3d74a..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 @@ -15,35 +15,36 @@ #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 * -get_request_subscriber(rmw_service_t * service); +eprosima::fastdds::dds::DataReader * +get_request_datareader(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 * -get_response_publisher(rmw_service_t * service); +eprosima::fastdds::dds::DataWriter * +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 f6614e7bf..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 @@ -15,23 +15,24 @@ #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" 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::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription); +eprosima::fastdds::dds::DataReader * +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 017fa3a94..69a7af466 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * -get_request_publisher(rmw_client_t * client) +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client) { if (!client) { return nullptr; @@ -30,11 +30,11 @@ get_request_publisher(rmw_client_t * client) return nullptr; } auto impl = static_cast(client->data); - return impl->request_publisher_; + return impl->request_writer_; } -eprosima::fastrtps::Subscriber * -get_response_subscriber(rmw_client_t * client) +eprosima::fastdds::dds::DataReader * +get_response_datareader(rmw_client_t * client) { if (!client) { return nullptr; @@ -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/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index a1c2107de..23a2c56b2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -14,6 +14,8 @@ #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" @@ -21,8 +23,8 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Participant * -get_participant(rmw_node_t * node) +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node) { if (!node) { return nullptr; @@ -31,7 +33,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..2b5b5dadd 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Publisher * -get_publisher(rmw_publisher_t * publisher) +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher) { if (!publisher) { return nullptr; @@ -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/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp index 4eb691bc5..b93ea26df 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * -get_request_subscriber(rmw_service_t * service) +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service) { if (!service) { return nullptr; @@ -30,11 +30,11 @@ get_request_subscriber(rmw_service_t * service) return nullptr; } auto impl = static_cast(service->data); - return impl->request_subscriber_; + return impl->request_reader_; } -eprosima::fastrtps::Publisher * -get_response_publisher(rmw_service_t * service) +eprosima::fastdds::dds::DataWriter * +get_response_datawriter(rmw_service_t * service) { if (!service) { return nullptr; @@ -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/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp index 001249f2e..c3c51fa58 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -20,8 +20,8 @@ namespace rmw_fastrtps_dynamic_cpp { -eprosima::fastrtps::Subscriber * -get_subscriber(rmw_subscription_t * subscription) +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription) { if (!subscription) { return nullptr; @@ -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/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/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 899b05a9e..e5e51dba7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -15,7 +15,16 @@ #include -#include "fastrtps/xmlparser/XMLProfileManager.h" +#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" @@ -32,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_dynamic_cpp/identifier.hpp" #include "rmw_fastrtps_dynamic_cpp/publisher.hpp" @@ -39,12 +49,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,14 +61,15 @@ rmw_fastrtps_dynamic_cpp::create_publisher( bool keyed, bool create_publisher_listener) { - (void)keyed; - (void)create_publisher_listener; + ///// + // Check input parameters + 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); @@ -75,7 +81,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; } } @@ -88,9 +95,15 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - Participant * participant = participant_info->participant; - RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); + return 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) { @@ -111,38 +124,60 @@ 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; } - // 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); - - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; - info = new (std::nothrow) CustomPublisherInfo(); + ///// + // Create the custom Publisher struct (info) + auto 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]() { + [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { - _unregister_type(participant, info->type_support_); + dds_participant->unregister_type(info->type_support_.get_type_name()); } - delete info->listener_; 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) { - 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( @@ -153,76 +188,116 @@ rmw_fastrtps_dynamic_cpp::create_publisher( info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_impl; - 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"); + 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_type(participant, info->type_support_); + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - 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 (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(dds_participant)) { + RMW_SET_ERROR_MSG("create_publisher() failed to register type"); + return nullptr; } - 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); + info->type_support_ = fastdds_type; - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; + ///// + // Create Listener + if (create_publisher_listener) { + info->listener_ = new (std::nothrow) PubListener(info); - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + return nullptr; + } + } - if (!get_datawriter_qos(*qos_policies, publisherParam)) { + ///// + // Create and register Topic + 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; } - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_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; + } + + ///// + // 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 Fast DDS 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, writer_qos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + // Get QoS from RMW + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } - info->publisher_ = Domain::createPublisher( - participant, - publisherParam, + // Creates DataWriter (with publisher name to not change name policy) + info->data_writer_ = publisher->create_datawriter( + topic.topic, + writer_qos, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + + if (!info->data_writer_) { + 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->data_writer_); }); + ///// + // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->publisher_->getGuid()); + eprosima_fastrtps_identifier, info->data_writer_->guid()); - rmw_publisher = rmw_publisher_allocate(); + ///// + // 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( @@ -237,16 +312,17 @@ 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); rmw_publisher->options = *publisher_options; + topic.should_be_deleted = false; cleanup_rmw_publisher.cancel(); - cleanup_publisher.cancel(); - cleanup_info.cancel(); + cleanup_datawriter.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..696dab61b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -14,6 +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/logging_macros.h" #include "rmw/allocators.h" @@ -33,6 +47,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 +55,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 +63,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 +91,25 @@ rmw_create_client( } } + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); + 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"); - return nullptr; - } - Participant * participant = participant_info->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + 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_; + ///// + // 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,270 +130,373 @@ 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; + 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &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", + 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &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", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // Create the custom Client struct (info) + CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_client() failed to allocate custom info"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); + } + delete info; + }); - info = new CustomClientInfo(); - info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; + ///// + // 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) { - delete info; - 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( + [&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"); + RMW_SET_ERROR_MSG("create_client() 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; - 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 (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_client() failed to allocate request TypeSupportProxy"); + 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_); + request_fastdds_type.reset(tsupport); + } - 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; + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_client() failed to allocate response TypeSupportProxy"); + return nullptr; } - _register_type(participant, info->request_type_support_); + + response_fastdds_type.reset(tsupport); + } + + 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(dds_participant)) { + RMW_SET_ERROR_MSG("create_client() failed to register response type"); + return nullptr; + } + info->response_type_support_ = response_fastdds_type; + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); + return nullptr; } - if (!Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) + 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; + } + + ///// + // Create and register Topics + // Same default topic QoS for both topics + 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; + } + + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, false, &response_topic)) { - 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_); + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); + return nullptr; } - // 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); + response_topic_desc = response_topic.desc; - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, true, &request_topic)) { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); + return nullptr; } + info->request_topic_ = request_topic_name; + info->response_topic_ = response_topic_name; + + // Keyword to find DataWrtier and DataReader QoS + const std::string topic_name_fallback = "client"; + + ///// + // 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 + // with profile_name "client" is attempted. Else, use the default Fast DDS 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 + // 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, reader_qos); + subscriber->get_datareader_qos_from_profile(response_topic_name, reader_qos); + if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = + reader_qos.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, reader_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); + return nullptr; + } - // 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); + // Creates DataReader + info->response_reader_ = subscriber->create_datareader( + response_topic_desc, + reader_qos, + info->listener_); - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + if (!info->response_reader_) { + RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); + return nullptr; } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->response_reader_); + }); + + // 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 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 + // 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, writer_qos); + publisher->get_datawriter_qos_from_profile(request_topic_name, writer_qos); + + // 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; + writer_qos.publish_mode().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; + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = request_type_name; - publisherParam.topic.topicName = pub_topic_name; + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); + return nullptr; + } + + // Creates DataWriter + info->request_writer_ = publisher->create_datawriter( + request_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->request_writer_) { + RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); + return nullptr; + } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->request_writer_); + }); + + ///// + // 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", response_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", request_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_writer_->guid(); + info->reader_guid_ = info->response_reader_->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; + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for rmw_client"); + 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; 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"); - goto fail; + 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); { // 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()); + 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_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + + 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, 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, 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( + response_subscriber_gid, 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( - gid, + common_context->graph_cache.dissociate_writer( + request_publisher_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; + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; + cleanup_rmw_client.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_client; } rmw_ret_t @@ -405,12 +520,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_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/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 85ecddbc0..bb0f7b567 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,25 @@ 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 RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); return nullptr; } - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - 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); + + 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_; + ///// + // 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,256 +138,365 @@ 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; + 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 = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + const void * 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &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", + 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_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &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", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // 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"); + return nullptr; + } + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); + } + delete info; + }); - info = new CustomServiceInfo(); - info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; + ///// + // 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) { - 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( + [&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"); + RMW_SET_ERROR_MSG("create_service() failed to get 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; - 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 (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate request TypeSupportProxy"); + 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_); + request_fastdds_type.reset(tsupport); + } - 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; + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate response TypeSupportProxy"); + return nullptr; } - _register_type(participant, info->request_type_support_); + + response_fastdds_type.reset(tsupport); } - if (!Domain::getRegisteredType( - participant, response_type_name.c_str(), - reinterpret_cast(&info->response_type_support_))) + 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(dds_participant)) { + RMW_SET_ERROR_MSG("create_service() failed to register response type"); + return nullptr; + } + info->response_type_support_ = response_fastdds_type; + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); + return nullptr; + } + + 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; + } + + ///// + // Create and register Topics + // Same default topic QoS for both topics + 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; + } + + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, false, &request_topic)) { - 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_); + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); + return nullptr; } - // 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); + request_topic_desc = request_topic.desc; - if (XMLProfileManager::fillSubscriberAttributes( - sub_topic_name.to_string(), subscriberParam, false) != XMLP_ret::XML_OK) + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, true, &response_topic)) { - XMLProfileManager::fillSubscriberAttributes(topic_name_fallback, subscriberParam, false); + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; } - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = + // Keyword to find DataWrtier and DataReader QoS + const std::string topic_name_fallback = "service"; + + ///// + // Create request 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 + // with profile_name "service" is attempted. Else, use the default Fast DDS 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 + // 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, reader_qos); + subscriber->get_datareader_qos_from_profile(request_topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - 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. - pub_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply"); - Domain::getDefaultPublisherAttributes(publisherParam); - - if (XMLProfileManager::fillPublisherAttributes( - pub_topic_name.to_string(), publisherParam, false) != XMLP_ret::XML_OK) - { - XMLProfileManager::fillPublisherAttributes(topic_name_fallback, publisherParam, false); + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); + 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; + // Creates DataReader + info->request_reader_ = subscriber->create_datareader( + request_topic_desc, + reader_qos, + info->listener_); + + if (!info->request_reader_) { + RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->request_reader_); + }); + + + ///// + // Create response DataWriter + + // 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 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 + // 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, 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) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = response_type_name; - publisherParam.topic.topicName = pub_topic_name; + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); + return nullptr; + } + + // Creates DataWriter + info->response_writer_ = publisher->create_datawriter( + response_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->response_writer_) { + RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); + return nullptr; + } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->response_writer_); + }); + + ///// + // 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", request_topic_name.c_str()); RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_dynamic_cpp", - "Pub Topic %s", publisherParam.topic.topicName.c_str()); + "Pub Topic %s", response_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; + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for rmw_service"); + 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; + 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); { // 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()); + 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_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + + 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, 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, + response_publisher_gid, 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, + request_subscriber_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_); + return nullptr; } - - type_registry.return_request_type_support(type_support); - type_registry.return_response_type_support(type_support); - delete info; } - 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; + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; + cleanup_rmw_service.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_service; } rmw_ret_t @@ -402,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 41091471e..86301ff17 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -15,6 +15,16 @@ #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/TopicDescription.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcutils/error_handling.h" #include "rmw/allocators.h" @@ -30,6 +40,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 +52,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 PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; -using XMLProfileManager = eprosima::fastrtps::xmlparser::XMLProfileManager; namespace rmw_fastrtps_dynamic_cpp { @@ -62,11 +67,15 @@ 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); 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); @@ -78,17 +87,22 @@ 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; } } RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - (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 RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return 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) { @@ -109,79 +123,147 @@ create_subscription( } } - 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; + 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 with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); 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); + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); + ///// + // Create the custom Subscriber struct (info) + auto 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; } + 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); - } + [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { - _unregister_type(participant, info->type_support_); + dds_participant->unregister_type(info->type_support_.get_type_name()); } - delete info->listener_; 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"); + RMW_SET_ERROR_MSG("create_subscription() failed to get message_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; - 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"); + 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; } - _register_type(participant, info->type_support_); + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); } - if (!participant_info->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; } - 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 (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + if (create_subscription_listener) { + info->listener_ = new (std::nothrow) SubListener(info); - if (!get_datareader_qos(*qos_policies, subscriberParam)) { + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + } + + ///// + // Create and register Topic + 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; } - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_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; } - eprosima::fastrtps::SubscriberAttributes originalParam = subscriberParam; + des_topic = topic.desc; + + ///// + // 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 Fast DDS 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, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + 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: @@ -194,45 +276,48 @@ create_subscription( // Ensure we request unique network flow endpoints if (nullptr == PropertyPolicyHelper::find_property( - subscriberParam.properties, + reader_qos.properties(), "fastdds.unique_network_flows")) { - subscriberParam.properties.properties().emplace_back("fastdds.unique_network_flows", ""); + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); } break; } - info->subscriber_ = Domain::createSubscriber( - participant, - subscriberParam, + // Creates DataReader (with subscriber name to not change name policy) + info->data_reader_ = subscriber->create_datareader( + des_topic, + reader_qos, info->listener_); - if (!info->subscriber_ && + if (!info->data_reader_ && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { - info->subscriber_ = Domain::createSubscriber( - participant, - originalParam, + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, info->listener_); } - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + + if (!info->data_reader_) { + RMW_SET_ERROR_MSG("create_subscription() 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->data_reader_); }); + + ///// + // Create RMW GID info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + eprosima_fastrtps_identifier, info->data_reader_->guid()); 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( @@ -246,7 +331,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); @@ -254,8 +340,10 @@ create_subscription( rmw_subscription->options = *subscription_options; rmw_subscription->can_loan_messages = false; + topic.should_be_deleted = false; cleanup_rmw_subscription.cancel(); - cleanup_subscription.cancel(); + cleanup_datareader.cancel(); + return_type_support.cancel(); cleanup_info.cancel(); return rmw_subscription; } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index fc5e3a23b..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 a396d68f3..1846b7ba5 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,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_get_native_entities.cpp b/rmw_fastrtps_dynamic_cpp/test/test_get_native_entities.cpp index 19341f2bf..89ac01a9e 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 "gtest/gtest.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" @@ -68,19 +68,19 @@ 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)); +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_participant(node)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); node->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_participant(node)); + 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)); +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); @@ -92,17 +92,17 @@ TEST_F(TestNativeEntities, get_publisher) { 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)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(pub)); pub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_publisher(pub)); + 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; } -TEST_F(TestNativeEntities, get_subscriber) { - EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(nullptr)); +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); @@ -115,18 +115,18 @@ TEST_F(TestNativeEntities, get_subscriber) { 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)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(sub)); sub->implementation_identifier = implementation_identifier; - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_subscriber(sub)); + 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; } 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)); + 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); @@ -137,20 +137,20 @@ TEST_F(TestNativeEntities, get_service) { 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)); + 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_subscriber(srv)); - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_publisher(srv)); + 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; } 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)); + 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); @@ -161,12 +161,12 @@ TEST_F(TestNativeEntities, get_client) { 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)); + 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_publisher(client)); - EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_subscriber(client)); + 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; diff --git a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp index cd1068094..f0ed4037e 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp @@ -12,30 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#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 5bf283fc9..d74142afa 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) @@ -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 @@ -86,7 +87,7 @@ 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 + src/utils.cpp ) target_include_directories(rmw_fastrtps_shared_cpp PUBLIC 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 4515ba465..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 @@ -15,14 +15,17 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ #define RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ -#include -#include - -#include -#include #include #include +#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" + #include "rcutils/logging_macros.h" #include "./visibility_control.h" @@ -38,7 +41,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,16 +87,6 @@ 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; - } -} - } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ 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..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 @@ -22,15 +22,22 @@ #include #include #include +#include #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/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/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" @@ -41,16 +48,20 @@ 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::fastrtps::Subscriber * response_subscriber_{nullptr}; - eprosima::fastrtps::Publisher * 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_; + ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; - eprosima::fastrtps::Participant * participant_{nullptr}; + const char * typesupport_identifier_{nullptr}; ClientPubListener * pub_listener_{nullptr}; std::atomic_size_t response_subscriber_matched_count_; @@ -61,10 +72,10 @@ 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 +class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: explicit ClientListener(CustomClientInfo * info) @@ -73,9 +84,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 @@ -85,8 +96,8 @@ 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 (response.sample_info_.valid_data) { response.sample_identity_ = response.sample_info_.related_sample_identity; if (response.sample_identity_.writer_guid() == info_->reader_guid_ || @@ -146,18 +157,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; } @@ -185,7 +195,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) @@ -193,18 +203,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_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..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 @@ -20,10 +20,14 @@ #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/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" @@ -51,11 +55,17 @@ enum class publishing_mode_t typedef struct CustomParticipantInfo { - eprosima::fastrtps::Participant * participant; - ::ParticipantListener * listener; + eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; + ParticipantListener * listener_{nullptr}; + + eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + + // 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. @@ -63,7 +73,7 @@ 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( @@ -73,8 +83,8 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener 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 +117,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 +128,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 +147,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); + 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_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 5d626a2ee..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 @@ -16,17 +16,24 @@ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ #include -#include #include +#include #include -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/publisher/PublisherListener.h" +#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" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" @@ -36,9 +43,9 @@ typedef struct CustomPublisherInfo : public CustomEventInfo { virtual ~CustomPublisherInfo() = default; - eprosima::fastrtps::Publisher * 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}; @@ -48,7 +55,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) @@ -60,32 +67,32 @@ class PubListener : public EventListenerInterface, public eprosima::fastrtps::Pu (void) info; } - // PublisherListener implementation + // DataWriterListener 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); - } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) { - subscriptions_.erase(info.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)); } } 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 @@ -127,11 +134,11 @@ class PubListener : public EventListenerInterface, public eprosima::fastrtps::Pu 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 2b30ab929..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 @@ -24,17 +24,23 @@ #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/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/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" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" class ServiceListener; class ServicePubListener; @@ -49,15 +55,16 @@ 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::fastrtps::Subscriber * request_subscriber_{nullptr}; - eprosima::fastrtps::Publisher * response_publisher_{nullptr}; + eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; + ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; - eprosima::fastrtps::Participant * participant_{nullptr}; + const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; @@ -65,13 +72,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; + explicit 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; @@ -166,31 +177,30 @@ 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) : info_(info), list_has_data_(false), conditionMutex_(nullptr), conditionVariable_(nullptr) { - (void)info_; } 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); + 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(); @@ -199,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 (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/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..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 @@ -21,14 +21,20 @@ #include #include -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" +#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" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" @@ -38,9 +44,9 @@ struct CustomSubscriberInfo : public CustomEventInfo { virtual ~CustomSubscriberInfo() = default; - eprosima::fastrtps::Subscriber * 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}; @@ -50,11 +56,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), @@ -64,38 +70,39 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su (void)info; } - // SubscriberListener implementation + // DataReaderListener 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_has_data(reader); } void - onNewDataMessage(eprosima::fastrtps::Subscriber * sub) final + on_data_available(eprosima::fastdds::dds::DataReader * reader) final { - update_unread_count(sub); + update_has_data(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 @@ -127,23 +134,20 @@ 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_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. -#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 + // 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_); - data_.store(unread_count, std::memory_order_relaxed); + data_.store(has_data, std::memory_order_relaxed); } size_t publisherCount() @@ -155,14 +159,14 @@ 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_ + 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/guid_utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp index 21cd07fee..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 @@ -20,7 +20,7 @@ #include #include -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" namespace rmw_fastrtps_shared_cpp { @@ -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/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index f10303cf8..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,6 +25,12 @@ namespace rmw_fastrtps_shared_cpp { +// 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 0cdbca24f..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 @@ -16,25 +16,17 @@ #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 +35,29 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr); + eprosima::fastdds::dds::DataReaderQos & reader_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::PublisherAttributes & pattr); + 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 & topic_qos); 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 - * by this function. + * Converts the DDS QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. * - * \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 dds_qos as a rmw_qos_profile_t */ template void @@ -69,7 +65,76 @@ 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::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (dds_qos.durability().kind) { + case eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline = dds_duration_to_rmw(dds_qos.deadline().period); + qos->lifespan = dds_duration_to_rmw(dds_qos.lifespan().duration); + + switch (dds_qos.liveliness().kind) { + case eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastdds::dds::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.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); +} + +/* + * 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 RTPSQoSPolicyT & rtps_qos, + rmw_qos_profile_t * qos) +{ + switch (rtps_qos.m_reliability.kind) { case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; @@ -81,7 +146,7 @@ dds_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; @@ -93,10 +158,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(rtps_qos.m_deadline.period); + qos->lifespan = dds_duration_to_rmw(rtps_qos.m_lifespan.duration); - 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; @@ -107,9 +172,20 @@ 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(rtps_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( 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..8b6910e64 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -0,0 +1,142 @@ +// 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. + +#ifndef RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ + +#include +#include + +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#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" + + +namespace rmw_fastrtps_shared_cpp +{ + +/** +* 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() + { + 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; +}; + +/** +* Convert a Fast DDS return code into the corresponding rmw_ret_t +* \param[in] 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 code); + +/** +* Auxiliary method to reuse or create a topic during the creation of an entity. +* +* \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) +* \return false when the topic could not be created +*/ +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); + +/** +* Tries to find already registered topic and type. +* +* \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 +* if they were not previously registered on the participant. +*/ +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); + +/** +* Performs removal of associated topic and type. +* +* \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 +remove_topic_and_type( + const CustomParticipantInfo * participant_info, + const eprosima::fastdds::dds::TopicDescription * topic, + const eprosima::fastdds::dds::TypeSupport & type); + +} // 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 c860e8ea2..1baf926fb 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include #include +#include "fastdds/rtps/common/SerializedPayload.h" + +#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/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 f38f653dd..840d2e830 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 * @@ -23,8 +25,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,8 +43,8 @@ PubListener::on_offered_deadline_missed( } void PubListener::on_liveliness_lost( - eprosima::fastrtps::Publisher * /* publisher */, - const eprosima::fastrtps::LivelinessLostStatus & status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::LivelinessLostStatus & status) { std::lock_guard lock(internalMutex_); @@ -79,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; @@ -89,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 172c20ab1..17da1fa79 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 * @@ -23,8 +27,8 @@ CustomSubscriberInfo::getListener() const void SubListener::on_requested_deadline_missed( - eprosima::fastrtps::Subscriber * /* subscriber */, - const eprosima::fastrtps::RequestedDeadlineMissedStatus & status) + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -41,8 +45,8 @@ SubListener::on_requested_deadline_missed( } void SubListener::on_liveliness_changed( - eprosima::fastrtps::Subscriber * /* subscriber */, - const eprosima::fastrtps::LivelinessChangedStatus & status) + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::LivelinessChangedStatus & status) { std::lock_guard lock(internalMutex_); @@ -81,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; @@ -94,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/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 608ac658e..90ebed03d 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -13,24 +13,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #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/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" +#include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" +#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" -#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 "rcpputils/scope_exit.hpp" #include "rcutils/filesystem.h" #include "rcutils/get_env.h" @@ -40,18 +41,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" - -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; -#endif +#include "rmw_fastrtps_shared_cpp/utils.hpp" #if HAVE_SECURITY static @@ -90,53 +80,98 @@ get_security_file_paths( } #endif -static -CustomParticipantInfo * +// Private function to create Participant with QoS +static CustomParticipantInfo * __create_participant( const char * identifier, - const ParticipantAttributes & participantAttrs, + const eprosima::fastdds::dds::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 { - listener = new ::ParticipantListener( - identifier, common_context); + participant_info = new CustomParticipantInfo(); } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate participant listener"); - goto fail; - } - - participant = Domain::createParticipant(participantAttrs, listener); - if (!participant) { - RMW_SET_ERROR_MSG("create_node() could not create participant"); + 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( + [participant_info]() { + 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; + }); + ///// + // Create Participant listener try { - participant_info = new CustomParticipantInfo(); + participant_info->listener_ = new ParticipantListener( + identifier, common_context); } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate node impl struct"); - goto fail; + RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); + return nullptr; + } + + ///// + // 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. + eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); + + participant_info->participant_ = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( + static_cast(domain_id), domainParticipantQos, + participant_info->listener_, participant_mask); + + if (!participant_info->participant_) { + RMW_SET_ERROR_MSG("__create_participant failed to create participant"); + return nullptr; } + + ///// + // Set participant info parameters participant_info->leave_middleware_default_qos = leave_middleware_default_qos; participant_info->publishing_mode = publishing_mode; - participant_info->participant = participant; - participant_info->listener = listener; + ///// + // Create Publisher + eprosima::fastdds::dds::PublisherQos publisherQos = + participant_info->participant_->get_default_publisher_qos(); + publisherQos.entity_factory(domainParticipantQos.entity_factory()); - return participant_info; -fail: - rmw_free(listener); - if (participant) { - Domain::removeParticipant(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; + } + + ///// + // Create Subscriber + 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); + if (!participant_info->subscriber_) { + RMW_SET_ERROR_MSG("__create_participant could not create subscriber"); + return nullptr; } - return nullptr; + + cleanup_participant_info.cancel(); + + return participant_info; } CustomParticipantInfo * @@ -154,44 +189,38 @@ rmw_fastrtps_shared_cpp::create_participant( RMW_SET_ERROR_MSG("security_options is null"); return nullptr; } - ParticipantAttributes participantAttrs; // Load default XML profile. - Domain::getDefaultParticipantAttributes(participantAttrs); + 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(shm_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; @@ -227,9 +256,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) { @@ -271,38 +300,93 @@ 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; } #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 } return __create_participant( identifier, - participantAttrs, + domainParticipantQos, leave_middleware_default_qos, publishing_mode, - common_context); + common_context, + domain_id); } 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; } - Domain::removeParticipant(participant_info->participant); - delete participant_info->listener; - participant_info->listener = nullptr; + + // Make the participant stop listening to discovery + participant_info->participant_->set_listener(nullptr); + + ReturnCode_t ret = ReturnCode_t::RETCODE_OK; + + // Collect topics that should be deleted + std::vector topics_to_remove; + + // Remove datawriters and publisher from participant + { + 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 (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds publisher from participant"); + } + } + + // Remove datareaders and subscriber from 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); + } + ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds subscriber from 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( + participant_info->participant_); + + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete participant"); + } + + // Delete Listener + delete participant_info->listener_; + + // Delete Custom 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/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 90eb10984..b0a97c865 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" @@ -23,9 +20,7 @@ #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; +#include "rmw_fastrtps_shared_cpp/utils.hpp" namespace rmw_fastrtps_shared_cpp { @@ -38,12 +33,33 @@ destroy_publisher( assert(publisher->implementation_identifier == identifier); static_cast(identifier); - auto info = static_cast(publisher->data); - Domain::removePublisher(info->publisher_); - delete info->listener_; + { + 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 (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 + delete info->listener_; + + // Delete topic and unregister type + remove_topic_and_type(participant_info, topic, info->type_support_); - _unregister_type(participant_info->participant, info->type_support_); - delete info; + // Delete CustomPublisherInfo structure + delete info; + } rmw_free(const_cast(publisher->topic_name)); rmw_publisher_free(publisher); diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index c5aad2e94..129c3be8b 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -16,8 +16,10 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastdds/dds/publisher/qos/DataWriterQos.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" #include "rmw_dds_common/time_utils.hpp" @@ -53,18 +55,19 @@ dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) return result; } +// 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, - 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::fastdds::dds::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::fastdds::dds::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::fastdds::dds::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::fastdds::dds::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::fastdds::dds::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::fastdds::dds::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.history().depth >= 0); if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(history_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( "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::fastdds::dds::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::fastdds::dds::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,25 @@ 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, datawriter_qos); +} + +bool +get_topic_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::TopicQos & topic_qos) { - return fill_entity_qos_from_profile(qos_policies, pattr.qos, pattr.topic.historyQos); + return fill_entity_qos_from_profile(qos_policies, topic_qos); } bool @@ -190,7 +202,7 @@ dds_attributes_to_rmw_qos( break; } qos->depth = static_cast(dds_qos.topic.historyQos.depth); - dds_qos_to_rmw_qos(dds_qos.qos, qos); + rtps_qos_to_rmw_qos(dds_qos.qos, qos); } template @@ -202,3 +214,13 @@ 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 67f226bdc..d3fde8d9c 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 { @@ -41,43 +38,92 @@ __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 participant_info = + static_cast(node->context->impl->participant_info); auto info = static_cast(client->data); + { // 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_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_->getGuid()); + 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_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, static_cast(&msg), 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; + 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 + { + 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_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = RMW_RET_ERROR; + info->response_reader_->set_listener(nullptr); + } + + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = RMW_RET_ERROR; + info->request_writer_->set_listener(nullptr); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // 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 CustomClientInfo structure + delete info; + } 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_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_get_endpoint_network_flow.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp index f227eaf7d..73e6d3769 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_endpoint_network_flow.cpp @@ -43,7 +43,7 @@ __rmw_publisher_get_network_flow_endpoints( CustomPublisherInfo * data = static_cast(publisher->data); LocatorList_t locators; - data->publisher_->get_sending_locators(locators); + data->data_writer_->get_sending_locators(locators); if (locators.empty()) { return res; @@ -100,7 +100,7 @@ __rmw_subscription_get_network_flow_endpoints( CustomSubscriberInfo * data = static_cast(subscription->data); LocatorList_t locators; - data->subscriber_->get_listening_locators(locators); + data->data_reader_->get_listening_locators(locators); if (locators.empty()) { return res; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp index 80710b1de..db6b86a74 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp @@ -17,17 +17,18 @@ #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; 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::fastrtps::Log::SetVerbosity(log_kind); + 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_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 fb8cd2a49..8a1bd6a0d 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/qos/DataWriterQos.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" @@ -112,7 +115,7 @@ __rmw_publisher_assert_liveliness( return RMW_RET_ERROR; } - info->publisher_->assert_liveliness(); + info->data_writer_->assert_liveliness(); return RMW_RET_OK; } @@ -122,11 +125,10 @@ __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 * fastdds_dw = info->data_writer_; + const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastdds_dw->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 3d21032f4..73ce6c534 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -17,7 +17,7 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/rtps/common/WriteParams.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -58,7 +58,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; @@ -94,9 +94,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 @@ -106,8 +108,8 @@ __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(); + 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..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" @@ -55,15 +55,17 @@ __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_.sourceTimestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.receptionTimestamp.to_ns(); + request_header->source_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; @@ -133,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_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 df5b47133..1eba8692c 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 { @@ -53,44 +50,92 @@ __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 participant_info = + static_cast(node->context->impl->participant_info); auto info = static_cast(service->data); { // 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_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_->getGuid()); + 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_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, static_cast(&msg), 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; + 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 + { + 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_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datareader"); + final_ret = RMW_RET_ERROR; + info->request_reader_->set_listener(nullptr); + } + + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; + } + + // 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"); + final_ret = RMW_RET_ERROR; + info->response_writer_->set_listener(nullptr); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // 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 CustomServiceInfo structure + delete info; + } 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 ba17e788c..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_publisher_->getAttributes().topic.getTopicName().to_string(); + auto pub_topic_name = client_info->request_topic_; - auto sub_topic_name = - client_info->response_subscriber_->getAttributes().topic.getTopicName().to_string(); + auto sub_topic_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 4ce18ac5e..98f42159f 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/qos/DataReaderQos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -98,11 +99,10 @@ __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 * fastdds_dr = info->data_reader_; + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->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 c915b91d5..f5e442acc 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" @@ -28,17 +26,19 @@ #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 { + 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(); + 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); @@ -65,19 +65,21 @@ _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::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)) { - info->listener_->update_unread_count(info->subscriber_); + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + // Update hasData from listener + info->listener_->update_has_data(info->data_reader_); - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + if (sinfo.valid_data) { if (message_info) { _assign_message_info(identifier, message_info, &sinfo); } @@ -107,13 +109,13 @@ _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. // 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; } @@ -270,20 +272,22 @@ _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; - eprosima::fastrtps::SampleInfo_t sinfo; + eprosima::fastdds::dds::SampleInfo sinfo; rmw_fastrtps_shared_cpp::SerializedData data; 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)) { - info->listener_->update_unread_count(info->subscriber_); - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + // Update hasData from listener + info->listener_->update_has_data(info->data_reader_); + + 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); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index df35f97d8..4275e4ffc 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" @@ -49,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; } @@ -59,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; } @@ -116,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; @@ -131,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); } } @@ -207,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; diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index c66955350..cd0cdddcf 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,22 +39,38 @@ destroy_subscription( assert(subscription->implementation_identifier == identifier); static_cast(identifier); - 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_; + { + 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(); - Participant * participant = participant_info->participant; - _unregister_type(participant, info->type_support_); - delete info; + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->data_reader_); + 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 + delete info->listener_; + + // Delete topic and unregister type + remove_topic_and_type(participant_info, topic, info->type_support_); + + // Delete CustomSubscriberInfo structure + delete info; + } 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 diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp new file mode 100644 index 000000000..ce541c244 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -0,0 +1,130 @@ +// 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 + +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#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 +{ + +rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) +{ + // not switch because it is not an enum class + if (ReturnCode_t::RETCODE_OK == code) { + return RMW_RET_OK; + } else if (ReturnCode_t::RETCODE_ERROR == code) { + // repeats the error to avoid too many 'if' comparisons + return RMW_RET_ERROR; + } else if (ReturnCode_t::RETCODE_TIMEOUT == code) { + return RMW_RET_TIMEOUT; + } else if (ReturnCode_t::RETCODE_UNSUPPORTED == code) { + return RMW_RET_UNSUPPORTED; + } else if (ReturnCode_t::RETCODE_BAD_PARAMETER == code) { + return RMW_RET_INVALID_ARGUMENT; + } 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 { + return RMW_RET_ERROR; + } +} + +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, + const std::string & topic_name, + const std::string & type_name, + 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) { + 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) +{ + // 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); + if (nullptr != topic) { + participant_info->participant_->delete_topic(topic); + } + + if (type) { + participant_info->participant_->unregister_type(type.get_type_name()); + } +} + +} // namespace rmw_fastrtps_shared_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..88aef4e5d --- /dev/null +++ b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp @@ -0,0 +1,161 @@ +// 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 + +#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)); +} 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 4401d290b..93969fa37 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#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 b5365ff35..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,15 +17,16 @@ #include "gtest/gtest.h" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.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" #include "rmw/error_handling.h" -using eprosima::fastrtps::SubscriberAttributes; +using eprosima::fastdds::dds::DataReaderQos; static const eprosima::fastrtps::Duration_t InfiniteDuration = eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); @@ -33,33 +34,33 @@ class GetDataReaderQoSTest : public ::testing::Test { protected: rmw_qos_profile_t qos_profile_{rmw_qos_profile_default}; - SubscriberAttributes subscriber_attributes_{}; + DataReaderQos subscriber_qos_{}; }; TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -77,87 +78,87 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_EQ( - eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, - subscriber_attributes_.qos.m_reliability.kind); + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, + subscriber_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, - subscriber_attributes_.qos.m_durability.kind); + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, + subscriber_qos_.durability().kind); EXPECT_EQ( - eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, - subscriber_attributes_.qos.m_liveliness.kind); - EXPECT_EQ(0, subscriber_attributes_.qos.m_lifespan.duration.seconds); - EXPECT_EQ(500000000u, subscriber_attributes_.qos.m_lifespan.duration.nanosec); - EXPECT_EQ(0, subscriber_attributes_.qos.m_deadline.period.seconds); - EXPECT_EQ(100000000u, subscriber_attributes_.qos.m_deadline.period.nanosec); - EXPECT_EQ(10, subscriber_attributes_.qos.m_liveliness.lease_duration.seconds); - EXPECT_EQ(0u, subscriber_attributes_.qos.m_liveliness.lease_duration.nanosec); + 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); + EXPECT_EQ(0, subscriber_qos_.deadline().period.seconds); + EXPECT_EQ(100000000u, subscriber_qos_.deadline().period.nanosec); + 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, - subscriber_attributes_.topic.historyQos.kind); - EXPECT_GE(10, subscriber_attributes_.topic.historyQos.depth); + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + subscriber_qos_.history().kind); + EXPECT_GE(10, subscriber_qos_.history().depth); } TEST_F(GetDataReaderQoSTest, large_depth_conversion) { - size_t depth = subscriber_attributes_.topic.historyQos.depth + 1; + size_t depth = subscriber_qos_.history().depth + 1; qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_attributes_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, - subscriber_attributes_.topic.historyQos.kind); - EXPECT_LE(depth, static_cast(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 +176,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); + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, + publisher_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, - publisher_attributes_.qos.m_durability.kind); + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, + 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); + 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); + 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); + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + 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)); + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + 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 +229,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 +240,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); } 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