diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 977ff5e4f..fe014fd39 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -38,10 +38,8 @@ find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(tracetools REQUIRED) -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) find_package(rmw REQUIRED) find_package(rosidl_dynamic_typesupport REQUIRED) @@ -100,7 +98,7 @@ add_library(rmw_fastrtps_cpp ) target_link_libraries(rmw_fastrtps_cpp PUBLIC fastcdr - fastrtps + fastdds rmw::rmw rmw_fastrtps_shared_cpp::rmw_fastrtps_shared_cpp rosidl_runtime_c::rosidl_runtime_c @@ -153,7 +151,7 @@ if(BUILD_TESTING) ament_add_gtest(test_logging test/test_logging.cpp) target_link_libraries(test_logging - fastrtps + fastdds rmw::rmw rmw_fastrtps_cpp ) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp index 5f431bc46..7d67d69b7 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp @@ -25,7 +25,9 @@ namespace rmw_fastrtps_cpp class MessageTypeSupport : public TypeSupport { public: - explicit MessageTypeSupport(const message_type_support_callbacks_t * members); + explicit MessageTypeSupport( + const message_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports); }; } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp index 7284bd4f5..e1613a065 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp @@ -31,19 +31,23 @@ namespace rmw_fastrtps_cpp class ServiceTypeSupport : public TypeSupport { protected: - ServiceTypeSupport(); + explicit ServiceTypeSupport(const rosidl_message_type_support_t * type_supports); }; class RequestTypeSupport : public ServiceTypeSupport { public: - explicit RequestTypeSupport(const service_type_support_callbacks_t * members); + explicit RequestTypeSupport( + const service_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports); }; class ResponseTypeSupport : public ServiceTypeSupport { public: - explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); + explicit ResponseTypeSupport( + const service_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports); }; } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 6323ca35a..d0121eff0 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -42,7 +42,7 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; - TypeSupport(); + explicit TypeSupport(const rosidl_message_type_support_t * type_supports); protected: void set_members(const message_type_support_callbacks_t * members); diff --git a/rmw_fastrtps_cpp/package.xml b/rmw_fastrtps_cpp/package.xml index a65229e62..c52215320 100644 --- a/rmw_fastrtps_cpp/package.xml +++ b/rmw_fastrtps_cpp/package.xml @@ -16,13 +16,11 @@ Ricardo González ament_cmake_ros - fastrtps_cmake_module ament_cmake fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcpputils rcutils rmw @@ -37,8 +35,7 @@ tracetools fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcutils rmw_dds_common rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake b/rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake index b338e694d..c153f273d 100644 --- a/rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake +++ b/rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake @@ -14,11 +14,9 @@ # copied from rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) -list(APPEND rmw_fastrtps_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +list(APPEND rmw_fastrtps_cpp_INCLUDE_DIRS ${FAST_INCLUDE_DIR}) # specific order: dependents before dependencies -list(APPEND rmw_fastrtps_cpp_LIBRARIES fastrtps fastcdr) +list(APPEND rmw_fastrtps_cpp_LIBRARIES fastdds fastcdr) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c16db932d..4270f022a 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -23,7 +23,7 @@ #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 "fastdds/rtps/attributes/ResourceManagement.hpp" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -176,7 +176,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create the Type Support struct if (!fastdds_type) { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks, type_supports); if (!tsupport) { RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); return nullptr; @@ -186,18 +186,13 @@ rmw_fastrtps_cpp::create_publisher( fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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; - if (!rmw_fastrtps_shared_cpp::register_type_object(type_supports, type_name)) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to register type object with incompatible type %s", - type_name.c_str()); - return nullptr; - } + info->type_support_->register_type_object_representation(); ///// // Create Listener @@ -244,13 +239,13 @@ rmw_fastrtps_cpp::create_publisher( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } @@ -304,7 +299,9 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher_free(rmw_publisher); }); - rmw_publisher->can_loan_messages = info->type_support_->is_plain(); + // The type support in the RMW implementation is always XCDR1. + rmw_publisher->can_loan_messages = + info->type_support_->is_plain(eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION); rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index d94ae4859..74ca3e89f 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -26,7 +26,7 @@ #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" -#include "fastdds/rtps/resources/ResourceManagement.h" +#include "fastdds/rtps/attributes/ResourceManagement.hpp" #include "rcpputils/scope_exit.hpp" #include "rcutils/error_handling.h" @@ -213,7 +213,8 @@ rmw_create_client( info->response_type_support_impl_ = response_members; if (!request_fastdds_type) { - auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); + auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members, + type_supports->request_typesupport); if (!tsupport) { RMW_SET_ERROR_MSG("create_client() failed to allocate request typesupport"); return nullptr; @@ -222,7 +223,8 @@ rmw_create_client( request_fastdds_type.reset(tsupport); } if (!response_fastdds_type) { - auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members, + type_supports->response_typesupport); if (!tsupport) { RMW_SET_ERROR_MSG("create_client() failed to allocate response typesupport"); return nullptr; @@ -231,13 +233,13 @@ rmw_create_client( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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)) { + if (eprosima::fastdds::dds::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } @@ -309,7 +311,7 @@ rmw_create_client( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } @@ -361,13 +363,13 @@ rmw_create_client( // 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; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_cpp/src/rmw_dynamic_message_type_support.cpp b/rmw_fastrtps_cpp/src/rmw_dynamic_message_type_support.cpp index 3560eb76b..e45512787 100644 --- a/rmw_fastrtps_cpp/src/rmw_dynamic_message_type_support.cpp +++ b/rmw_fastrtps_cpp/src/rmw_dynamic_message_type_support.cpp @@ -76,13 +76,13 @@ rmw_serialization_support_init( rosidl_dynamic_typesupport_serialization_support_interface_t methods = rosidl_dynamic_typesupport_get_zero_initialized_serialization_support_interface(); - ret = rosidl_dynamic_typesupport_fastrtps_init_serialization_support_impl(allocator, &impl); + ret = rosidl_dynamic_typesupport_fastdds_init_serialization_support_impl(allocator, &impl); if (ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG_AND_APPEND_PREV_ERROR("Could not initialize serialization support impl"); goto fail; } - ret = rosidl_dynamic_typesupport_fastrtps_init_serialization_support_interface( + ret = rosidl_dynamic_typesupport_fastdds_init_serialization_support_interface( allocator, &methods); if (ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG_AND_APPEND_PREV_ERROR("could not initialize serialization support interface"); diff --git a/rmw_fastrtps_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_cpp/src/rmw_serialize.cpp index 90b3ebbb6..34ae81ab2 100644 --- a/rmw_fastrtps_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_cpp/src/rmw_serialize.cpp @@ -40,7 +40,7 @@ rmw_serialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport_cpp(callbacks); + auto tss = MessageTypeSupport_cpp(callbacks, type_support); auto data_length = tss.getEstimatedSerializedSize(ros_message, callbacks); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { @@ -79,7 +79,7 @@ rmw_deserialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport_cpp(callbacks); + auto tss = MessageTypeSupport_cpp(callbacks, type_support); eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 118b4e2f9..fbb569d1c 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -215,7 +215,8 @@ rmw_create_service( info->response_type_support_impl_ = response_members; if (!request_fastdds_type) { - auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members); + auto tsupport = new (std::nothrow) RequestTypeSupport_cpp(service_members, + type_supports->request_typesupport); if (!tsupport) { RMW_SET_ERROR_MSG("create_service() failed to allocate request typesupport"); return nullptr; @@ -224,7 +225,8 @@ rmw_create_service( request_fastdds_type.reset(tsupport); } if (!response_fastdds_type) { - auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members); + auto tsupport = new (std::nothrow) ResponseTypeSupport_cpp(service_members, + type_supports->response_typesupport); if (!tsupport) { RMW_SET_ERROR_MSG("create_service() failed to allocate response typesupport"); return nullptr; @@ -233,13 +235,13 @@ rmw_create_service( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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)) { + if (eprosima::fastdds::dds::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } @@ -308,7 +310,7 @@ rmw_create_service( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } @@ -364,13 +366,13 @@ rmw_create_service( // 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; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 2375f45cb..51ffddc76 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -25,10 +25,9 @@ #include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" -#include "fastdds/rtps/resources/ResourceManagement.h" +#include "fastdds/rtps/attributes/ResourceManagement.hpp" -#include "fastrtps/types/DynamicType.h" -#include "fastrtps/types/DynamicTypePtr.h" +#include "fastdds/dds/xtypes/dynamic_types/DynamicType.hpp" #include "rcutils/allocator.h" #include "rcutils/error_handling.h" @@ -60,7 +59,7 @@ #include "type_support_common.hpp" -using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; +using PropertyPolicyHelper = eprosima::fastdds::rtps::PropertyPolicyHelper; namespace rmw_fastrtps_cpp { @@ -185,12 +184,12 @@ __create_dynamic_subscription( // Find and check existing topic and type // Create Topic and Type names - auto dyn_type_ptr = eprosima::fastrtps::types::DynamicType_ptr( - *static_cast( + auto dyn_type_ptr = eprosima::fastdds::dds::DynamicType::_ref_type( + *static_cast( ts_impl->dynamic_message_type->impl.handle)); // Check if we need to split the name into namespace and type name - std::string type_name = dyn_type_ptr->get_name(); + std::string type_name = dyn_type_ptr->get_name().to_string(); int occurrences = 0; std::string::size_type pos = 0; @@ -280,23 +279,24 @@ __create_dynamic_subscription( // We will still need a DynamicPubSubType later on (constructed from a // DynamicType_ptr) to convert the CDR buffer to a DynamicData, however... // fastdds_type.reset(dyn_type_ptr); - auto tsupport = new (std::nothrow) TypeSupport_cpp(); // NOT MessageTypeSupport_cpp!!! + auto tsupport = new + (std::nothrow) TypeSupport_cpp(type_support); // NOT MessageTypeSupport_cpp! if (!tsupport) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupport"); return nullptr; } // Because we're using TypeSupport_cpp, we need to do this - tsupport->setName(type_name.c_str()); + tsupport->set_name(type_name.c_str()); fastdds_type.reset(tsupport); } - if (keyed && !fastdds_type->m_isGetKeyDefined) { + if (keyed && !fastdds_type->is_compute_key_provided) { RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } @@ -379,7 +379,7 @@ __create_dynamic_subscription( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } @@ -549,7 +549,7 @@ __create_subscription( ///// // Create the Type Support struct if (!fastdds_type) { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks, type_supports); if (!tsupport) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); return nullptr; @@ -559,23 +559,18 @@ __create_subscription( fastdds_type.reset(tsupport); } - if (keyed && !fastdds_type->m_isGetKeyDefined) { + if (keyed && !fastdds_type->is_compute_key_provided) { RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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; - if (!rmw_fastrtps_shared_cpp::register_type_object(type_supports, type_name)) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to register type object with incompatible type %s", - type_name.c_str()); - return nullptr; - } + info->type_support_->register_type_object_representation(); ///// // Create Listener @@ -646,7 +641,7 @@ __create_subscription( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index eb3d38232..7c31a950e 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -23,9 +23,12 @@ namespace rmw_fastrtps_cpp { -TypeSupport::TypeSupport() +TypeSupport::TypeSupport( + const rosidl_message_type_support_t * type_supports +) +: rmw_fastrtps_shared_cpp::TypeSupport(type_supports) { - m_isGetKeyDefined = false; + is_compute_key_provided = false; max_size_bound_ = false; is_plain_ = false; } @@ -54,15 +57,15 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) } // Total size is encapsulation size + data size - m_typeSize = 4 + data_size; + max_serialized_type_size = 4 + data_size; // Account for RTPS submessage alignment - m_typeSize = (m_typeSize + 3) & ~3; + max_serialized_type_size = (max_serialized_type_size + 3) & ~3; } size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const { if (is_plain_) { - return m_typeSize; + return max_serialized_type_size; } assert(ros_message); @@ -117,52 +120,62 @@ bool TypeSupport::deserializeROSmessage( } catch (const eprosima::fastcdr::exception::Exception &) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "Fast CDR exception deserializing message of type %s.", - getName()); + get_name().c_str()); return false; } catch (const std::bad_alloc &) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "'Bad alloc' exception deserializing message of type %s.", - getName()); + get_name().c_str()); return false; } return true; } -MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) +MessageTypeSupport::MessageTypeSupport( + const message_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports) +: rmw_fastrtps_cpp::TypeSupport(type_supports) { assert(members); std::string name = _create_type_name(members); - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(members); } -ServiceTypeSupport::ServiceTypeSupport() +ServiceTypeSupport::ServiceTypeSupport(const rosidl_message_type_support_t * type_supports) +: rmw_fastrtps_cpp::TypeSupport(type_supports) { } -RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) +RequestTypeSupport::RequestTypeSupport( + const service_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports) +: ServiceTypeSupport(type_supports) { assert(members); auto msg = static_cast( members->request_members_->data); std::string name = _create_type_name(msg); // + "Request_"; - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(msg); } -ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) +ResponseTypeSupport::ResponseTypeSupport( + const service_type_support_callbacks_t * members, + const rosidl_message_type_support_t * type_supports) +: ServiceTypeSupport(type_supports) { assert(members); auto msg = static_cast( members->response_members_->data); std::string name = _create_type_name(msg); // + "Response_"; - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(msg); } diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 758f8bed8..8ffe07d2c 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -18,6 +18,8 @@ #include #include +#include "rcpputils/find_and_replace.hpp" + #include "rmw/error_handling.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" @@ -46,7 +48,9 @@ _create_type_name( { std::ostringstream ss; if (!message_namespace.empty()) { - ss << message_namespace << "::"; + // Find and replace C namespace separator with C++, in case this is using C typesupport + std::string message_namespace_new = rcpputils::find_and_replace(message_namespace, "__", "::"); + ss << message_namespace_new << "::"; } ss << "dds_::" << message_name << "_"; return ss.str(); diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 914e7acc7..bc4af9625 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -38,10 +38,8 @@ find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(tracetools REQUIRED) -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) find_package(rmw REQUIRED) find_package(rosidl_runtime_c REQUIRED) @@ -101,7 +99,7 @@ add_library(rmw_fastrtps_dynamic_cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp PUBLIC fastcdr - fastrtps + fastdds rcpputils::rcpputils rcutils::rcutils rmw::rmw @@ -161,7 +159,7 @@ if(BUILD_TESTING) ament_add_gtest(test_logging test/test_logging.cpp) target_link_libraries(test_logging - fastrtps + fastdds rmw::rmw rmw_fastrtps_dynamic_cpp ) diff --git a/rmw_fastrtps_dynamic_cpp/package.xml b/rmw_fastrtps_dynamic_cpp/package.xml index 233d5c72e..aa1a21f5e 100644 --- a/rmw_fastrtps_dynamic_cpp/package.xml +++ b/rmw_fastrtps_dynamic_cpp/package.xml @@ -16,13 +16,11 @@ Ricardo González ament_cmake_ros - fastrtps_cmake_module ament_cmake fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcpputils rcutils rmw @@ -34,8 +32,7 @@ tracetools fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcpputils rcutils rmw diff --git a/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake b/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake index 354cb9484..de9dbc530 100644 --- a/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake +++ b/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake @@ -14,11 +14,9 @@ # copied from rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) -list(APPEND rmw_fastrtps_dynamic_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +list(APPEND rmw_fastrtps_dynamic_cpp_INCLUDE_DIRS ${FAST_INCLUDE_DIR}) # specific order: dependents before dependencies -list(APPEND rmw_fastrtps_dynamic_cpp_LIBRARIES fastrtps fastcdr) +list(APPEND rmw_fastrtps_dynamic_cpp_LIBRARIES fastdds fastcdr) diff --git a/rmw_fastrtps_dynamic_cpp/src/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/src/MessageTypeSupport_impl.hpp index c56945ae1..9646f2fff 100644 --- a/rmw_fastrtps_dynamic_cpp/src/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/MessageTypeSupport_impl.hpp @@ -48,20 +48,21 @@ MessageTypeSupport::MessageTypeSupport( ss << message_namespace << "::"; } ss << "dds_::" << message_name << "_"; - this->setName(ss.str().c_str()); + this->set_name(ss.str().c_str()); // Fully bound and plain by default this->max_size_bound_ = true; this->is_plain_ = true; // Encapsulation size - this->m_typeSize = 4; + this->max_serialized_type_size = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(members, 0)); + this->max_serialized_type_size += + static_cast(this->calculateMaxSerializedSize(members, 0)); } else { - this->m_typeSize++; + this->max_serialized_type_size++; } // Account for RTPS submessage alignment - this->m_typeSize = (this->m_typeSize + 3) & ~3; + this->max_serialized_type_size = (this->max_serialized_type_size + 3) & ~3; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport.hpp index 01f2713e6..c0e0ba5df 100644 --- a/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport.hpp @@ -30,14 +30,20 @@ template class RequestTypeSupport : public TypeSupport { public: - RequestTypeSupport(const ServiceMembersType * members, const void * ros_type_support); + RequestTypeSupport( + const ServiceMembersType * members, + const void * ros_type_support, + const void * message_type_supports); }; template class ResponseTypeSupport : public TypeSupport { public: - ResponseTypeSupport(const ServiceMembersType * members, const void * ros_type_support); + ResponseTypeSupport( + const ServiceMembersType * members, + const void * ros_type_support, + const void * message_type_supports); }; } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport_impl.hpp index b7152480a..f397174cb 100644 --- a/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/ServiceTypeSupport_impl.hpp @@ -32,8 +32,10 @@ namespace rmw_fastrtps_dynamic_cpp template RequestTypeSupport::RequestTypeSupport( - const ServiceMembersType * members, const void * ros_type_support) -: TypeSupport(ros_type_support) + const ServiceMembersType * members, + const void * ros_type_support, + const void * message_type_supports) +: TypeSupport(ros_type_support, message_type_supports) { assert(members); this->members_ = members->request_members_; @@ -47,26 +49,29 @@ RequestTypeSupport::RequestTypeSupport( ss << service_namespace << "::"; } ss << "dds_::" << service_name << "_Request_"; - this->setName(ss.str().c_str()); + this->set_name(ss.str().c_str()); // Fully bound and plain by default this->max_size_bound_ = true; this->is_plain_ = true; // Encapsulation size - this->m_typeSize = 4; + this->max_serialized_type_size = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + this->max_serialized_type_size += + static_cast(this->calculateMaxSerializedSize(this->members_, 0)); } else { - this->m_typeSize++; + this->max_serialized_type_size++; } // Account for RTPS submessage alignment - this->m_typeSize = (this->m_typeSize + 3) & ~3; + this->max_serialized_type_size = (this->max_serialized_type_size + 3) & ~3; } template ResponseTypeSupport::ResponseTypeSupport( - const ServiceMembersType * members, const void * ros_type_support) -: TypeSupport(ros_type_support) + const ServiceMembersType * members, + const void * ros_type_support, + const void * message_type_supports) +: TypeSupport(ros_type_support, message_type_supports) { assert(members); this->members_ = members->response_members_; @@ -80,20 +85,21 @@ ResponseTypeSupport::ResponseTypeSupport ss << service_namespace << "::"; } ss << "dds_::" << service_name << "_Response_"; - this->setName(ss.str().c_str()); + this->set_name(ss.str().c_str()); // Fully bound and plain by default this->max_size_bound_ = true; this->is_plain_ = true; // Encapsulation size - this->m_typeSize = 4; + this->max_serialized_type_size = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + this->max_serialized_type_size += + static_cast(this->calculateMaxSerializedSize(this->members_, 0)); } else { - this->m_typeSize++; + this->max_serialized_type_size++; } // Account for RTPS submessage alignment - this->m_typeSize = (this->m_typeSize + 3) & ~3; + this->max_serialized_type_size = (this->max_serialized_type_size + 3) & ~3; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/src/TypeSupport.hpp index 627e4d5c2..ae6b6f72c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/TypeSupport.hpp @@ -149,12 +149,27 @@ class BaseTypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport } protected: + // Constructor meant to be used by message typesupports explicit BaseTypeSupport(const void * ros_type_support) + : rmw_fastrtps_shared_cpp::TypeSupport( + static_cast(ros_type_support)) + { + ros_type_support_ = ros_type_support; + } + + // Constructor for service typesupports + explicit BaseTypeSupport( + const void * ros_type_support, + const void * ros_message_type_supports) + : rmw_fastrtps_shared_cpp::TypeSupport( + static_cast(ros_message_type_supports)) { ros_type_support_ = ros_type_support; } private: + // Can either be a rosidl_message_type_support_t + // or rosidl_service_type_support_t const void * ros_type_support_; }; @@ -171,8 +186,14 @@ class TypeSupport : public BaseTypeSupport eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; protected: + // Meant for messages typesupport explicit TypeSupport(const void * ros_type_support); + // Services typesupport + explicit TypeSupport( + const void * ros_type_support, + const void * ros_message_type_supports); + size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment); const MembersType * members_; diff --git a/rmw_fastrtps_dynamic_cpp/src/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/src/TypeSupport_impl.hpp index 142648fbf..37a414110 100644 --- a/rmw_fastrtps_dynamic_cpp/src/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/TypeSupport_impl.hpp @@ -62,7 +62,18 @@ template TypeSupport::TypeSupport(const void * ros_type_support) : BaseTypeSupport(ros_type_support) { - m_isGetKeyDefined = false; + is_compute_key_provided = false; + max_size_bound_ = false; + is_plain_ = false; +} + +template +TypeSupport::TypeSupport( + const void * ros_type_support, + const void * ros_message_type_supports) +: BaseTypeSupport(ros_type_support, ros_message_type_supports) +{ + is_compute_key_provided = false; max_size_bound_ = false; is_plain_ = false; } @@ -906,7 +917,7 @@ size_t TypeSupport::getEstimatedSerializedSize( const void * ros_message, const void * impl) const { if (is_plain_) { - return m_typeSize; + return max_serialized_type_size; } assert(ros_message); @@ -967,12 +978,12 @@ bool TypeSupport::deserializeROSmessage( } catch (const eprosima::fastcdr::exception::Exception &) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "Fast CDR exception deserializing message of type %s.", - getName()); + get_name().c_str()); return false; } catch (const std::bad_alloc &) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "'Bad alloc' exception deserializing message of type %s.", - getName()); + get_name().c_str()); return false; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 6d597abe7..cdd29e885 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -24,7 +24,7 @@ #include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" -#include "fastdds/rtps/resources/ResourceManagement.h" +#include "fastdds/rtps/attributes/ResourceManagement.hpp" #include "rcutils/error_handling.h" @@ -199,7 +199,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; } @@ -252,13 +252,13 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // Modify specific DataWriter Qos if (!participant_info->leave_middleware_default_qos) { if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { - writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } @@ -311,7 +311,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher_free(rmw_publisher); }); - rmw_publisher->can_loan_messages = info->type_support_->is_plain(); + // The type support in the RMW implementation is always XCDR1. + rmw_publisher->can_loan_messages = + info->type_support_->is_plain(eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION); rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index d8626b54c..25509b402 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -25,7 +25,7 @@ #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" -#include "fastdds/rtps/resources/ResourceManagement.h" +#include "fastdds/rtps/attributes/ResourceManagement.hpp" #include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" @@ -262,13 +262,13 @@ rmw_create_client( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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)) { + if (eprosima::fastdds::dds::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_client() failed to register response type"); return nullptr; } @@ -340,7 +340,7 @@ rmw_create_client( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } @@ -392,13 +392,13 @@ rmw_create_client( // 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; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 4e1fe8341..21ce34e21 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -264,13 +264,13 @@ rmw_create_service( response_fastdds_type.reset(tsupport); } - if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::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)) { + if (eprosima::fastdds::dds::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_service() failed to register response type"); return nullptr; } @@ -339,7 +339,7 @@ rmw_create_service( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } @@ -395,13 +395,13 @@ rmw_create_service( // 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; + writer_qos.publish_mode().kind = eprosima::fastdds::dds::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.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE; } writer_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; writer_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index c3d3433e3..821aff48a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -23,7 +23,7 @@ #include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" -#include "fastdds/rtps/resources/ResourceManagement.h" +#include "fastdds/rtps/attributes/ResourceManagement.hpp" #include "rcutils/error_handling.h" @@ -45,10 +45,6 @@ #include "rmw_fastrtps_shared_cpp/subscription.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/xmlparser/XMLProfileManager.h" - #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" #include "tracetools/tracetools.h" @@ -57,7 +53,7 @@ #include "type_support_common.hpp" #include "type_support_registry.hpp" -using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; +using PropertyPolicyHelper = eprosima::fastdds::rtps::PropertyPolicyHelper; namespace rmw_fastrtps_dynamic_cpp { @@ -204,12 +200,12 @@ create_subscription( fastdds_type.reset(tsupport); } - if (keyed && !fastdds_type->m_isGetKeyDefined) { + if (keyed && !fastdds_type->is_compute_key_provided) { RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); return nullptr; } - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + if (eprosima::fastdds::dds::RETCODE_OK != fastdds_type.register_type(dds_participant)) { RMW_SET_ERROR_MSG("create_subscription() failed to register type"); return nullptr; } @@ -263,7 +259,7 @@ create_subscription( if (!participant_info->leave_middleware_default_qos) { reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; reader_qos.data_sharing().off(); } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp index f4924c7a4..3f7c8123d 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp @@ -18,10 +18,11 @@ namespace rmw_fastrtps_dynamic_cpp { TypeSupportProxy::TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_type) +: rmw_fastrtps_shared_cpp::TypeSupport(inner_type->ros_message_type_supports()) { - setName(inner_type->getName()); - m_typeSize = inner_type->m_typeSize; - is_plain_ = inner_type->is_plain(); + set_name(inner_type->get_name()); + max_serialized_type_size = inner_type->max_serialized_type_size; + is_plain_ = inner_type->is_plain(eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION); max_size_bound_ = inner_type->is_bounded(); } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp index d7f635b84..3400522ea 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp @@ -105,11 +105,13 @@ type_support_ptr TypeSupportRegistry::get_request_type_support( if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { auto members = static_cast( ros_type_support->data); - return new RequestTypeSupport_c(members, ros_type_support); + return new RequestTypeSupport_c(members, ros_type_support, + ros_type_support->request_typesupport); } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { auto members = static_cast( ros_type_support->data); - return new RequestTypeSupport_cpp(members, ros_type_support); + return new RequestTypeSupport_cpp(members, ros_type_support, + ros_type_support->request_typesupport); } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; @@ -126,11 +128,13 @@ type_support_ptr TypeSupportRegistry::get_response_type_support( if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { auto members = static_cast( ros_type_support->data); - return new ResponseTypeSupport_c(members, ros_type_support); + return new ResponseTypeSupport_c(members, ros_type_support, + ros_type_support->response_typesupport); } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { auto members = static_cast( ros_type_support->data); - return new ResponseTypeSupport_cpp(members, ros_type_support); + return new ResponseTypeSupport_cpp(members, ros_type_support, + ros_type_support->response_typesupport); } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index aa5d67573..ebe32229d 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -45,10 +45,8 @@ find_package(rosidl_typesupport_introspection_c REQUIRED) find_package(rosidl_typesupport_introspection_cpp REQUIRED) find_package(tracetools REQUIRED) -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) find_package(rmw REQUIRED) @@ -105,7 +103,7 @@ target_include_directories(rmw_fastrtps_shared_cpp "$") target_link_libraries(rmw_fastrtps_shared_cpp PUBLIC fastcdr - fastrtps + fastdds rcpputils::rcpputils rcutils::rcutils rmw::rmw 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 df370b0f5..476bf160d 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 @@ -19,9 +19,8 @@ #include #include "fastdds/dds/topic/TopicDataType.hpp" - -#include "fastdds/rtps/common/InstanceHandle.h" -#include "fastdds/rtps/common/SerializedPayload.h" +#include "fastdds/rtps/common/InstanceHandle.hpp" +#include "fastdds/rtps/common/SerializedPayload.hpp" #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" @@ -37,9 +36,9 @@ namespace rmw_fastrtps_shared_cpp enum SerializedDataType { - FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER, - FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE, - FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE + FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER, + FASTDDS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE, + FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE }; // Publishers write method will receive a pointer to this struct @@ -62,9 +61,19 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const = 0; RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool getKey( - void * data, - eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool compute_key( + const void * const data, + eprosima::fastdds::rtps::InstanceHandle_t & ihandle, + bool force_md5 = false) override + { + (void)data; (void)ihandle; (void)force_md5; + return false; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool compute_key( + eprosima::fastdds::rtps::SerializedPayload_t & data, + eprosima::fastdds::rtps::InstanceHandle_t & ihandle, bool force_md5 = false) override { (void)data; (void)ihandle; (void)force_md5; @@ -72,19 +81,25 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType } RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) override; + bool serialize( + const void * const data, + eprosima::fastdds::rtps::SerializedPayload_t & payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data) override; + bool deserialize(eprosima::fastdds::rtps::SerializedPayload_t & payload, void * data) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC - std::function getSerializedSizeProvider(void * data) override; + uint32_t calculate_serialized_size( + const void * const data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) + override; RMW_FASTRTPS_SHARED_CPP_PUBLIC - void * createData() override; + void * create_data() override; RMW_FASTRTPS_SHARED_CPP_PUBLIC - void deleteData(void * data) override; + void delete_data(void * data) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC inline bool is_bounded() const @@ -96,18 +111,17 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType } RMW_FASTRTPS_SHARED_CPP_PUBLIC - inline bool is_plain() const -#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN - override -#endif + inline bool is_plain(eprosima::fastdds::dds::DataRepresentationId_t rep) const override { - return is_plain_; + return is_plain_ && rep == eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION; } + RMW_FASTRTPS_SHARED_CPP_PUBLIC void register_type_object_representation() override; + RMW_FASTRTPS_SHARED_CPP_PUBLIC - inline bool is_plain(eprosima::fastdds::dds::DataRepresentationId_t rep) const override + inline const rosidl_message_type_support_t * ros_message_type_supports() const { - return is_plain_ && rep == eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION; + return type_supports_; } RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -115,17 +129,15 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType protected: RMW_FASTRTPS_SHARED_CPP_PUBLIC - TypeSupport(); + TypeSupport( + const rosidl_message_type_support_t * type_supports + ); - bool max_size_bound_; - bool is_plain_; + bool max_size_bound_ {false}; + bool is_plain_ {false}; + const rosidl_message_type_support_t * type_supports_ {nullptr}; }; -RMW_FASTRTPS_SHARED_CPP_PUBLIC -bool register_type_object( - const rosidl_message_type_support_t * type_supports, - const std::string & type_name); - } // 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 aaecad510..1551558d4 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 "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.hpp" #include "rmw/types.h" @@ -26,7 +26,7 @@ namespace rmw_fastrtps_shared_cpp RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_gid_t -create_rmw_gid(const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid); +create_rmw_gid(const char * identifier, const eprosima::fastdds::rtps::GUID_t & guid); } // namespace rmw_fastrtps_shared_cpp 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 8446e89b7..d0c338e9d 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 @@ -33,9 +33,9 @@ #include "fastdds/dds/subscriber/qos/DataReaderQos.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 "fastdds/rtps/common/Guid.hpp" +#include "fastdds/rtps/common/InstanceHandle.hpp" +#include "fastdds/rtps/common/SampleIdentity.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -62,8 +62,8 @@ typedef struct CustomClientInfo eprosima::fastdds::dds::Topic * response_topic_{nullptr}; ClientListener * listener_{nullptr}; - eprosima::fastrtps::rtps::GUID_t writer_guid_; - eprosima::fastrtps::rtps::GUID_t reader_guid_; + eprosima::fastdds::rtps::GUID_t writer_guid_; + eprosima::fastdds::rtps::GUID_t reader_guid_; const char * typesupport_identifier_{nullptr}; ClientPubListener * pub_listener_{nullptr}; @@ -73,7 +73,7 @@ typedef struct CustomClientInfo typedef struct CustomClientResponse { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastdds::rtps::SampleIdentity sample_identity_; std::unique_ptr buffer_; } CustomClientResponse; @@ -109,9 +109,9 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener return; } if (info.current_count_change == 1) { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + publishers_.insert(eprosima::fastdds::rtps::iHandle2GUID(info.last_publication_handle)); } else if (info.current_count_change == -1) { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + publishers_.erase(eprosima::fastdds::rtps::iHandle2GUID(info.last_publication_handle)); } else { return; } @@ -160,7 +160,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener private: CustomClientInfo * info_; - std::set publishers_; + std::set publishers_; rmw_event_callback_t on_new_response_cb_{nullptr}; @@ -186,9 +186,9 @@ class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener return; } if (info.current_count_change == 1) { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + subscriptions_.insert(eprosima::fastdds::rtps::iHandle2GUID(info.last_subscription_handle)); } else if (info.current_count_change == -1) { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + subscriptions_.erase(eprosima::fastdds::rtps::iHandle2GUID(info.last_subscription_handle)); } else { return; } @@ -197,7 +197,7 @@ class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener private: CustomClientInfo * info_; - std::set subscriptions_; + std::set subscriptions_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_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 52bd91962..8b4ee92b9 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 @@ -29,9 +29,9 @@ #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 "fastdds/rtps/builtin/data/PublicationBuiltinTopicData.hpp" +#include "fastdds/rtps/builtin/data/SubscriptionBuiltinTopicData.hpp" +#include "fastdds/rtps/participant/ParticipantDiscoveryInfo.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" @@ -145,15 +145,17 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList {} void on_participant_discovery( - eprosima::fastdds::dds::DomainParticipant *, - eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info, + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::rtps::ParticipantDiscoveryStatus reason, + const eprosima::fastdds::dds::ParticipantBuiltinTopicData & info, bool & should_be_ignored) override { + static_cast(participant); should_be_ignored = false; - switch (info.status) { - case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: + switch (reason) { + case eprosima::fastdds::rtps::ParticipantDiscoveryStatus::DISCOVERED_PARTICIPANT: { - auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); + auto map = rmw::impl::cpp::parse_key_value(info.user_data); auto name_found = map.find("enclave"); if (name_found == map.end()) { @@ -164,41 +166,47 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList context->graph_cache.add_participant( rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier_, info.info.m_guid), + identifier_, info.guid), enclave); break; } - case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: + case eprosima::fastdds::rtps::ParticipantDiscoveryStatus::REMOVED_PARTICIPANT: // fall through - case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: + case eprosima::fastdds::rtps::ParticipantDiscoveryStatus::DROPPED_PARTICIPANT: context->graph_cache.remove_participant( rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier_, info.info.m_guid)); + identifier_, info.guid)); break; default: return; } } - void on_subscriber_discovery( + void on_data_reader_discovery( eprosima::fastdds::dds::DomainParticipant *, - eprosima::fastrtps::rtps::ReaderDiscoveryInfo && info) override + eprosima::fastdds::rtps::ReaderDiscoveryStatus reason, + const eprosima::fastdds::dds::SubscriptionBuiltinTopicData & info, + bool & should_be_ignored) override { - if (eprosima::fastrtps::rtps::ReaderDiscoveryInfo::CHANGED_QOS_READER != info.status) { + should_be_ignored = false; + if (eprosima::fastdds::rtps::ReaderDiscoveryStatus::CHANGED_QOS_READER != reason) { bool is_alive = - eprosima::fastrtps::rtps::ReaderDiscoveryInfo::DISCOVERED_READER == info.status; - process_discovery_info(info.info, is_alive, true); + eprosima::fastdds::rtps::ReaderDiscoveryStatus::DISCOVERED_READER == reason; + process_discovery_info(info, is_alive, true); } } - void on_publisher_discovery( + void on_data_writer_discovery( eprosima::fastdds::dds::DomainParticipant *, - eprosima::fastrtps::rtps::WriterDiscoveryInfo && info) override + eprosima::fastdds::rtps::WriterDiscoveryStatus reason, + const eprosima::fastdds::rtps::PublicationBuiltinTopicData & info, + bool & should_be_ignored) override { - if (eprosima::fastrtps::rtps::WriterDiscoveryInfo::CHANGED_QOS_WRITER != info.status) { + should_be_ignored = false; + if (eprosima::fastdds::rtps::WriterDiscoveryStatus::CHANGED_QOS_WRITER != reason) { bool is_alive = - eprosima::fastrtps::rtps::WriterDiscoveryInfo::DISCOVERED_WRITER == info.status; - process_discovery_info(info.info, is_alive, false); + eprosima::fastdds::rtps::WriterDiscoveryStatus::DISCOVERED_WRITER == reason; + process_discovery_info(info, is_alive, false); } } @@ -210,9 +218,9 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList { if (is_alive) { rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; - rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); + rtps_qos_to_rmw_qos(proxyData, &qos_profile); - const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); + const auto & userDataValue = proxyData.user_data.getValue(); rosidl_type_hash_t type_hash; if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( userDataValue.data(), userDataValue.size(), type_hash)) @@ -239,20 +247,20 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, - proxyData.guid()), - proxyData.topicName().to_string(), - proxyData.typeName().to_string(), + proxyData.guid), + proxyData.topic_name.to_string(), + proxyData.type_name.to_string(), type_hash, rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, - iHandle2GUID(proxyData.RTPSParticipantKey())), + proxyData.participant_guid), qos_profile, is_reader); } else { context->graph_cache.remove_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, - proxyData.guid()), + proxyData.guid), is_reader); } } 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 f3023f72b..8dc52bd6e 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 @@ -28,8 +28,8 @@ #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 "fastdds/rtps/common/Guid.hpp" +#include "fastdds/rtps/common/InstanceHandle.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" @@ -122,7 +122,7 @@ class RMWPublisherEvent final : public EventListenerInterface * \param[in] guid The GUID of the newly-matched subscription to track. */ RMW_FASTRTPS_SHARED_CPP_PUBLIC - void track_unique_subscription(eprosima::fastrtps::rtps::GUID_t guid); + void track_unique_subscription(eprosima::fastdds::rtps::GUID_t guid); /// Remove a GUID from the internal set of unique subscriptions matched to this publisher. /** @@ -132,7 +132,7 @@ class RMWPublisherEvent final : public EventListenerInterface * \param[in] guid The GUID of the newly-unmatched subscription to track. */ RMW_FASTRTPS_SHARED_CPP_PUBLIC - void untrack_unique_subscription(eprosima::fastrtps::rtps::GUID_t guid); + void untrack_unique_subscription(eprosima::fastdds::rtps::GUID_t guid); /// Return the number of unique subscriptions matched to this publisher. /** @@ -162,7 +162,7 @@ class RMWPublisherEvent final : public EventListenerInterface private: CustomPublisherInfo * publisher_info_ = nullptr; - std::set subscriptions_ + std::set subscriptions_ RCPPUTILS_TSA_GUARDED_BY(subscriptions_mutex_); mutable std::mutex subscriptions_mutex_; 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 dd2b4cdd3..693e94b1b 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 @@ -32,9 +32,9 @@ #include "fastdds/dds/subscriber/qos/DataReaderQos.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 "fastdds/rtps/common/Guid.hpp" +#include "fastdds/rtps/common/InstanceHandle.hpp" +#include "fastdds/rtps/common/SampleIdentity.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -74,19 +74,19 @@ typedef struct CustomServiceInfo typedef struct CustomServiceRequest { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastdds::rtps::SampleIdentity sample_identity_; eprosima::fastcdr::FastBuffer * buffer_{nullptr}; } CustomServiceRequest; class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { using subscriptions_set_t = - std::unordered_set; + std::unordered_set; using clients_endpoints_map_t = - std::unordered_map; + std::unordered_map; public: explicit ServicePubListener( @@ -102,10 +102,10 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { std::lock_guard lock(mutex_); if (info.current_count_change == 1) { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + subscriptions_.insert(eprosima::fastdds::rtps::iHandle2GUID(info.last_subscription_handle)); } else if (info.current_count_change == -1) { - eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + eprosima::fastdds::rtps::GUID_t erase_endpoint_guid = + eprosima::fastdds::rtps::iHandle2GUID(info.last_subscription_handle); subscriptions_.erase(erase_endpoint_guid); auto endpoint = clients_endpoints_.find(erase_endpoint_guid); if (endpoint != clients_endpoints_.end()) { @@ -121,7 +121,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener template bool wait_for_subscription( - const eprosima::fastrtps::rtps::GUID_t & guid, + const eprosima::fastdds::rtps::GUID_t & guid, const std::chrono::duration & rel_time) { auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool @@ -136,7 +136,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener template client_present_t check_for_subscription( - const eprosima::fastrtps::rtps::GUID_t & guid, + const eprosima::fastdds::rtps::GUID_t & guid, const std::chrono::duration & max_blocking_time) { { @@ -155,7 +155,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener } void endpoint_erase_if_exists( - const eprosima::fastrtps::rtps::GUID_t & endpointGuid) + const eprosima::fastdds::rtps::GUID_t & endpointGuid) { std::lock_guard lock(mutex_); auto endpoint = clients_endpoints_.find(endpointGuid); @@ -166,8 +166,8 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener } void endpoint_add_reader_and_writer( - const eprosima::fastrtps::rtps::GUID_t & readerGuid, - const eprosima::fastrtps::rtps::GUID_t & writerGuid) + const eprosima::fastdds::rtps::GUID_t & readerGuid, + const eprosima::fastdds::rtps::GUID_t & writerGuid) { std::lock_guard lock(mutex_); clients_endpoints_.emplace(readerGuid, writerGuid); @@ -197,7 +197,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { if (info.current_count_change == -1) { info_->pub_listener_->endpoint_erase_if_exists( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + eprosima::fastdds::rtps::iHandle2GUID(info.last_publication_handle)); } } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index a1c8c3d0b..fed1d9445 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 @@ -32,8 +32,8 @@ #include "fastdds/dds/topic/ContentFilteredTopic.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" -#include "fastdds/rtps/common/Guid.h" -#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/Guid.hpp" +#include "fastdds/rtps/common/InstanceHandle.hpp" #include "rcpputils/thread_safety_annotations.hpp" @@ -64,12 +64,12 @@ class CustomDataReaderListener final : public eprosima::fastdds::dds::DataReader RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_requested_deadline_missed( eprosima::fastdds::dds::DataReader * reader, - const eprosima::fastrtps::RequestedDeadlineMissedStatus & status) override; + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_liveliness_changed( eprosima::fastdds::dds::DataReader * reader, - const eprosima::fastrtps::LivelinessChangedStatus & status) override; + const eprosima::fastdds::dds::LivelinessChangedStatus & status) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_sample_lost( @@ -174,7 +174,7 @@ class RMWSubscriptionEvent final : public EventListenerInterface * \param[in] guid The GUID of the newly-matched publisher to track. */ RMW_FASTRTPS_SHARED_CPP_PUBLIC - void track_unique_publisher(eprosima::fastrtps::rtps::GUID_t guid); + void track_unique_publisher(eprosima::fastdds::rtps::GUID_t guid); /// Remove a GUID from the internal set of unique publishers matched to this subscription. /** @@ -184,7 +184,7 @@ class RMWSubscriptionEvent final : public EventListenerInterface * \param[in] guid The GUID of the newly-unmatched publisher to track. */ RMW_FASTRTPS_SHARED_CPP_PUBLIC - void untrack_unique_publisher(eprosima::fastrtps::rtps::GUID_t guid); + void untrack_unique_publisher(eprosima::fastdds::rtps::GUID_t guid); /// Return the number of unique publishers matched to this subscription. /** @@ -252,7 +252,7 @@ class RMWSubscriptionEvent final : public EventListenerInterface eprosima::fastdds::dds::SubscriptionMatchedStatus matched_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( + std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( publishers_mutex_); rmw_event_callback_t on_new_message_cb_{nullptr}; 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 e645f952b..099486687 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,16 +20,16 @@ #include #include -#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.hpp" namespace rmw_fastrtps_shared_cpp { template void -copy_from_byte_array_to_fastrtps_guid( +copy_from_byte_array_to_fastdds_guid( const ByteT * guid_byte_array, - eprosima::fastrtps::rtps::GUID_t * guid) + eprosima::fastdds::rtps::GUID_t * guid) { static_assert( std::is_same::value || std::is_same::value, @@ -43,8 +43,8 @@ copy_from_byte_array_to_fastrtps_guid( template void -copy_from_fastrtps_guid_to_byte_array( - const eprosima::fastrtps::rtps::GUID_t & guid, +copy_from_fastdds_guid_to_byte_array( + const eprosima::fastdds::rtps::GUID_t & guid, ByteT * guid_byte_array) { static_assert( @@ -56,9 +56,9 @@ copy_from_fastrtps_guid_to_byte_array( memcpy(&guid_byte_array[prefix_size], &guid.entityId, guid.entityId.size); } -struct hash_fastrtps_guid +struct hash_fastdds_guid { - std::size_t operator()(const eprosima::fastrtps::rtps::GUID_t & guid) const + std::size_t operator()(const eprosima::fastdds::rtps::GUID_t & guid) const { union u_convert { uint8_t plain_value[sizeof(guid)]; @@ -71,7 +71,7 @@ struct hash_fastrtps_guid offsetof(u_convert, plain_value) == offsetof(u_convert, plain_ints), "Plain guid should be easily convertible to uint32_t[4]"); - copy_from_fastrtps_guid_to_byte_array(guid, u.plain_value); + copy_from_fastdds_guid_to_byte_array(guid, u.plain_value); constexpr std::size_t prime_1 = 7; constexpr std::size_t prime_2 = 31; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp index b19e95706..e1b59201f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp @@ -17,7 +17,7 @@ #include -#include "fastrtps/utils/fixed_size_string.hpp" +#include "fastcdr/cdr/fixed_size_string.hpp" #include "rmw/types.h" #include "namespace_prefix.hpp" @@ -28,7 +28,7 @@ * \param[in] suffix Optional suffix for topic name. */ inline -eprosima::fastrtps::string_255 +eprosima::fastcdr::string_255 _mangle_topic_name( const char * prefix, const char * base, @@ -53,7 +53,7 @@ _mangle_topic_name( * \param[in] suffix Optional suffix for topic name. */ inline -eprosima::fastrtps::string_255 +eprosima::fastcdr::string_255 _create_topic_name( const rmw_qos_profile_t * qos_profile, const char * prefix, 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 694745b35..76edddad5 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 @@ -21,8 +21,6 @@ #include #include -#include "fastrtps/qos/QosPolicies.h" - #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/visibility_control.h" @@ -55,7 +53,7 @@ get_topic_qos( RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_time_t -dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); +dds_duration_to_rmw(const eprosima::fastdds::dds::Duration_t & duration); /* * Converts the DDS QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. @@ -132,17 +130,17 @@ dds_qos_to_rmw_qos( * \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 +template void rtps_qos_to_rmw_qos( - const RTPSQoSPolicyT & rtps_qos, + const BuiltinData & rtps_qos, rmw_qos_profile_t * qos) { - switch (rtps_qos.m_reliability.kind) { - case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + switch (rtps_qos.reliability.kind) { + case eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; - case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + case eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; break; default: @@ -150,11 +148,11 @@ rtps_qos_to_rmw_qos( break; } - switch (rtps_qos.m_durability.kind) { - case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + switch (rtps_qos.durability.kind) { + case eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; break; - case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + case eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; break; default: @@ -162,21 +160,21 @@ rtps_qos_to_rmw_qos( break; } - qos->deadline = dds_duration_to_rmw(rtps_qos.m_deadline.period); - qos->lifespan = dds_duration_to_rmw(rtps_qos.m_lifespan.duration); + qos->deadline = dds_duration_to_rmw(rtps_qos.deadline.period); + qos->lifespan = dds_duration_to_rmw(rtps_qos.lifespan.duration); - switch (rtps_qos.m_liveliness.kind) { - case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + switch (rtps_qos.liveliness.kind) { + case eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; break; - case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + case eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; break; default: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; break; } - qos->liveliness_lease_duration = dds_duration_to_rmw(rtps_qos.m_liveliness.lease_duration); + qos->liveliness_lease_duration = dds_duration_to_rmw(rtps_qos.liveliness.lease_duration); } extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -189,23 +187,4 @@ 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( - const AttributeT & dds_qos, - rmw_qos_profile_t * qos); - -extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void -dds_attributes_to_rmw_qos( - const eprosima::fastrtps::PublisherAttributes & dds_qos, - rmw_qos_profile_t * qos); - -extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC -void -dds_attributes_to_rmw_qos( - const eprosima::fastrtps::SubscriberAttributes & dds_qos, - rmw_qos_profile_t * qos); - #endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp index 611fa926e..3cf2a183f 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 "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/attributes/PropertyPolicy.hpp" #include "rmw_fastrtps_shared_cpp/visibility_control.h" @@ -26,6 +26,6 @@ * \returns true if the requested configuration was applied (or no configuration was requested). */ RMW_FASTRTPS_SHARED_CPP_PUBLIC -bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPolicy & policy); +bool apply_security_logging_configuration(eprosima::fastdds::rtps::PropertyPolicy & policy); #endif // RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 67f1a5ad6..9fd4bc925 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -21,7 +21,7 @@ #include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" -#include "fastrtps/types/TypesBase.h" +#include "fastdds/dds/core/ReturnCode.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -40,7 +40,7 @@ namespace rmw_fastrtps_shared_cpp */ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t -cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); +cast_error_dds_to_rmw(eprosima::fastdds::dds::ReturnCode_t code); /** * Tries to find already registered topic and type. diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml index aa5010ce7..a683b2c63 100644 --- a/rmw_fastrtps_shared_cpp/package.xml +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -16,14 +16,12 @@ Ricardo González ament_cmake_ros - fastrtps_cmake_module ament_cmake rosidl_dynamic_typesupport fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcpputils rcutils rmw @@ -35,8 +33,7 @@ rosidl_dynamic_typesupport fastcdr - fastrtps - fastrtps_cmake_module + fastdds rcpputils rcutils rmw diff --git a/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake b/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake index a053d5971..2c90bd73e 100644 --- a/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake +++ b/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake @@ -14,11 +14,9 @@ # copied from rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr 2 REQUIRED CONFIG) -find_package(fastrtps 2.13 REQUIRED CONFIG) -find_package(FastRTPS 2.13 REQUIRED MODULE) +find_package(fastcdr 2 REQUIRED) +find_package(fastdds 3 REQUIRED) -list(APPEND rmw_fastrtps_shared_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +list(APPEND rmw_fastrtps_shared_cpp_INCLUDE_DIRS ${FastDDS_INCLUDE_DIR}) # specific order: dependents before dependencies -list(APPEND rmw_fastrtps_shared_cpp_LIBRARIES fastrtps fastcdr) +list(APPEND rmw_fastrtps_shared_cpp_LIBRARIES fastdds fastcdr) diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index 7a57bef7c..fceb90b2b 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -18,19 +18,20 @@ #include #include -#include "fastdds/rtps/common/SerializedPayload.h" +#include "fastdds/rtps/common/SerializedPayload.hpp" +#include "fastdds/utils/md5.hpp" #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include "fastrtps/rtps/common/SerializedPayload.h" -#include "fastrtps/utils/md5.h" -#include "fastrtps/types/DynamicData.h" -#include "fastrtps/types/DynamicPubSubType.h" -#include "fastrtps/types/TypesBase.h" -#include "fastrtps/types/TypeObjectFactory.h" -#include "fastrtps/types/TypeNamesGenerator.h" -#include "fastrtps/types/AnnotationParameterValue.h" +#include "fastdds/dds/domain/DomainParticipantFactory.hpp" +#include "fastdds/dds/xtypes/dynamic_types/DynamicData.hpp" +#include "fastdds/dds/xtypes/dynamic_types/DynamicPubSubType.hpp" +#include "fastdds/dds/xtypes/type_representation/ITypeObjectRegistry.hpp" +#include "fastdds/dds/xtypes/type_representation/TypeObject.hpp" +#include "fastdds/dds/xtypes/type_representation/TypeObjectUtils.hpp" + +#include "rcpputils/find_and_replace.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw/error_handling.h" @@ -44,72 +45,77 @@ namespace rmw_fastrtps_shared_cpp { -TypeSupport::TypeSupport() +namespace xtypes = eprosima::fastdds::dds::xtypes; + +TypeSupport::TypeSupport( + const rosidl_message_type_support_t * type_supports +) +: type_supports_(type_supports) { - m_isGetKeyDefined = false; - max_size_bound_ = false; - is_plain_ = false; - auto_fill_type_object(false); - auto_fill_type_information(false); + is_compute_key_provided = false; } -void TypeSupport::deleteData(void * data) +void TypeSupport::delete_data(void * data) { assert(data); delete static_cast(data); } -void * TypeSupport::createData() +void * TypeSupport::create_data() { return new eprosima::fastcdr::FastBuffer(); } bool TypeSupport::serialize( - void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) + const void * const data, eprosima::fastdds::rtps::SerializedPayload_t & payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) { assert(data); - assert(payload); + static_cast(data_representation); - auto ser_data = static_cast(data); + const SerializedData * const ser_data = static_cast(data); switch (ser_data->type) { - case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: + case FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: { eprosima::fastcdr::FastBuffer fastbuffer( // Object that manages the raw buffer - reinterpret_cast(payload->data), payload->max_size); + reinterpret_cast(payload.data), payload.max_size); eprosima::fastcdr::Cdr ser( // Object that serializes the data fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::CdrVersion::XCDRv1); ser.set_encoding_flag(eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); if (this->serializeROSmessage(ser_data->data, ser, ser_data->impl)) { - payload->encapsulation = ser.endianness() == + payload.encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - payload->length = (uint32_t)ser.get_serialized_data_length(); + payload.length = (uint32_t)ser.get_serialized_data_length(); return true; } break; } - case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: + case FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER: { auto ser = static_cast(ser_data->data); - if (payload->max_size >= ser->get_serialized_data_length()) { - payload->length = static_cast(ser->get_serialized_data_length()); - payload->encapsulation = ser->endianness() == + if (payload.max_size >= ser->get_serialized_data_length()) { + payload.length = static_cast(ser->get_serialized_data_length()); + payload.encapsulation = ser->endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - memcpy(payload->data, ser->get_buffer_pointer(), ser->get_serialized_data_length()); + memcpy(payload.data, ser->get_buffer_pointer(), ser->get_serialized_data_length()); return true; } break; } - case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: + case FASTDDS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: { - auto m_type = std::make_shared(); + auto m_type = std::make_shared(); + eprosima::fastdds::dds::DynamicData::_ref_type * dyn_data {static_cast(ser_data->data)}; // Serializes payload into dynamic data stored in data->data return m_type->serialize( - static_cast(ser_data->data), payload + dyn_data, payload, data_representation ); } @@ -120,42 +126,42 @@ bool TypeSupport::serialize( } bool TypeSupport::deserialize( - eprosima::fastrtps::rtps::SerializedPayload_t * payload, + eprosima::fastdds::rtps::SerializedPayload_t & payload, void * data) { assert(data); - assert(payload); auto ser_data = static_cast(data); switch (ser_data->type) { - case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: + case FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: { eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload->data), payload->length); + reinterpret_cast(payload.data), payload.length); eprosima::fastcdr::Cdr deser( fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN); return deserializeROSmessage(deser, ser_data->data, ser_data->impl); } - case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: + case FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER: { auto buffer = static_cast(ser_data->data); - if (!buffer->reserve(payload->length)) { + if (!buffer->reserve(payload.length)) { return false; } - memcpy(buffer->getBuffer(), payload->data, payload->length); + memcpy(buffer->getBuffer(), payload.data, payload.length); return true; } - case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: + case FASTDDS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: { - auto m_type = std::make_shared(); + auto m_type = std::make_shared(); + eprosima::fastdds::dds::DynamicData::_ref_type * dyn_data {static_cast(ser_data->data)}; // Deserializes payload into dynamic data stored in data->data (copies!) - return m_type->deserialize( - payload, static_cast(ser_data->data) - ); + return m_type->deserialize(payload, dyn_data); } default: @@ -164,33 +170,45 @@ bool TypeSupport::deserialize( return false; } -std::function TypeSupport::getSerializedSizeProvider(void * data) +uint32_t TypeSupport::calculate_serialized_size( + const void * const data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) { assert(data); + static_cast(data_representation); - auto ser_data = static_cast(data); - auto ser_size = [this, ser_data]() -> uint32_t - { - if (ser_data->type == FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER) { - auto ser = static_cast(ser_data->data); - return static_cast(ser->get_serialized_data_length()); - } - return static_cast( - this->getEstimatedSerializedSize(ser_data->data, ser_data->impl)); - }; + const SerializedData * const ser_data = static_cast(data); + uint32_t ser_size {0}; + if (ser_data->type == FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER) { + auto ser = static_cast(ser_data->data); + ser_size = static_cast(ser->get_serialized_data_length()); + } else { + ser_size = static_cast(this->getEstimatedSerializedSize( + ser_data->data, + ser_data->impl)); + } return ser_size; } // TODO(iuhilnehc-ynos): add the following content into new files named TypeObject? -using CompleteStructType = eprosima::fastrtps::types::CompleteStructType; -using CompleteStructMember = eprosima::fastrtps::types::CompleteStructMember; -using MinimalStructType = eprosima::fastrtps::types::MinimalStructType; -using MinimalStructMember = eprosima::fastrtps::types::MinimalStructMember; -using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; -using TypeNamesGenerator = eprosima::fastrtps::types::TypeNamesGenerator; -using TypeIdentifier = eprosima::fastrtps::types::TypeIdentifier; -using TypeObject = eprosima::fastrtps::types::TypeObject; -using TypeObjectFactory = eprosima::fastrtps::types::TypeObjectFactory; +using CommonStructMember = xtypes::CommonStructMember; +using CompleteMemberDetail = xtypes::CompleteMemberDetail; +using CompleteStructHeader = xtypes::CompleteStructHeader; +using CompleteStructMember = xtypes::CompleteStructMember; +using CompleteStructMemberSeq = xtypes::CompleteStructMemberSeq; +using CompleteStructType = xtypes::CompleteStructType; +using CompleteTypeDetail = xtypes::CompleteTypeDetail; +using ITypeObjectRegistry = xtypes::ITypeObjectRegistry; +using MemberId = xtypes::MemberId; +using MinimalStructType = xtypes::MinimalStructType; +using MinimalStructMember = xtypes::MinimalStructMember; +using SerializedPayload_t = eprosima::fastdds::rtps::SerializedPayload_t; +using StructMemberFlag = xtypes::StructMemberFlag; +using StructTypeFlag = xtypes::StructTypeFlag; +using TypeIdentifier = xtypes::TypeIdentifier; +using TypeIdentifierPair = xtypes::TypeIdentifierPair; +using TypeObject = xtypes::TypeObject; +using TypeObjectUtils = xtypes::TypeObjectUtils; const rosidl_message_type_support_t * get_type_support_introspection(const rosidl_message_type_support_t * type_supports) @@ -234,258 +252,215 @@ _create_type_name(const MembersType * members) std::string message_namespace(members->message_namespace_); std::string message_name(members->message_name_); if (!message_namespace.empty()) { + // Find and replace C namespace separator with C++, in case this is using C typesupport + message_namespace = rcpputils::find_and_replace(message_namespace, "__", "::"); ss << message_namespace << "::"; } ss << "dds_::" << message_name << "_"; return ss.str(); } -typedef std::pair MemberIdentifierName; +typedef std::pair MemberIdentifierName; template -MemberIdentifierName GetTypeIdentifier(const MembersType * member, uint32_t index, bool complete); +MemberIdentifierName GetTypeIdentifier(const MembersType * members, uint32_t index); template -const TypeObject * GetCompleteObject( +TypeIdentifierPair register_type_identifiers( const std::string & type_name, const MembersType * members) { - const TypeObject * c_type_object = - TypeObjectFactory::get_instance()->get_type_object(type_name, true); - if (c_type_object != nullptr && c_type_object->_d() == eprosima::fastrtps::types::EK_COMPLETE) { - return c_type_object; - } - - TypeObject * type_object = new TypeObject(); - - type_object->_d(eprosima::fastrtps::types::EK_COMPLETE); - type_object->complete()._d(eprosima::fastrtps::types::TK_STRUCTURE); - type_object->complete().struct_type().struct_flags().IS_FINAL(false); - type_object->complete().struct_type().struct_flags().IS_APPENDABLE(false); - type_object->complete().struct_type().struct_flags().IS_MUTABLE(false); - // Not sure whether current type is nested or not, make all Type Nested - type_object->complete().struct_type().struct_flags().IS_NESTED(true); - type_object->complete().struct_type().struct_flags().IS_AUTOID_HASH(false); // Unsupported - - for (uint32_t i = 0; i < members->member_count_; ++i) { - CompleteStructMember cst_field; - cst_field.common().member_id(i); - cst_field.common().member_flags().TRY_CONSTRUCT1(false); // Unsupported - cst_field.common().member_flags().TRY_CONSTRUCT2(false); // Unsupported - cst_field.common().member_flags().IS_EXTERNAL(false); // Unsupported - cst_field.common().member_flags().IS_OPTIONAL(false); - cst_field.common().member_flags().IS_MUST_UNDERSTAND(false); - cst_field.common().member_flags().IS_KEY(false); - cst_field.common().member_flags().IS_DEFAULT(false); // Doesn't apply - - MemberIdentifierName pair = GetTypeIdentifier(members, i, true); - if (!pair.first) { - continue; - } - cst_field.common().member_type_id(*pair.first); - cst_field.detail().name(pair.second); - type_object->complete().struct_type().member_seq().emplace_back(cst_field); - } - - // Header - type_object->complete().struct_type().header().detail().type_name(type_name); - - TypeIdentifier identifier; - identifier._d(eprosima::fastrtps::types::EK_COMPLETE); - - SerializedPayload_t payload(static_cast( - CompleteStructType::getCdrSerializedSize(type_object->complete().struct_type()) + 4)); - - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload.data), payload.max_size); - - // Fixed endian (Page 221, EquivalenceHash definition of Extensible and Dynamic Topic Types for - // DDS document) - eprosima::fastcdr::Cdr ser( - fastbuffer, eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, - eprosima::fastcdr::CdrVersion::XCDRv1); // Object that serializes the data. - ser.set_encoding_flag(eprosima::fastcdr::PLAIN_CDR); - payload.encapsulation = CDR_LE; - - type_object->serialize(ser); - payload.length = - static_cast(ser.get_serialized_data_length()); // Get the serialized length - MD5 objectHash; - objectHash.update(reinterpret_cast(payload.data), payload.length); - objectHash.finalize(); - for (int i = 0; i < 14; ++i) { - identifier.equivalence_hash()[i] = objectHash.digest[i]; + TypeIdentifierPair struct_type_ids; + if (eprosima::fastdds::dds::RETCODE_OK == + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->type_object_registry(). + get_type_identifiers(type_name, struct_type_ids)) + { + return struct_type_ids; } - TypeObjectFactory::get_instance()->add_type_object(type_name, &identifier, type_object); - delete type_object; + StructTypeFlag struct_flags {TypeObjectUtils::build_struct_type_flag( + xtypes::ExtensibilityKind::FINAL, + false, false)}; + CompleteTypeDetail detail {TypeObjectUtils::build_complete_type_detail({}, {}, type_name)}; + CompleteStructHeader header {TypeObjectUtils::build_complete_struct_header({}, detail)}; + CompleteStructMemberSeq member_seq; - return TypeObjectFactory::get_instance()->get_type_object(type_name, true); -} - -template -const TypeObject * GetMinimalObject( - const std::string & type_name, - const MembersType * members) -{ - const TypeObject * c_type_object = - TypeObjectFactory::get_instance()->get_type_object(type_name, false); - if (c_type_object != nullptr) { - return c_type_object; - } - - TypeObject * type_object = new TypeObject(); - type_object->_d(eprosima::fastrtps::types::EK_MINIMAL); - type_object->minimal()._d(eprosima::fastrtps::types::TK_STRUCTURE); - type_object->minimal().struct_type().struct_flags().IS_FINAL(false); - type_object->minimal().struct_type().struct_flags().IS_APPENDABLE(false); - type_object->minimal().struct_type().struct_flags().IS_MUTABLE(false); - type_object->minimal().struct_type().struct_flags().IS_NESTED(true); - type_object->minimal().struct_type().struct_flags().IS_AUTOID_HASH(false); // Unsupported - - for (uint32_t i = 0; i < members->member_count_; ++i) { - MinimalStructMember mst_field; - mst_field.common().member_id(i); - mst_field.common().member_flags().TRY_CONSTRUCT1(false); // Unsupported - mst_field.common().member_flags().TRY_CONSTRUCT2(false); // Unsupported - mst_field.common().member_flags().IS_EXTERNAL(false); // Unsupported - mst_field.common().member_flags().IS_OPTIONAL(false); - mst_field.common().member_flags().IS_MUST_UNDERSTAND(false); - mst_field.common().member_flags().IS_KEY(false); - mst_field.common().member_flags().IS_DEFAULT(false); // Doesn't apply - - MemberIdentifierName pair = GetTypeIdentifier(members, i, false); - if (!pair.first) { + for (uint32_t i {0}; i < members->member_count_; ++i) { + MemberIdentifierName pair {GetTypeIdentifier(members, i)}; + if (eprosima::fastdds::dds::TK_NONE == pair.first._d()) { continue; } - mst_field.common().member_type_id(*pair.first); - MD5 field_hash(pair.second); - for (int i = 0; i < 4; ++i) { - mst_field.detail().name_hash()[i] = field_hash.digest[i]; + + TypeIdentifierPair type_ids; + type_ids.type_identifier1(pair.first); + StructMemberFlag member_flags {TypeObjectUtils::build_struct_member_flag( + xtypes::TryConstructFailAction::DISCARD, + false, false, false, false)}; + MemberId member_id {static_cast(i)}; + bool common_var {false}; + CommonStructMember member_common{TypeObjectUtils::build_common_struct_member( + member_id, member_flags, TypeObjectUtils::retrieve_complete_type_identifier( + type_ids, + common_var))}; + if (!common_var) { + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Structure %s member TypeIdentifier inconsistent.", + pair.second.c_str()); + return {}; } - type_object->minimal().struct_type().member_seq().emplace_back(mst_field); + CompleteMemberDetail member_detail {TypeObjectUtils::build_complete_member_detail( + pair.second, {}, {})}; + CompleteStructMember member {TypeObjectUtils::build_complete_struct_member( + member_common, member_detail)}; + TypeObjectUtils::add_complete_struct_member(member_seq, member); } - TypeIdentifier identifier; - identifier._d(eprosima::fastrtps::types::EK_MINIMAL); - - SerializedPayload_t payload( - static_cast( - MinimalStructType::getCdrSerializedSize(type_object->minimal().struct_type()) + 4)); - - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload.data), payload.max_size); - - // Fixed endian (Page 221, EquivalenceHash definition of Extensible and Dynamic Topic Types for - // DDS document) - eprosima::fastcdr::Cdr ser( - fastbuffer, eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, - eprosima::fastcdr::CdrVersion::XCDRv1); // Object that serializes the data. - ser.set_encoding_flag(eprosima::fastcdr::PLAIN_CDR); - payload.encapsulation = CDR_LE; - - type_object->serialize(ser); - payload.length = - static_cast(ser.get_serialized_data_length()); // Get the serialized length - MD5 objectHash; - objectHash.update(reinterpret_cast(payload.data), payload.length); - objectHash.finalize(); - for (int i = 0; i < 14; ++i) { - identifier.equivalence_hash()[i] = objectHash.digest[i]; + CompleteStructType struct_type {TypeObjectUtils::build_complete_struct_type( + struct_flags, header, member_seq)}; + if (eprosima::fastdds::dds::RETCODE_BAD_PARAMETER == + TypeObjectUtils::build_and_register_struct_type_object( + struct_type, + type_name, struct_type_ids)) + { + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "%s already registered in TypeObjectRegistry for a different type.", + type_name.c_str()); + return {}; } - TypeObjectFactory::get_instance()->add_type_object(type_name, &identifier, type_object); - delete type_object; - return TypeObjectFactory::get_instance()->get_type_object(type_name, false); + return struct_type_ids; } template -MemberIdentifierName GetTypeIdentifier(const MembersType * members, uint32_t index, bool complete) +MemberIdentifierName GetTypeIdentifier(const MembersType * members, uint32_t index) { const auto member = members->members_ + index; - const TypeIdentifier * type_identifier = nullptr; + TypeIdentifierPair type_identifiers; std::string name = member->name_; + xtypes::ITypeObjectRegistry& type_object_registry = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->type_object_registry(); std::string type_name; - bool complete_type = false; switch (member->type_id_) { case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: { - type_name = "float"; + type_name = "_float"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: { - type_name = "double"; + type_name = "_double"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: { - type_name = "char"; + type_name = "_char"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: { - type_name = "wchar"; + type_name = "_wchar_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: { - type_name = "bool"; + type_name = "_bool"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: { - type_name = "uint8_t"; + type_name = "_uint8_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: { - type_name = "int8_t"; + type_name = "_int8_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: { - type_name = "uint16_t"; + type_name = "_uint16_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: { - type_name = "int16_t"; + type_name = "_int16_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: { - type_name = "uint32_t"; + type_name = "_uint32_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: { - type_name = "int32_t"; + type_name = "_int32_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: { - type_name = "uint64_t"; + type_name = "_uint64_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: { - type_name = "int64_t"; + type_name = "_int64_t"; + type_object_registry.get_type_identifiers(type_name, type_identifiers); break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: { - uint32_t bound = member->string_upper_bound_ ? - static_cast(member->string_upper_bound_) : 255; - bool wide = - (member->type_id_ == ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) ? - false : true; - TypeObjectFactory::get_instance()->get_string_identifier(bound, wide); - type_name = TypeNamesGenerator::get_string_type_name( - bound, wide); + type_name = "anonymous_"; + if (member->type_id_ == ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) { + type_name += "string_"; + } else { + type_name += "wstring_"; + } + if (member->string_upper_bound_) { + type_name += std::to_string(static_cast(member->string_upper_bound_)); + } else { + type_name += "unbounded"; + } + + if (eprosima::fastdds::dds::RETCODE_OK != + type_object_registry.get_type_identifiers( + type_name, type_identifiers)) + { + if (255 < member->string_upper_bound_) { + xtypes::LBound bound = + static_cast(member->string_upper_bound_); + xtypes::StringLTypeDefn string_ldefn = + TypeObjectUtils::build_string_l_type_defn(bound); + TypeObjectUtils::build_and_register_l_string_type_identifier( + string_ldefn, + type_name, type_identifiers); + } else { + xtypes::SBound bound = + static_cast(member->string_upper_bound_); + xtypes::StringSTypeDefn string_sdefn = + TypeObjectUtils::build_string_s_type_defn(bound); + TypeObjectUtils::build_and_register_s_string_type_identifier( + string_sdefn, + type_name, type_identifiers); + } + } + break; } case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: @@ -495,132 +470,198 @@ MemberIdentifierName GetTypeIdentifier(const MembersType * members, uint32_t ind const MembersType * sub_members = static_cast(type_support_intro->data); std::string sub_type_name = _create_type_name(sub_members); - if (complete) { - GetCompleteObject(sub_type_name, sub_members); - } else { - GetMinimalObject(sub_type_name, sub_members); - } + type_identifiers = register_type_identifiers(sub_type_name, sub_members); type_name = sub_type_name; - complete_type = complete; } break; default: break; } - if (!type_name.empty()) { - if (!member->is_array_) { - type_identifier = TypeObjectFactory::get_instance()->get_type_identifier( - type_name, complete_type); - } else if (member->array_size_ && !member->is_upper_bound_) { - type_identifier = TypeObjectFactory::get_instance()->get_array_identifier( - type_name, {static_cast(member->array_size_)}, complete_type); + if (!type_name.empty() && member->is_array_) { + if (member->array_size_) { + if (!member->is_upper_bound_) { + std::string array_type_name {"anonymous_array_" + type_name + "_" + std::to_string( + member->array_size_)}; + if (eprosima::fastdds::dds::RETCODE_OK != + type_object_registry.get_type_identifiers( + array_type_name, type_identifiers)) + { + xtypes::TypeIdentifierPair element_type_identifiers; + if (eprosima::fastdds::dds::RETCODE_OK == + type_object_registry.get_type_identifiers(type_name, element_type_identifiers)) + { + xtypes::EquivalenceKind equiv_kind {xtypes::EK_COMPLETE}; + if (xtypes::TK_NONE == + element_type_identifiers.type_identifier2()._d()) + { + equiv_kind = xtypes::EK_BOTH; + } + xtypes::PlainCollectionHeader header {TypeObjectUtils:: + build_plain_collection_header(equiv_kind, 0)}; + bool ec = false; + TypeIdentifier * element_identifier = {new TypeIdentifier( + TypeObjectUtils::retrieve_complete_type_identifier( + element_type_identifiers, + ec))}; + if (255 < member->array_size_) { + xtypes::LBoundSeq array_bound_seq; + TypeObjectUtils::add_array_dimension( + array_bound_seq, + static_cast(member->array_size_)); + xtypes::PlainArrayLElemDefn array_ldefn {TypeObjectUtils:: + build_plain_array_l_elem_defn( + header, array_bound_seq, + eprosima::fastcdr::external(element_identifier))}; + TypeObjectUtils::build_and_register_l_array_type_identifier( + array_ldefn, + array_type_name, + type_identifiers); + } else { + xtypes::SBoundSeq array_bound_seq; + TypeObjectUtils::add_array_dimension( + array_bound_seq, + static_cast(member->array_size_)); + xtypes::PlainArraySElemDefn array_sdefn {TypeObjectUtils:: + build_plain_array_s_elem_defn( + header, array_bound_seq, + eprosima::fastcdr::external(element_identifier))}; + TypeObjectUtils::build_and_register_s_array_type_identifier( + array_sdefn, + array_type_name, + type_identifiers); + } + } + } + } else { + std::string sequence_type_name {"anonymous_sequence_" + type_name + "_" + std::to_string( + member->array_size_)}; + if (eprosima::fastdds::dds::RETCODE_OK != + type_object_registry.get_type_identifiers(sequence_type_name, type_identifiers)) + { + xtypes::TypeIdentifierPair element_type_identifiers; + if (eprosima::fastdds::dds::RETCODE_OK == + type_object_registry.get_type_identifiers(type_name, element_type_identifiers)) + { + xtypes::EquivalenceKind equiv_kind {xtypes::EK_COMPLETE}; + if (xtypes::TK_NONE == + element_type_identifiers.type_identifier2()._d()) + { + equiv_kind = xtypes::EK_BOTH; + } + xtypes::PlainCollectionHeader header {TypeObjectUtils:: + build_plain_collection_header(equiv_kind, 0)}; + bool ec = false; + TypeIdentifier * element_identifier = {new TypeIdentifier( + TypeObjectUtils::retrieve_complete_type_identifier( + element_type_identifiers, + ec))}; + if (255 < member->array_size_) { + xtypes::LBound bound {static_cast(member->array_size_)}; + xtypes::PlainSequenceLElemDefn seq_ldefn {TypeObjectUtils:: + build_plain_sequence_l_elem_defn( + header, bound, + eprosima::fastcdr::external(element_identifier))}; + TypeObjectUtils::build_and_register_l_sequence_type_identifier( + seq_ldefn, + sequence_type_name, + type_identifiers); + } else { + xtypes::SBound bound {static_cast(member->array_size_)}; + xtypes::PlainSequenceSElemDefn seq_sdefn {TypeObjectUtils:: + build_plain_sequence_s_elem_defn( + header, bound, + eprosima::fastcdr::external(element_identifier))}; + TypeObjectUtils::build_and_register_s_sequence_type_identifier( + seq_sdefn, + sequence_type_name, + type_identifiers); + } + } + } + } } else { - type_identifier = TypeObjectFactory::get_instance()->get_sequence_identifier( - type_name, 0, complete_type); + std::string sequence_type_name {"anonymous_sequence_" + type_name + "_unbounded"}; + if (eprosima::fastdds::dds::RETCODE_OK != + type_object_registry.get_type_identifiers(sequence_type_name, type_identifiers)) + { + xtypes::TypeIdentifierPair element_type_identifiers; + if (eprosima::fastdds::dds::RETCODE_OK == + type_object_registry.get_type_identifiers(type_name, element_type_identifiers)) + { + xtypes::EquivalenceKind equiv_kind {xtypes::EK_COMPLETE}; + if (xtypes::TK_NONE == element_type_identifiers.type_identifier2()._d()) + { + equiv_kind = xtypes::EK_BOTH; + } + xtypes::PlainCollectionHeader header {TypeObjectUtils:: + build_plain_collection_header(equiv_kind, 0)}; + bool ec = false; + TypeIdentifier * element_identifier = {new TypeIdentifier( + TypeObjectUtils::retrieve_complete_type_identifier( + element_type_identifiers, + ec))}; + xtypes::SBound bound {0}; + xtypes::PlainSequenceSElemDefn seq_sdefn {TypeObjectUtils:: + build_plain_sequence_s_elem_defn( + header, bound, + eprosima::fastcdr::external(element_identifier))}; + TypeObjectUtils::build_and_register_s_sequence_type_identifier( + seq_sdefn, + sequence_type_name, + type_identifiers); + } + } } } - return {type_identifier, name}; -} - -template -const TypeObject * GetTypeObject( - const std::string & type_name, bool complete, - const MembersType * members) -{ - const TypeObject * c_type_object = - TypeObjectFactory::get_instance()->get_type_object(type_name, complete); - if (c_type_object != nullptr) { - return c_type_object; - } else if (complete) { - return GetCompleteObject(type_name, members); - } - // else - return GetMinimalObject(type_name, members); + bool ec = false; + return {TypeObjectUtils::retrieve_complete_type_identifier(type_identifiers, ec), name}; } template -const TypeIdentifier * GetTypeIdentifier( - const std::string & type_name, bool complete, +TypeIdentifierPair GetTypeIdentifier( + const std::string & type_name, const MembersType * members) { - const TypeIdentifier * c_identifier = - TypeObjectFactory::get_instance()->get_type_identifier(type_name, complete); - if (c_identifier != nullptr && - (!complete || c_identifier->_d() == eprosima::fastrtps::types::EK_COMPLETE)) - { - return c_identifier; - } - - GetTypeObject(type_name, complete, members); // Generated inside - return TypeObjectFactory::get_instance()->get_type_identifier(type_name, complete); + // The following method tries to get the typeidentifiers and + // if not regeistered, it registers them. + return register_type_identifiers(type_name, members); } template -inline bool -add_type_object( +inline TypeIdentifierPair +register_type_identifiers( const void * untype_members, const std::string & type_name) { const MembersType * members = static_cast(untype_members); if (!members) { - return false; - } - - TypeObjectFactory * factory = TypeObjectFactory::get_instance(); - if (!factory) { - return false; - } - - const TypeIdentifier * identifier = nullptr; - const TypeObject * type_object = nullptr; - identifier = GetTypeIdentifier(type_name, true, members); - if (!identifier) { - return false; - } - type_object = GetTypeObject(type_name, true, members); - if (!type_object) { - return false; - } - - factory->add_type_object(type_name, identifier, type_object); - - identifier = GetTypeIdentifier(type_name, false, members); - if (!identifier) { - return false; - } - type_object = GetTypeObject(type_name, false, members); - if (!type_object) { - return false; + return {}; } - factory->add_type_object(type_name, identifier, type_object); - return true; + return GetTypeIdentifier(type_name, members); } -bool register_type_object( - const rosidl_message_type_support_t * type_supports, - const std::string & type_name) +void TypeSupport::register_type_object_representation() { const rosidl_message_type_support_t * type_support_intro = - get_type_support_introspection(type_supports); + get_type_support_introspection(type_supports_); if (!type_support_intro) { - return false; + return; } - bool ret = false; if (type_support_intro->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { - ret = add_type_object( - type_support_intro->data, type_name); + type_identifiers_ = + register_type_identifiers( + type_support_intro->data, get_name()); } else { - ret = add_type_object( - type_support_intro->data, type_name); + type_identifiers_ = + register_type_identifiers( + type_support_intro->data, get_name()); } - - return ret; } } // 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 49b90492b..415ea678e 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 "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.hpp" #include "rmw/types.h" @@ -21,15 +21,15 @@ rmw_gid_t rmw_fastrtps_shared_cpp::create_rmw_gid( - const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid) + const char * identifier, const eprosima::fastdds::rtps::GUID_t & guid) { rmw_gid_t rmw_gid = {}; rmw_gid.implementation_identifier = identifier; static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + sizeof(eprosima::fastdds::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, "RMW_GID_STORAGE_SIZE insufficient to store the fastrtps GUID_t." ); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + rmw_fastrtps_shared_cpp::copy_from_fastdds_guid_to_byte_array( guid, rmw_gid.data); return rmw_gid; diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index f6d4cfbe8..8d8308f95 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -42,10 +42,10 @@ void CustomDataWriterListener::on_publication_matched( if (status.current_count_change == 1) { publisher_event_->track_unique_subscription( - eprosima::fastrtps::rtps::iHandle2GUID(status.last_subscription_handle)); + eprosima::fastdds::rtps::iHandle2GUID(status.last_subscription_handle)); } else if (status.current_count_change == -1) { publisher_event_->untrack_unique_subscription( - eprosima::fastrtps::rtps::iHandle2GUID(status.last_subscription_handle)); + eprosima::fastdds::rtps::iHandle2GUID(status.last_subscription_handle)); } else { return; } @@ -277,13 +277,13 @@ void RMWPublisherEvent::set_on_new_event_callback( publisher_info_->data_writer_->set_listener(publisher_info_->data_writer_listener_, status_mask); } -void RMWPublisherEvent::track_unique_subscription(eprosima::fastrtps::rtps::GUID_t guid) +void RMWPublisherEvent::track_unique_subscription(eprosima::fastdds::rtps::GUID_t guid) { std::lock_guard lock(subscriptions_mutex_); subscriptions_.insert(guid); } -void RMWPublisherEvent::untrack_unique_subscription(eprosima::fastrtps::rtps::GUID_t guid) +void RMWPublisherEvent::untrack_unique_subscription(eprosima::fastdds::rtps::GUID_t guid) { std::lock_guard lock(subscriptions_mutex_); subscriptions_.erase(guid); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 1ab6523c9..66bb30ded 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -42,10 +42,10 @@ CustomDataReaderListener::on_subscription_matched( if (info.current_count_change == 1) { subscription_event_->track_unique_publisher( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + eprosima::fastdds::rtps::iHandle2GUID(info.last_publication_handle)); } else if (info.current_count_change == -1) { subscription_event_->untrack_unique_publisher( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + eprosima::fastdds::rtps::iHandle2GUID(info.last_publication_handle)); } else { return; } @@ -377,13 +377,13 @@ size_t RMWSubscriptionEvent::publisher_count() const return publishers_.size(); } -void RMWSubscriptionEvent::track_unique_publisher(eprosima::fastrtps::rtps::GUID_t guid) +void RMWSubscriptionEvent::track_unique_publisher(eprosima::fastdds::rtps::GUID_t guid) { std::lock_guard lock(publishers_mutex_); publishers_.insert(guid); } -void RMWSubscriptionEvent::untrack_unique_publisher(eprosima::fastrtps::rtps::GUID_t guid) +void RMWSubscriptionEvent::untrack_unique_publisher(eprosima::fastdds::rtps::GUID_t guid) { std::lock_guard lock(publishers_mutex_); publishers_.erase(guid); diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 4e1db5c3b..14bbef197 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -27,12 +27,12 @@ #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/Locator.h" -#include "fastdds/rtps/common/Property.h" -#include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" -#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" -#include "fastrtps/utils/IPLocator.h" +#include "fastdds/rtps/attributes/PropertyPolicy.hpp" +#include "fastdds/rtps/common/Locator.hpp" +#include "fastdds/rtps/common/Property.hpp" +#include "fastdds/rtps/transport/UDPv4TransportDescriptor.hpp" +#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.hpp" +#include "fastdds/utils/IPLocator.hpp" #include "rcpputils/scope_exit.hpp" #include "rcutils/env.h" @@ -176,7 +176,7 @@ rmw_fastrtps_shared_cpp::create_participant( // Clear the list of multicast listening locators domainParticipantQos.wire_protocol().builtin.metatrafficMulticastLocatorList.clear(); // Add a unicast locator to prevent creation of default multicast locator - eprosima::fastrtps::rtps::Locator_t default_unicast_locator; + eprosima::fastdds::rtps::Locator_t default_unicast_locator; domainParticipantQos.wire_protocol() .builtin.metatrafficUnicastLocatorList.push_back(default_unicast_locator); break; @@ -185,7 +185,7 @@ rmw_fastrtps_shared_cpp::create_participant( // Clear the list of multicast listening locators domainParticipantQos.wire_protocol().builtin.metatrafficMulticastLocatorList.clear(); // Add a unicast locator to prevent creation of default multicast locator - eprosima::fastrtps::rtps::Locator_t default_unicast_locator; + eprosima::fastdds::rtps::Locator_t default_unicast_locator; domainParticipantQos.wire_protocol() .builtin.metatrafficUnicastLocatorList.push_back(default_unicast_locator); // Disable built-in transports, since we are configuring our own. @@ -223,12 +223,12 @@ rmw_fastrtps_shared_cpp::create_participant( RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET == discovery_options->automatic_discovery_range) { for (size_t ii = 0; ii < discovery_options->static_peers_count; ++ii) { - eprosima::fastrtps::rtps::Locator_t peer; - auto response = eprosima::fastrtps::rtps::IPLocator::resolveNameDNS( + eprosima::fastdds::rtps::Locator_t peer; + auto response = eprosima::fastdds::rtps::IPLocator::resolveNameDNS( discovery_options->static_peers[ii].peer_address); // Get the first returned IPv4 if (response.first.size() > 0) { - eprosima::fastrtps::rtps::IPLocator::setIPv4(peer, response.first.begin()->data()); + eprosima::fastdds::rtps::IPLocator::setIPv4(peer, response.first.begin()->data()); } else { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "Unable to resolve peer %s\n", @@ -245,8 +245,8 @@ rmw_fastrtps_shared_cpp::create_participant( if (RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST == discovery_options->automatic_discovery_range) { // Add localhost as a static peer - eprosima::fastrtps::rtps::Locator_t peer; - eprosima::fastrtps::rtps::IPLocator::setIPv4(peer, "127.0.0.1"); + eprosima::fastdds::rtps::Locator_t peer; + eprosima::fastdds::rtps::IPLocator::setIPv4(peer, "127.0.0.1"); domainParticipantQos.wire_protocol().builtin.initialPeersList.push_back(peer); } @@ -255,8 +255,8 @@ rmw_fastrtps_shared_cpp::create_participant( domainParticipantQos.wire_protocol().builtin.initialPeersList.size()) { // Make sure we send an announcment on the multicast address - eprosima::fastrtps::rtps::Locator_t locator; - eprosima::fastrtps::rtps::IPLocator::setIPv4(locator, 239, 255, 0, 1); + eprosima::fastdds::rtps::Locator_t locator; + eprosima::fastdds::rtps::IPLocator::setIPv4(locator, 239, 255, 0, 1); domainParticipantQos.wire_protocol() .builtin.initialPeersList.push_back(locator); } @@ -309,9 +309,9 @@ rmw_fastrtps_shared_cpp::create_participant( // allow reallocation to support discovery messages bigger than 5000 bytes if (!leave_middleware_default_qos) { domainParticipantQos.wire_protocol().builtin.readerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; domainParticipantQos.wire_protocol().builtin.writerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } if (security_options->security_root_path) { // if security_root_path provided, try to find the key and certificate files @@ -320,7 +320,7 @@ rmw_fastrtps_shared_cpp::create_participant( if (rmw_dds_common::get_security_files( true, "file://", security_options->security_root_path, security_files_paths)) { - eprosima::fastrtps::rtps::PropertyPolicy property_policy; + eprosima::fastdds::rtps::PropertyPolicy property_policy; property_policy.properties().emplace_back( "dds.sec.auth.plugin", "builtin.PKI-DH"); property_policy.properties().emplace_back( @@ -386,7 +386,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant // Make the participant stop listening to discovery participant_info->participant_->set_listener(nullptr); - ReturnCode_t ret = ReturnCode_t::RETCODE_OK; + eprosima::fastdds::dds::ReturnCode_t ret = eprosima::fastdds::dds::RETCODE_OK; // Collect topics that should be deleted std::vector topics_to_remove; @@ -400,7 +400,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant participant_info->publisher_->delete_datawriter(writer); } ret = participant_info->participant_->delete_publisher(participant_info->publisher_); - if (ReturnCode_t::RETCODE_OK != ret) { + if (eprosima::fastdds::dds::RETCODE_OK != ret) { RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds publisher from participant"); } } @@ -414,7 +414,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant participant_info->subscriber_->delete_datareader(reader); } ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); - if (ReturnCode_t::RETCODE_OK != ret) { + if (eprosima::fastdds::dds::RETCODE_OK != ret) { RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds subscriber from participant"); } } @@ -434,7 +434,7 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant ret = participant_info->factory_->delete_participant( participant_info->participant_); - if (ReturnCode_t::RETCODE_OK != ret) { + if (eprosima::fastdds::dds::RETCODE_OK != ret) { RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete participant"); } diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index b73c2d83f..581dacd6e 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -40,8 +40,9 @@ destroy_publisher( auto info = static_cast(publisher->data); // Delete DataWriter - ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); - if (ReturnCode_t::RETCODE_OK != ret) { + eprosima::fastdds::dds::ReturnCode_t ret = participant_info->publisher_->delete_datawriter( + info->data_writer_); + if (eprosima::fastdds::dds::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 diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 1b1506151..c11bead55 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -39,9 +39,9 @@ is_rmw_duration_unspecified(const rmw_time_t & time) } rmw_time_t -dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) +dds_duration_to_rmw(const eprosima::fastdds::dds::Duration_t & duration) { - if (duration == eprosima::fastrtps::rtps::c_RTPSTimeInfinite) { + if (duration == eprosima::fastdds::rtps::c_RTPSTimeInfinite) { return RMW_DURATION_INFINITE; } rmw_time_t result = {(uint64_t)duration.seconds, (uint64_t)duration.nanosec}; @@ -143,7 +143,7 @@ bool fill_entity_qos_from_profile( // a8691a40be6b8460b01edde36ad8563170a3a35a/include/fastrtps/qos/QosPolicies.h#L223-L232 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.liveliness().announcement_period = eprosima::fastrtps::Duration_t(period_in_s); + entity_qos.liveliness().announcement_period = eprosima::fastdds::dds::Duration_t(period_in_s); } return true; @@ -184,8 +184,8 @@ get_datareader_qos( if (fill_data_entity_qos_from_profile(qos_policies, type_hash, datareader_qos)) { // The type support in the RMW implementation is always XCDR1. constexpr auto rep = eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION; - datareader_qos.type_consistency().representation.clear(); - datareader_qos.type_consistency().representation.m_value.push_back(rep); + datareader_qos.representation().clear(); + datareader_qos.representation().m_value.push_back(rep); return true; } @@ -223,37 +223,6 @@ is_valid_qos(const rmw_qos_profile_t & /* qos_policies */) return true; } -template -void -dds_attributes_to_rmw_qos( - const AttributeT & dds_qos, - rmw_qos_profile_t * qos) -{ - switch (dds_qos.topic.historyQos.kind) { - case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: - qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - break; - case eprosima::fastrtps::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.topic.historyQos.depth); - rtps_qos_to_rmw_qos(dds_qos.qos, qos); -} - -template -void dds_attributes_to_rmw_qos( - const eprosima::fastrtps::PublisherAttributes & dds_qos, - rmw_qos_profile_t * qos); - -template -void dds_attributes_to_rmw_qos( - const eprosima::fastrtps::SubscriberAttributes & dds_qos, - rmw_qos_profile_t * qos); - template void dds_qos_to_rmw_qos( const eprosima::fastdds::dds::DataWriterQos & dds_qos, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 1e583abcd..9ed38f969 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -70,8 +70,9 @@ __rmw_destroy_client( std::lock_guard lck(participant_info->entity_creation_mutex_); // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); - if (ret != ReturnCode_t::RETCODE_OK) { + eprosima::fastdds::dds::ReturnCode_t ret = participant_info->subscriber_->delete_datareader( + info->response_reader_); + if (ret != eprosima::fastdds::dds::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); final_ret = RMW_RET_ERROR; @@ -85,7 +86,7 @@ __rmw_destroy_client( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); - if (ret != ReturnCode_t::RETCODE_OK) { + if (ret != eprosima::fastdds::dds::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); final_ret = RMW_RET_ERROR; 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 2a902a60f..dc3d37bfc 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 "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.hpp" #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" @@ -45,7 +45,7 @@ __rmw_compare_gids_equal( RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); *result = - memcmp(gid1->data, gid2->data, sizeof(eprosima::fastrtps::rtps::GUID_t)) == 0; + memcmp(gid1->data, gid2->data, sizeof(eprosima::fastdds::rtps::GUID_t)) == 0; return RMW_RET_OK; } 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 73e6d3769..97f5fa857 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 @@ -19,15 +19,15 @@ #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "fastrtps/utils/IPLocator.h" +#include "fastdds/utils/IPLocator.hpp" namespace rmw_fastrtps_shared_cpp { -using Locator_t = eprosima::fastrtps::rtps::Locator_t; -using LocatorList_t = eprosima::fastrtps::rtps::LocatorList_t; -using IPLocator = eprosima::fastrtps::rtps::IPLocator; +using Locator_t = eprosima::fastdds::rtps::Locator_t; +using LocatorList_t = eprosima::fastdds::rtps::LocatorList_t; +using IPLocator = eprosima::fastdds::rtps::IPLocator; rmw_ret_t fill_network_flow_endpoint(rmw_network_flow_endpoint_t *, const Locator_t &); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_client.cpp index 18b7773a9..3a8449b0c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_client.cpp @@ -41,7 +41,7 @@ __rmw_get_gid_for_client( // Use client's reader guid instead of writer guid for service event, // because service server uses client's reader guid for the event. // Service event message requires gid must be unique during transaction. - copy_from_fastrtps_guid_to_byte_array(info->reader_guid_, gid->data); + copy_from_fastdds_guid_to_byte_array(info->reader_guid_, gid->data); gid->implementation_identifier = identifier; return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp index 51f6003cc..4ce1dffcf 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp @@ -15,7 +15,7 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" -#include "fastdds/rtps/common/Time_t.h" +#include "fastdds/rtps/common/Time_t.hpp" #include "rmw/allocators.h" #include "rmw/error_handling.h" @@ -56,13 +56,15 @@ __rmw_publish( RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR); rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; data.data = const_cast(ros_message); data.impl = info->type_support_impl_; - eprosima::fastrtps::Time_t stamp; - eprosima::fastrtps::Time_t::now(stamp); + eprosima::fastdds::dds::Time_t stamp; + eprosima::fastdds::dds::Time_t::now(stamp); TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_message, stamp.to_ns()); - if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) { + if (eprosima::fastdds::dds::RETCODE_OK != info->data_writer_->write_w_timestamp(&data, + eprosima::fastdds::dds::HANDLE_NIL, stamp)) + { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; } @@ -106,13 +108,15 @@ __rmw_publish_serialized_message( } rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER; data.data = &ser; - data.impl = nullptr; // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER - eprosima::fastrtps::Time_t stamp; - eprosima::fastrtps::Time_t::now(stamp); + data.impl = nullptr; // not used when type is FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER + eprosima::fastdds::dds::Time_t stamp; + eprosima::fastdds::dds::Time_t::now(stamp); TRACETOOLS_TRACEPOINT(rmw_publish, publisher, serialized_message, stamp.to_ns()); - if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) { + if (eprosima::fastdds::dds::RETCODE_OK != info->data_writer_->write_w_timestamp(&data, + eprosima::fastdds::dds::HANDLE_NIL, stamp)) + { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; } @@ -143,10 +147,10 @@ __rmw_publish_loaned_message( RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT); auto info = static_cast(publisher->data); - eprosima::fastrtps::Time_t stamp; - eprosima::fastrtps::Time_t::now(stamp); + eprosima::fastdds::dds::Time_t stamp; + eprosima::fastdds::dds::Time_t::now(stamp); TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_message, stamp.to_ns()); - if (!info->data_writer_->write_w_timestamp( + if (eprosima::fastdds::dds::RETCODE_OK != info->data_writer_->write_w_timestamp( const_cast(ros_message), eprosima::fastdds::dds::HANDLE_NIL, stamp)) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index f499fcd0d..8ba6aba54 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -130,10 +130,10 @@ __rmw_publisher_wait_for_all_acked( auto info = static_cast(publisher->data); - eprosima::fastrtps::Duration_t timeout = rmw_time_to_fastrtps(wait_timeout); + eprosima::fastdds::dds::Duration_t timeout = rmw_time_to_fastrtps(wait_timeout); - ReturnCode_t ret = info->data_writer_->wait_for_acknowledgments(timeout); - if (ReturnCode_t::RETCODE_OK == ret) { + eprosima::fastdds::dds::ReturnCode_t ret = info->data_writer_->wait_for_acknowledgments(timeout); + if (eprosima::fastdds::dds::RETCODE_OK == ret) { return RMW_RET_OK; } @@ -177,7 +177,7 @@ __rmw_borrow_loaned_message( } auto info = static_cast(publisher->data); - if (!info->data_writer_->loan_sample(*ros_message)) { + if (eprosima::fastdds::dds::RETCODE_OK != info->data_writer_->loan_sample(*ros_message)) { return RMW_RET_ERROR; } @@ -202,7 +202,7 @@ __rmw_return_loaned_message_from_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); auto info = static_cast(publisher->data); - if (!info->data_writer_->discard_loan(loaned_message)) { + if (eprosima::fastdds::dds::RETCODE_OK != info->data_writer_->discard_loan(loaned_message)) { return RMW_RET_ERROR; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 5cf04f46d..bfcff5e0b 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 "fastdds/rtps/common/WriteParams.h" +#include "fastdds/rtps/common/WriteParams.hpp" #include "fastdds/dds/core/StackAllocatedSequence.hpp" #include "rmw/error_handling.h" @@ -53,13 +53,13 @@ __rmw_send_request( auto info = static_cast(client->data); assert(info); - eprosima::fastrtps::rtps::WriteParams wparams; + eprosima::fastdds::rtps::WriteParams wparams; rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; 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_writer_->write(&data, wparams)) { + if (eprosima::fastdds::dds::RETCODE_OK == 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; @@ -98,26 +98,28 @@ __rmw_take_request( if (request.buffer_ != nullptr) { rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER; data.data = request.buffer_; - data.impl = nullptr; // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER + data.impl = nullptr; // not used when type is FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER eprosima::fastdds::dds::StackAllocatedSequence data_values; const_cast(data_values.buffer())[0] = &data; eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - if (ReturnCode_t::RETCODE_OK == info->request_reader_->take(data_values, info_seq, 1)) { + if (eprosima::fastdds::dds::RETCODE_OK == + info->request_reader_->take(data_values, info_seq, 1)) + { if (info_seq[0].valid_data) { request.sample_identity_ = info_seq[0].sample_identity; // Use response subscriber guid (on related_sample_identity) when present. - const eprosima::fastrtps::rtps::GUID_t & reader_guid = + const eprosima::fastdds::rtps::GUID_t & reader_guid = info_seq[0].related_sample_identity.writer_guid(); - if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown()) { + if (reader_guid != eprosima::fastdds::rtps::GUID_t::unknown()) { request.sample_identity_.writer_guid() = reader_guid; } // Save both guids in the clients_endpoints map - const eprosima::fastrtps::rtps::GUID_t & writer_guid = + const eprosima::fastdds::rtps::GUID_t & writer_guid = info_seq[0].sample_identity.writer_guid(); info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); @@ -128,7 +130,7 @@ __rmw_take_request( deser, ros_request, info->request_type_support_impl_)) { // Get header - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + rmw_fastrtps_shared_cpp::copy_from_fastdds_guid_to_byte_array( request.sample_identity_.writer_guid(), request_header->request_id.writer_guid); request_header->request_id.sequence_number = diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index f894998e9..1f28d830d 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 "fastdds/rtps/common/WriteParams.h" +#include "fastdds/rtps/common/WriteParams.hpp" #include "fastdds/dds/core/StackAllocatedSequence.hpp" #include "rmw/error_handling.h" @@ -58,15 +58,17 @@ __rmw_take_response( // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER; data.data = response.buffer_.get(); - data.impl = nullptr; // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER + data.impl = nullptr; // not used when type is FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER eprosima::fastdds::dds::StackAllocatedSequence data_values; const_cast(data_values.buffer())[0] = &data; eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - if (ReturnCode_t::RETCODE_OK == info->response_reader_->take(data_values, info_seq, 1)) { + if (eprosima::fastdds::dds::RETCODE_OK == + info->response_reader_->take(data_values, info_seq, 1)) + { if (info_seq[0].valid_data) { response.sample_identity_ = info_seq[0].related_sample_identity; @@ -116,8 +118,8 @@ __rmw_send_response( auto info = static_cast(service->data); assert(info); - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + eprosima::fastdds::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastdds_guid( request_header->writer_guid, &wparams.related_sample_identity().writer_guid()); wparams.related_sample_identity().sequence_number().high = @@ -135,7 +137,7 @@ __rmw_send_response( // readers will have this bit on, while writers will not. We use this to know // if the related guid is the request writer or the response reader. constexpr uint8_t entity_id_is_reader_bit = 0x04; - const eprosima::fastrtps::rtps::GUID_t & related_guid = + const eprosima::fastdds::rtps::GUID_t & related_guid = wparams.related_sample_identity().writer_guid(); if ((related_guid.entityId.value[3] & entity_id_is_reader_bit) != 0) { // Related guid is a reader, so it is the response subscription guid. @@ -156,10 +158,10 @@ __rmw_send_response( } rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; data.data = const_cast(ros_response); data.impl = info->response_type_support_impl_; - if (info->response_writer_->write(&data, wparams)) { + if (eprosima::fastdds::dds::RETCODE_OK == 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 de37aa8fd..87c0210b5 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp @@ -19,10 +19,10 @@ #include #include -#include "fastdds/rtps/common/Property.h" -#include "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/common/Property.hpp" +#include "fastdds/rtps/attributes/PropertyPolicy.hpp" -#include "fastrtps/config.h" +#include "fastdds/config.hpp" #include "rcutils/env.h" #include "rcutils/filesystem.h" @@ -105,8 +105,8 @@ bool validate_boolean(const std::string & str) } void add_property( - eprosima::fastrtps::rtps::PropertySeq & properties, - eprosima::fastrtps::rtps::Property && property) + eprosima::fastdds::rtps::PropertySeq & properties, + eprosima::fastdds::rtps::Property && property) { // Add property to vector. If property already exists, overwrite it. std::string property_name = property.name(); @@ -140,10 +140,10 @@ bool get_env(const std::string & variable_name, std::string & variable_value) #endif -bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPolicy & policy) +bool apply_security_logging_configuration(eprosima::fastdds::rtps::PropertyPolicy & policy) { #if HAVE_SECURITY - eprosima::fastrtps::rtps::PropertySeq properties; + eprosima::fastdds::rtps::PropertySeq properties; std::string env_value; // Handle logging to file @@ -153,7 +153,7 @@ bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPoli if (!env_value.empty()) { add_property( properties, - eprosima::fastrtps::rtps::Property( + eprosima::fastdds::rtps::Property( log_file_property_name, env_value.c_str())); } @@ -172,7 +172,7 @@ bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPoli add_property( properties, - eprosima::fastrtps::rtps::Property( + eprosima::fastdds::rtps::Property( distribute_enable_property_name, env_value.c_str())); } @@ -196,13 +196,13 @@ bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPoli add_property( properties, - eprosima::fastrtps::rtps::Property(verbosity_property_name, verbosity.c_str())); + eprosima::fastdds::rtps::Property(verbosity_property_name, verbosity.c_str())); } if (!properties.empty()) { add_property( properties, - eprosima::fastrtps::rtps::Property( + eprosima::fastdds::rtps::Property( logging_plugin_property_name, "builtin.DDS_LogTopic")); } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index d071b5220..f18db3fd8 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -83,8 +83,9 @@ __rmw_destroy_service( std::lock_guard lck(participant_info->entity_creation_mutex_); // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); - if (ret != ReturnCode_t::RETCODE_OK) { + eprosima::fastdds::dds::ReturnCode_t ret = + participant_info->subscriber_->delete_datareader(info->request_reader_); + if (ret != eprosima::fastdds::dds::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datareader"); final_ret = RMW_RET_ERROR; @@ -99,7 +100,7 @@ __rmw_destroy_service( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->response_writer_); - if (ret != ReturnCode_t::RETCODE_OK) { + if (ret != eprosima::fastdds::dds::RETCODE_OK) { show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datawriter"); final_ret = RMW_RET_ERROR; 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 3e52ec1a0..ab5ea733a 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 @@ -15,7 +15,7 @@ #include -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/dds/subscriber/Subscriber.hpp" #include "rcutils/logging_macros.h" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 218b0ff38..98e471563 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -125,9 +125,9 @@ __rmw_subscription_set_content_filter( expression_parameters.push_back(options->expression_parameters.data[i]); } - ReturnCode_t ret = + eprosima::fastdds::dds::ReturnCode_t ret = filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) { + if (ret != eprosima::fastdds::dds::RETCODE_OK) { RMW_SET_ERROR_MSG( "failed to set_filter_expression"); return RMW_RET_ERROR; @@ -230,8 +230,9 @@ __rmw_subscription_get_content_filter( return RMW_RET_ERROR; } std::vector expression_parameters; - ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) { + eprosima::fastdds::dds::ReturnCode_t ret = + filtered_topic->get_expression_parameters(expression_parameters); + if (ret != eprosima::fastdds::dds::RETCODE_OK) { RMW_SET_ERROR_MSG( "failed to get_expression_parameters"); return RMW_RET_ERROR; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 44d9d86e5..58abd88d6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -22,8 +22,7 @@ #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/core/StackAllocatedSequence.hpp" - -#include "fastrtps/utils/collections/ResourceLimitedVector.hpp" +#include "fastdds/utils/collections/ResourceLimitedVector.hpp" #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" @@ -61,7 +60,7 @@ _assign_message_info( sender_gid->implementation_identifier = identifier; memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + rmw_fastrtps_shared_cpp::copy_from_fastdds_guid_to_byte_array( sinfo->sample_identity.writer_guid(), sender_gid->data); } @@ -87,7 +86,7 @@ _take( RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_ROS_MESSAGE; data.data = ros_message; data.impl = info->type_support_impl_; @@ -95,7 +94,7 @@ _take( const_cast(data_values.buffer())[0] = &data; eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { + while (eprosima::fastdds::dds::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { // The info->data_reader_->take() call already modified the ros_message arg // See rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -108,7 +107,7 @@ _take( if (subscription->options.ignore_local_publications) { auto sample_writer_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info_seq[0].publication_handle); + eprosima::fastdds::rtps::iHandle2GUID(info_seq[0].publication_handle); if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { // This is a local publication. Ignore it @@ -314,15 +313,15 @@ _take_serialized_message( eprosima::fastcdr::FastBuffer buffer; rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER; data.data = &buffer; - data.impl = nullptr; // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER + data.impl = nullptr; // not used when type is FASTDDS_SERIALIZED_DATA_TYPE_CDR_BUFFER eprosima::fastdds::dds::StackAllocatedSequence data_values; const_cast(data_values.buffer())[0] = &data; eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { + while (eprosima::fastdds::dds::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { auto reset = rcpputils::make_scope_exit( [&]() { @@ -426,15 +425,15 @@ _take_dynamic_message( eprosima::fastcdr::FastBuffer buffer; rmw_fastrtps_shared_cpp::SerializedData data; - data.type = FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE; + data.type = FASTDDS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE; data.data = dynamic_data->impl.handle; - data.impl = nullptr; // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE + data.impl = nullptr; // not used when type is FASTDDS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE eprosima::fastdds::dds::StackAllocatedSequence data_values; const_cast(data_values.buffer())[0] = &data; eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { + while (eprosima::fastdds::dds::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { // The info->data_reader_->take() call already modified the dynamic_data arg // See rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -526,7 +525,7 @@ struct LoanManager }; explicit LoanManager( - const eprosima::fastrtps::ResourceLimitedContainerConfig & items_cfg) + const eprosima::fastdds::ResourceLimitedContainerConfig & items_cfg) : items(items_cfg) { } @@ -557,7 +556,7 @@ struct LoanManager private: std::mutex mtx; - using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; + using ItemVector = eprosima::fastdds::ResourceLimitedVector>; ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); }; @@ -567,7 +566,9 @@ __init_subscription_for_loans( { auto info = static_cast(subscription->data); const auto & qos = info->data_reader_->get_qos(); - subscription->can_loan_messages = info->type_support_->is_plain(); + // The type support in the RMW implementation is always XCDR1. + subscription->can_loan_messages = + info->type_support_->is_plain(eprosima::fastdds::dds::XCDR_DATA_REPRESENTATION); if (subscription->can_loan_messages) { const auto & allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; info->loan_manager_ = std::make_shared(allocation_qos); @@ -598,7 +599,9 @@ __rmw_take_loaned_message_internal( auto item = std::make_unique(); - while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) { + while (eprosima::fastdds::dds::RETCODE_OK == + info->data_reader_->take(item->data_seq, item->info_seq, 1)) + { if (item->info_seq[0].valid_data) { if (nullptr != message_info) { _assign_message_info(identifier, message_info, &item->info_seq[0]); @@ -640,7 +643,8 @@ __rmw_return_loaned_message_from_subscription( std::unique_ptr item; item = info->loan_manager_->erase_item(loaned_message); if (item != nullptr) { - if (!info->data_reader_->return_loan(item->data_seq, item->info_seq)) { + auto ret_code = info->data_reader_->return_loan(item->data_seq, item->info_seq); + if (eprosima::fastdds::dds::RETCODE_OK != ret_code) { RMW_SET_ERROR_MSG("Error returning loan"); return RMW_RET_ERROR; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index dc6cc0aed..495bbcb9b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -80,7 +80,7 @@ static bool has_triggered_condition( void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == + if (eprosima::fastdds::dds::RETCODE_OK == custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { return true; @@ -93,7 +93,7 @@ static bool has_triggered_condition( void * data = clients->clients[i]; auto custom_client_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == + if (eprosima::fastdds::dds::RETCODE_OK == custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { return true; @@ -106,7 +106,7 @@ static bool has_triggered_condition( void * data = services->services[i]; auto custom_service_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == + if (eprosima::fastdds::dds::RETCODE_OK == custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { return true; @@ -209,13 +209,13 @@ __rmw_wait( Duration_t timeout = (wait_timeout) ? Duration_t{static_cast(wait_timeout->sec), - static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; + static_cast(wait_timeout->nsec)} : eprosima::fastdds::dds::c_TimeInfinite; eprosima::fastdds::dds::ConditionSeq triggered_conditions; - ReturnCode_t ret_code = fastdds_wait_set->wait( + eprosima::fastdds::dds::ReturnCode_t ret_code = fastdds_wait_set->wait( triggered_conditions, timeout); - wait_result = (ret_code == ReturnCode_t::RETCODE_OK); + wait_result = (ret_code == eprosima::fastdds::dds::RETCODE_OK); // Detach all of the conditions from the wait set. // TODO(mjcarroll): When upstream has the ability to detach a vector of conditions, @@ -232,7 +232,7 @@ __rmw_wait( auto custom_subscriber_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != + if (eprosima::fastdds::dds::RETCODE_OK != custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { subscriptions->subscribers[i] = 0; @@ -246,7 +246,7 @@ __rmw_wait( auto custom_client_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != + if (eprosima::fastdds::dds::RETCODE_OK != custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { clients->clients[i] = 0; @@ -260,7 +260,7 @@ __rmw_wait( auto custom_service_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != + if (eprosima::fastdds::dds::RETCODE_OK != custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { services->services[i] = 0; diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index 3305a71e3..a3cc67584 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -47,8 +47,9 @@ destroy_subscription( auto info = static_cast(subscription->data); // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->data_reader_); - if (ReturnCode_t::RETCODE_OK != ret) { + eprosima::fastdds::dds::ReturnCode_t ret = + participant_info->subscriber_->delete_datareader(info->data_reader_); + if (eprosima::fastdds::dds::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 diff --git a/rmw_fastrtps_shared_cpp/src/time_utils.cpp b/rmw_fastrtps_shared_cpp/src/time_utils.cpp index 2c534e72e..50b0c58bb 100644 --- a/rmw_fastrtps_shared_cpp/src/time_utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/time_utils.cpp @@ -14,20 +14,22 @@ #include "rmw_dds_common/time_utils.hpp" +#include "fastdds/rtps/common/Time_t.hpp" + #include "time_utils.hpp" namespace rmw_fastrtps_shared_cpp { -eprosima::fastrtps::Duration_t +eprosima::fastdds::dds::Duration_t rmw_time_to_fastrtps(const rmw_time_t & time) { if (rmw_time_equal(time, RMW_DURATION_INFINITE)) { - return eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + return eprosima::fastdds::rtps::c_RTPSTimeInfinite.to_duration_t(); } rmw_time_t clamped_time = rmw_dds_common::clamp_rmw_time_to_dds_time(time); - return eprosima::fastrtps::Duration_t( + return eprosima::fastdds::dds::Duration_t( static_cast(clamped_time.sec), static_cast(clamped_time.nsec)); } diff --git a/rmw_fastrtps_shared_cpp/src/time_utils.hpp b/rmw_fastrtps_shared_cpp/src/time_utils.hpp index f22a37c84..eedf2436a 100644 --- a/rmw_fastrtps_shared_cpp/src/time_utils.hpp +++ b/rmw_fastrtps_shared_cpp/src/time_utils.hpp @@ -15,12 +15,12 @@ #ifndef TIME_UTILS_HPP_ #define TIME_UTILS_HPP_ -#include "fastdds/rtps/common/Time_t.h" +#include "fastdds/dds/core/Time_t.hpp" namespace rmw_fastrtps_shared_cpp { -eprosima::fastrtps::Duration_t rmw_time_to_fastrtps(const rmw_time_t & time); +eprosima::fastdds::dds::Duration_t rmw_time_to_fastrtps(const rmw_time_t & time); } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 47429680a..53ee6b381 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -18,36 +18,33 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" +#include "fastdds/dds/core/ReturnCode.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; - const char * const CONTENT_FILTERED_TOPIC_POSTFIX = "_filtered_name"; namespace rmw_fastrtps_shared_cpp { -rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) +rmw_ret_t cast_error_dds_to_rmw(eprosima::fastdds::dds::ReturnCode_t code) { // not switch because it is not an enum class - if (ReturnCode_t::RETCODE_OK == code) { + if (eprosima::fastdds::dds::RETCODE_OK == code) { return RMW_RET_OK; - } else if (ReturnCode_t::RETCODE_ERROR == code) { + } else if (eprosima::fastdds::dds::RETCODE_ERROR == code) { // repeats the error to avoid too many 'if' comparisons return RMW_RET_ERROR; - } else if (ReturnCode_t::RETCODE_TIMEOUT == code) { + } else if (eprosima::fastdds::dds::RETCODE_TIMEOUT == code) { return RMW_RET_TIMEOUT; - } else if (ReturnCode_t::RETCODE_UNSUPPORTED == code) { + } else if (eprosima::fastdds::dds::RETCODE_UNSUPPORTED == code) { return RMW_RET_UNSUPPORTED; - } else if (ReturnCode_t::RETCODE_BAD_PARAMETER == code) { + } else if (eprosima::fastdds::dds::RETCODE_BAD_PARAMETER == code) { return RMW_RET_INVALID_ARGUMENT; - } else if (ReturnCode_t::RETCODE_OUT_OF_RESOURCES == code) { + } else if (eprosima::fastdds::dds::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 { @@ -149,7 +146,7 @@ create_datareader( case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: // Ensure we request unique network flow endpoints - using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; + using PropertyPolicyHelper = eprosima::fastdds::rtps::PropertyPolicyHelper; if (nullptr == PropertyPolicyHelper::find_property( updated_qos.properties(), diff --git a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt index d4643de1b..73cd13d3a 100644 --- a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt @@ -1,11 +1,6 @@ find_package(ament_cmake_gtest REQUIRED) find_package(osrf_testing_tools_cpp REQUIRED) -ament_add_gtest(test_dds_attributes_to_rmw_qos test_dds_attributes_to_rmw_qos.cpp) -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) target_link_libraries(test_dds_qos_to_rmw_qos ${PROJECT_NAME}) diff --git a/rmw_fastrtps_shared_cpp/test/test_dds_attributes_to_rmw_qos.cpp b/rmw_fastrtps_shared_cpp/test/test_dds_attributes_to_rmw_qos.cpp deleted file mode 100644 index 91716863d..000000000 --- a/rmw_fastrtps_shared_cpp/test/test_dds_attributes_to_rmw_qos.cpp +++ /dev/null @@ -1,160 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "gtest/gtest.h" - -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" - -#include "rmw_fastrtps_shared_cpp/qos.hpp" - -using eprosima::fastrtps::PublisherAttributes; -using eprosima::fastrtps::SubscriberAttributes; -static const eprosima::fastrtps::Duration_t InfiniteDuration = - eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); - -class DDSAttributesToRMWQosTest : public ::testing::Test -{ -protected: - rmw_qos_profile_t qos_profile_ {}; - PublisherAttributes publisher_attributes_ {}; - SubscriberAttributes subscriber_attributes_ {}; -}; - - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_depth_conversion) { - publisher_attributes_.topic.historyQos.depth = 0; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.depth, 0u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_history_conversion) { - publisher_attributes_.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_ALL); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_durability_conversion) { - publisher_attributes_.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_UNKNOWN); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_reliability_conversion) { - publisher_attributes_.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_liveliness_conversion) { - publisher_attributes_.qos.m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_liveliness_lease_duration_conversion) { - publisher_attributes_.qos.m_liveliness.lease_duration = {8, 78901234}; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 8u); - EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 78901234u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_deadline_conversion) { - publisher_attributes_.qos.m_deadline.period = {12, 1234}; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.deadline.sec, 12u); - EXPECT_EQ(qos_profile_.deadline.nsec, 1234u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_publisher_lifespan_conversion) { - publisher_attributes_.qos.m_lifespan.duration = {19, 5432}; - dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.lifespan.sec, 19u); - EXPECT_EQ(qos_profile_.lifespan.nsec, 5432u); -} - - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_depth_conversion) { - subscriber_attributes_.topic.historyQos.depth = 1; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.depth, 1u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_history_conversion) { - subscriber_attributes_.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_durability_conversion) { - subscriber_attributes_.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_reliability_conversion) { - subscriber_attributes_.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_liveliness_conversion) { - subscriber_attributes_.qos.m_liveliness.kind = - eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_UNKNOWN); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_liveliness_lease_duration_conversion) { - subscriber_attributes_.qos.m_liveliness.lease_duration = {80, 34567}; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 80u); - EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 34567u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_deadline_conversion) { - subscriber_attributes_.qos.m_deadline.period = {1, 3324}; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.deadline.sec, 1u); - EXPECT_EQ(qos_profile_.deadline.nsec, 3324u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_lifespan_conversion) { - subscriber_attributes_.qos.m_lifespan.duration = {9, 432}; - dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); - EXPECT_EQ(qos_profile_.lifespan.sec, 9u); - EXPECT_EQ(qos_profile_.lifespan.nsec, 432u); -} - -TEST_F(DDSAttributesToRMWQosTest, test_subscriber_infinite_duration_conversions) { - subscriber_attributes_.qos.m_lifespan.duration = InfiniteDuration; - subscriber_attributes_.qos.m_deadline.period = InfiniteDuration; - subscriber_attributes_.qos.m_liveliness.lease_duration = InfiniteDuration; - dds_attributes_to_rmw_qos(subscriber_attributes_, &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(DDSAttributesToRMWQosTest, test_publisher_infinite_duration_conversions) { - publisher_attributes_.qos.m_lifespan.duration = InfiniteDuration; - publisher_attributes_.qos.m_deadline.period = InfiniteDuration; - publisher_attributes_.qos.m_liveliness.lease_duration = InfiniteDuration; - dds_attributes_to_rmw_qos(publisher_attributes_, &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_dds_qos_to_rmw_qos.cpp b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp index 88aef4e5d..1b1c2f551 100644 --- a/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_dds_qos_to_rmw_qos.cpp @@ -24,8 +24,8 @@ 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(); +static const eprosima::fastdds::dds::Duration_t InfiniteDuration = + eprosima::fastdds::rtps::c_RTPSTimeInfinite.to_duration_t(); class DDSQosToRMWQosTest : public ::testing::Test { diff --git a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp index 3bba2a29c..7ec9ce445 100644 --- a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp @@ -15,26 +15,26 @@ #include "gtest/gtest.h" #include "fastdds/rtps/common/EntityId_t.hpp" -#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.hpp" #include "fastdds/rtps/common/GuidPrefix_t.hpp" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -using rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid; -using rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array; +using rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastdds_guid; +using rmw_fastrtps_shared_cpp::copy_from_fastdds_guid_to_byte_array; static constexpr size_t byte_array_size = - eprosima::fastrtps::rtps::GuidPrefix_t::size + - eprosima::fastrtps::rtps::EntityId_t::size; + eprosima::fastdds::rtps::GuidPrefix_t::size + + eprosima::fastdds::rtps::EntityId_t::size; TEST(GUIDUtilsTest, bad_arguments) { #ifndef NDEBUG - eprosima::fastrtps::rtps::GUID_t guid; + eprosima::fastdds::rtps::GUID_t guid; uint8_t byte_array[byte_array_size] = {0}; uint8_t * null_byte_array = nullptr; - EXPECT_DEATH(copy_from_byte_array_to_fastrtps_guid(byte_array, nullptr), ""); - EXPECT_DEATH(copy_from_byte_array_to_fastrtps_guid(null_byte_array, &guid), ""); - EXPECT_DEATH(copy_from_fastrtps_guid_to_byte_array(guid, null_byte_array), ""); + EXPECT_DEATH(copy_from_byte_array_to_fastdds_guid(byte_array, nullptr), ""); + EXPECT_DEATH(copy_from_byte_array_to_fastdds_guid(null_byte_array, &guid), ""); + EXPECT_DEATH(copy_from_fastdds_guid_to_byte_array(guid, null_byte_array), ""); #endif } @@ -42,21 +42,21 @@ TEST(GUIDUtilsTest, byte_array_to_guid_and_back) { uint8_t input_byte_array[byte_array_size] = {0}; input_byte_array[0] = 0xA5; input_byte_array[byte_array_size - 1] = 0x4B; - eprosima::fastrtps::rtps::GUID_t guid; - copy_from_byte_array_to_fastrtps_guid(input_byte_array, &guid); + eprosima::fastdds::rtps::GUID_t guid; + copy_from_byte_array_to_fastdds_guid(input_byte_array, &guid); uint8_t output_byte_array[byte_array_size] = {0}; - copy_from_fastrtps_guid_to_byte_array(guid, output_byte_array); + copy_from_fastdds_guid_to_byte_array(guid, output_byte_array); EXPECT_EQ(0, memcmp(input_byte_array, output_byte_array, byte_array_size)); } TEST(GUIDUtilsTest, guid_to_byte_array_and_back) { - eprosima::fastrtps::rtps::GuidPrefix_t prefix; + eprosima::fastdds::rtps::GuidPrefix_t prefix; prefix.value[0] = 0xD2; - prefix.value[eprosima::fastrtps::rtps::GuidPrefix_t::size - 1] = 0x3E; - eprosima::fastrtps::rtps::GUID_t input_guid{prefix, 1234}; + prefix.value[eprosima::fastdds::rtps::GuidPrefix_t::size - 1] = 0x3E; + eprosima::fastdds::rtps::GUID_t input_guid{prefix, 1234}; uint8_t byte_array[byte_array_size] = {0}; - copy_from_fastrtps_guid_to_byte_array(input_guid, byte_array); - eprosima::fastrtps::rtps::GUID_t output_guid; - copy_from_byte_array_to_fastrtps_guid(byte_array, &output_guid); + copy_from_fastdds_guid_to_byte_array(input_guid, byte_array); + eprosima::fastdds::rtps::GUID_t output_guid; + copy_from_byte_array_to_fastdds_guid(byte_array, &output_guid); EXPECT_EQ(input_guid, output_guid); } 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 905a79e7c..60a548bfe 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 @@ -29,8 +29,8 @@ using eprosima::fastdds::dds::DataReaderQos; -static const eprosima::fastrtps::Duration_t InfiniteDuration = - eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); +static const eprosima::fastdds::dds::Duration_t InfiniteDuration = + eprosima::fastdds::rtps::c_RTPSTimeInfinite.to_duration_t(); static const rosidl_type_hash_t zero_type_hash = rosidl_get_zero_initialized_type_hash(); @@ -103,10 +103,10 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, subscriber_qos_.history().kind); EXPECT_GE(10, subscriber_qos_.history().depth); - ASSERT_EQ(1, subscriber_qos_.type_consistency().representation.m_value.size()); + ASSERT_EQ(1, subscriber_qos_.representation().m_value.size()); EXPECT_EQ( eprosima::fastdds::dds::DataRepresentationId::XCDR_DATA_REPRESENTATION, - subscriber_qos_.type_consistency().representation.m_value[0]); + subscriber_qos_.representation().m_value[0]); } TEST_F(GetDataReaderQoSTest, large_depth_conversion) { diff --git a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp index e1188e2cf..60108faf9 100644 --- a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp @@ -16,10 +16,10 @@ #include #include -#include "fastdds/rtps/common/Property.h" -#include "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/common/Property.hpp" +#include "fastdds/rtps/attributes/PropertyPolicy.hpp" -#include "fastrtps/config.h" +#include "fastdds/config.hpp" #include "rcutils/filesystem.h" #include "rmw/error_handling.h" #include "rmw/security_options.h" @@ -48,12 +48,12 @@ 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"; -const eprosima::fastrtps::rtps::Property & lookup_property( - const eprosima::fastrtps::rtps::PropertySeq & properties, const std::string & property_name) +const eprosima::fastdds::rtps::Property & lookup_property( + const eprosima::fastdds::rtps::PropertySeq & properties, const std::string & property_name) { auto iterator = std::find_if( properties.begin(), properties.end(), - [&property_name](const eprosima::fastrtps::rtps::Property & item) -> bool { + [&property_name](const eprosima::fastdds::rtps::Property & item) -> bool { return item.name() == property_name; }); @@ -64,26 +64,26 @@ const eprosima::fastrtps::rtps::Property & lookup_property( return *iterator; } -const eprosima::fastrtps::rtps::Property & logging_plugin_property( - const eprosima::fastrtps::rtps::PropertySeq & properties) +const eprosima::fastdds::rtps::Property & logging_plugin_property( + const eprosima::fastdds::rtps::PropertySeq & properties) { return lookup_property(properties, logging_plugin_property_name); } -const eprosima::fastrtps::rtps::Property & log_file_property( - const eprosima::fastrtps::rtps::PropertySeq & properties) +const eprosima::fastdds::rtps::Property & log_file_property( + const eprosima::fastdds::rtps::PropertySeq & properties) { return lookup_property(properties, log_file_property_name); } -const eprosima::fastrtps::rtps::Property & verbosity_property( - const eprosima::fastrtps::rtps::PropertySeq & properties) +const eprosima::fastdds::rtps::Property & verbosity_property( + const eprosima::fastdds::rtps::PropertySeq & properties) { return lookup_property(properties, verbosity_property_name); } -const eprosima::fastrtps::rtps::Property & distribute_enable_property( - const eprosima::fastrtps::rtps::PropertySeq & properties) +const eprosima::fastdds::rtps::Property & distribute_enable_property( + const eprosima::fastdds::rtps::PropertySeq & properties) { return lookup_property(properties, distribute_enable_property_name); } @@ -124,7 +124,7 @@ class SecurityLoggingTest : public ::testing::Test TEST_F(SecurityLoggingTest, test_nothing_enabled) { - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -135,7 +135,7 @@ TEST_F(SecurityLoggingTest, test_log_to_file) { custom_setenv(log_file_variable_name, "/test.log"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -154,7 +154,7 @@ TEST_F(SecurityLoggingTest, test_log_publish_true) { custom_setenv(log_publish_variable_name, "true"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -173,7 +173,7 @@ TEST_F(SecurityLoggingTest, test_log_publish_false) { custom_setenv(log_publish_variable_name, "false"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -192,7 +192,7 @@ TEST_F(SecurityLoggingTest, test_log_publish_invalid) { custom_setenv(log_publish_variable_name, "invalid"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_FALSE(apply_security_logging_configuration(policy)); EXPECT_TRUE(rmw_error_is_set()); EXPECT_THAT( @@ -205,7 +205,7 @@ TEST_F(SecurityLoggingTest, test_log_verbosity) { custom_setenv(log_verbosity_variable_name, "FATAL"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -224,7 +224,7 @@ TEST_F(SecurityLoggingTest, test_log_verbosity_invalid) { custom_setenv(log_verbosity_variable_name, "INVALID_VERBOSITY"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_FALSE(apply_security_logging_configuration(policy)); EXPECT_TRUE(rmw_error_is_set()); EXPECT_THAT( @@ -241,7 +241,7 @@ TEST_F(SecurityLoggingTest, test_all) custom_setenv(log_publish_variable_name, "true"); custom_setenv(log_verbosity_variable_name, "ERROR"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::rtps::PropertyPolicy policy; EXPECT_TRUE(apply_security_logging_configuration(policy)); EXPECT_FALSE(rmw_error_is_set()); @@ -266,7 +266,7 @@ TEST_F(SecurityLoggingTest, test_apply_logging_fails) { custom_setenv(log_file_variable_name, "/test.log"); - eprosima::fastrtps::rtps::PropertyPolicy policy; + eprosima::fastdds::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 DDS"));