diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 3b376b9cc..744eb7b6e 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -38,8 +38,8 @@ find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_runtime_c REQUIRED) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp index c2ffac1ff..69af229a2 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/get_subscriber.hpp @@ -22,12 +22,12 @@ namespace rmw_fastrtps_cpp { -/// Return a native Fast DDS subscriber handle. +/// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or * when the subscription handle is from a different rmw implementation. * - * \return native Fast DDS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_CPP_PUBLIC eprosima::fastdds::dds::DataReader * diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp index a569d5595..037930f55 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // unused + bool keyed, bool create_publisher_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp index 8f66b56fa..3413cea4c 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, // unused + bool keyed, bool create_subscription_listener); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index cb1058edb..7920dd1aa 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -15,12 +15,15 @@ #include +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" #include "rcutils/error_handling.h" #include "rcutils/macros.h" @@ -56,8 +59,6 @@ rmw_fastrtps_cpp::create_publisher( bool keyed, bool create_publisher_listener) { - (void)keyed; - ///// // Check input parameters RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -66,7 +67,7 @@ rmw_fastrtps_cpp::create_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_publisher() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -78,7 +79,8 @@ rmw_fastrtps_cpp::create_publisher( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with invalid topic name: %s", reason); return nullptr; } } @@ -87,7 +89,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); return nullptr; } @@ -146,9 +148,9 @@ rmw_fastrtps_cpp::create_publisher( topic_name_mangled.c_str(), type_name.c_str()); return nullptr; } - + ///// - // Create the RMW Publisher struct (info) + // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; info = new (std::nothrow) CustomPublisherInfo(); @@ -177,6 +179,11 @@ rmw_fastrtps_cpp::create_publisher( // Transfer ownership to fastdds_type fastdds_type.reset(tsupport); } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); @@ -207,38 +214,50 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create and register Topic + bool topic_created = false; + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); if (!get_topic_qos(*qos_policies, topicQos)) { - RMW_SET_ERROR_MSG("Error setting topic QoS for publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS for publisher"); return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } + + topic_created = true; } - - eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; + else { + topic = dynamic_cast(des_topic); + if (!topic) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic, topic_created]() { + if (topic_created) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataWriter // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name @@ -258,12 +277,13 @@ rmw_fastrtps_cpp::create_publisher( eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - // Get QoS from ROS + // Get QoS from RMW if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } - // Creates DataWriter (with publisher name to not change name policy) + // Creates DataWriter info->data_writer_ = publisher->create_datawriter( topic, dataWriterQos, @@ -287,11 +307,9 @@ rmw_fastrtps_cpp::create_publisher( ///// // Allocate publisher - rmw_publisher_t * rmw_publisher = nullptr; - - rmw_publisher = rmw_publisher_allocate(); + rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate rmw_publisher"); return nullptr; } auto cleanup_rmw_publisher = rcpputils::make_scope_exit( @@ -300,12 +318,13 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher_free(rmw_publisher); }); + rmw_publisher->can_loan_messages = false; rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate memory for rmw_publisher topic name"); return nullptr; } memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); @@ -314,6 +333,7 @@ rmw_fastrtps_cpp::create_publisher( cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); cleanup_info.cancel(); diff --git a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp index 8cf195c6b..7c91a4108 100644 --- a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" - #include "rmw/rmw.h" #include "rmw/error_handling.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 8603c6605..81ad0c1e7 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -86,7 +86,6 @@ rmw_create_publisher( true); if (!publisher) { - RMW_SET_ERROR_MSG("Publisher creation failed"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 44da0ca7f..a86d45e1d 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -18,9 +18,6 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" -#include "fastdds/dds/topic/TopicDataType.hpp" - #include "rmw/error_handling.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index ec43fe20f..9e035a7de 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -38,8 +38,8 @@ find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_runtime_c REQUIRED) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp index 24dfe6acd..34812e883 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -23,12 +23,12 @@ namespace rmw_fastrtps_dynamic_cpp { -/// Return a native FastRTPS subscriber handle. +/// Return a native Fast DDS DataReader handle. /** * The function returns `NULL` when either the subscription handle is `NULL` or * when the subscription handle is from a different rmw implementation. * - * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` */ RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC eprosima::fastdds::dds::DataReader * diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp index 4d419346c..4f98b6c14 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp @@ -28,7 +28,7 @@ create_publisher( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options, - bool keyed, // unused + bool keyed, bool create_publisher_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp index 3113ca04c..0db879040 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp @@ -30,7 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, // unused + bool keyed, bool create_subscription_listener); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index 471f7e508..34c046c01 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" + #include "fastdds/dds/domain/DomainParticipant.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" namespace rmw_fastrtps_dynamic_cpp { diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index b8d2f73aa..df8a2c471 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -20,8 +20,11 @@ #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" #include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" #include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/rtps/resources/ResourceManagement.h" + #include "rcutils/error_handling.h" #include "rmw/allocators.h" @@ -59,13 +62,13 @@ rmw_fastrtps_dynamic_cpp::create_publisher( { ///// // Check input parameters - (void)keyed; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + RMW_SET_ERROR_MSG("create_publisher() called with an empty topic_name argument"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); @@ -77,7 +80,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } if (RMW_TOPIC_VALID != validation_result) { const char * reason = rmw_full_topic_name_validation_result_string(validation_result); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with invalid topic name: %s", reason); return nullptr; } } @@ -86,7 +90,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Check RMW QoS if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("Invalid QoS"); + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); return nullptr; } @@ -147,7 +151,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } ///// - // Create the RMW Publisher struct (info) + // Create the custom Publisher struct (info) CustomPublisherInfo * info = nullptr; info = new (std::nothrow) CustomPublisherInfo(); @@ -166,7 +170,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_impl = type_registry.get_message_type_support(type_support); if (!type_impl) { - RMW_SET_ERROR_MSG("failed to allocate type support"); + RMW_SET_ERROR_MSG("create_publisher() failed to get message_type_support"); return nullptr; } auto return_type_support = rcpputils::make_scope_exit( @@ -188,6 +192,11 @@ rmw_fastrtps_dynamic_cpp::create_publisher( fastdds_type.reset(tsupport); } + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(domainParticipant)) { RMW_SET_ERROR_MSG("create_publisher() failed to register type"); return nullptr; @@ -218,6 +227,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Create and register Topic + bool topic_created = false; + eprosima::fastdds::dds::Topic * topic = nullptr; if (!des_topic) { // Use Topic Qos Default eprosima::fastdds::dds::TopicQos topicQos = domainParticipant->get_default_topic_qos(); @@ -227,29 +238,37 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - des_topic = domainParticipant->create_topic( + topic = domainParticipant->create_topic( topic_name_mangled, type_name, topicQos); - if (!des_topic) { + if (!topic) { RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); return nullptr; } } - - eprosima::fastdds::dds::Topic * topic = dynamic_cast(des_topic); - if (!topic) { - RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); - return nullptr; + else { + topic = dynamic_cast(des_topic); + if (!topic) { + RMW_SET_ERROR_MSG("create_publisher() failed, publisher topic can only be of class Topic"); + return nullptr; + } } + auto cleanup_topic = rcpputils::make_scope_exit( + [domainParticipant, topic, topic_created]() { + if (topic_created) { + domainParticipant->delete_topic(topic); + } + }); + ///// // Create DataWriter // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load // datawriter which profile name matches with topic_name. If such profile does not exist, - // then use the default QoS. + // then use the default Fast DDS QoS. eprosima::fastdds::dds::DataWriterQos dataWriterQos = publisher->get_default_datawriter_qos(); // Try to load the profile with the topic name @@ -269,8 +288,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - // Get QoS from ROS + // Get QoS from RMW if (!get_datawriter_qos(*qos_policies, dataWriterQos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } @@ -291,14 +311,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( publisher->delete_datawriter(info->data_writer_); }); - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - ///// // Create RMW GID info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -307,9 +319,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Allocate publisher rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate rmw_publisher"); return nullptr; } auto cleanup_rmw_publisher = rcpputils::make_scope_exit( @@ -324,7 +335,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + RMW_SET_ERROR_MSG("create_publisher() failed to allocate memory for rmw_publisher topic name"); return nullptr; } memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); @@ -333,6 +344,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( cleanup_rmw_publisher.cancel(); cleanup_datawriter.cancel(); + cleanup_topic.cancel(); cleanup_listener.cancel(); cleanup_type_support.cancel(); return_type_support.cancel(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp index 3c5cab0f4..9ebeb4b8b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" - #include "rmw/rmw.h" #include "rmw/error_handling.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index 88109944e..d60206970 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -34,11 +34,3 @@ using_introspection_cpp_typesupport(const char * typesupport_identifier) return typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier; } - -// void -// _register_type( -// eprosima::fastrtps::Participant * participant, -// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) -// { -// eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); -// } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp index f0756fec8..1846b7ba5 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -109,9 +109,4 @@ _create_type_name( return ""; } -// void -// _register_type( -// eprosima::fastrtps::Participant * participant, -// rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport); - #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp index 9f68be552..1892537e5 100644 --- a/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_dynamic_cpp/test/test_logging.cpp @@ -14,28 +14,30 @@ #include "gtest/gtest.h" -#include "fastrtps/log/Log.h" +#include "fastdds/dds/log/Log.hpp" #include "rmw/rmw.h" #include "rmw/error_handling.h" TEST(TestLogging, rmw_logging) { + using Log = eprosima::fastdds::dds::Log; + rmw_ret_t ret = rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Warning, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); ret = rmw_set_log_severity(RMW_LOG_SEVERITY_FATAL); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); } TEST(TestLogging, rmw_logging_bad_verbosity) diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index d4b3ab88c..bce4e76db 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -41,8 +41,8 @@ find_package(rmw_dds_common REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps REQUIRED CONFIG) -find_package(FastRTPS REQUIRED MODULE) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) find_package(rmw REQUIRED) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 8d715bd65..cc3f2548d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -18,7 +18,6 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDataType.hpp" #include "fastcdr/FastBuffer.h" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index d5a53778e..dad3c01e3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -26,7 +26,8 @@ #include "fastcdr/FastBuffer.h" -#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" @@ -34,6 +35,10 @@ #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index af9daadff..1ae23eb20 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -24,7 +24,10 @@ #include "fastdds/dds/domain/DomainParticipantListener.hpp" #include "fastdds/dds/publisher/Publisher.hpp" #include "fastdds/dds/subscriber/Subscriber.hpp" -#include "fastdds/rtps/common/Types.h" + +#include "fastdds/rtps/participant/ParticipantDiscoveryInfo.h" +#include "fastdds/rtps/reader/ReaderDiscoveryInfo.h" +#include "fastdds/rtps/writer/WriterDiscoveryInfo.h" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" @@ -61,8 +64,8 @@ typedef struct CustomParticipantInfo // Protects creation and destruction of topics, readers and writers mutable std::mutex entity_creation_mutex_; - // Flag to establish if the QoS of the participant, - // its publishers and its subscribers are going + // Flag to establish if the QoS of the DomainParticipant, + // its DataWriters, and its DataReaders are going // to be configured only from an XML file or if // their settings are going to be overwritten by code // with the default configuration. diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 35779c60d..b3492fb96 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -20,12 +20,17 @@ #include #include +#include "fastdds/dds/core/status/BaseStatus.hpp" #include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/topic/Topic.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/rmw.h" @@ -63,7 +68,7 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds (void) info; } - // PublisherListener implementation + // DataWriterListener implementation RMW_FASTRTPS_SHARED_CPP_PUBLIC void on_publication_matched( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 7ef5371e4..ea17ef4d9 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -24,14 +24,20 @@ #include "fastcdr/FastBuffer.h" -#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/InstanceState.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" @@ -179,15 +185,13 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener : info_(info), list_has_data_(false), conditionMutex_(nullptr), conditionVariable_(nullptr) { - (void)info_; } void on_subscription_matched( - eprosima::fastdds::dds::DataReader * reader, + eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { - (void) reader; if (info.current_count_change == -1) { info_->pub_listener_->endpoint_erase_if_exists( eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 89640db6a..379a42703 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -21,10 +21,16 @@ #include #include +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + #include "rcpputils/thread_safety_annotations.hpp" #include "rmw/impl/cpp/macros.hpp" @@ -65,7 +71,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds (void)info; } - // SubscriberListener implementation + // DataReaderListener implementation void on_subscription_matched( eprosima::fastdds::dds::DataReader * reader, @@ -79,13 +85,13 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } } - update_unread_count(reader); + update_has_data(reader); } void on_data_available(eprosima::fastdds::dds::DataReader * reader) final { - update_unread_count(reader); + update_has_data(reader); } RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -133,18 +139,12 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds } void - update_unread_count(eprosima::fastdds::dds::DataReader * reader) + update_has_data(eprosima::fastdds::dds::DataReader * reader) { - // Make sure to call into Fast-RTPS before taking the lock to avoid an - // ABBA deadlock between internalMutex_ and mutexes inside of Fast-RTPS. - eprosima::fastdds::dds::SampleInfo info; - ReturnCode_t has_data_ret = reader->get_first_untaken_info(&info); - - // In case there is data, get_first_untaken_info return OK. Else it returs NO_DATA - bool has_data = false; - if (has_data_ret == ReturnCode_t::RETCODE_OK) { - has_data = true; - } + // Make sure to call into Fast DDS before taking the lock to avoid an + // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. + auto unread_count = reader->get_unread_count(); + bool has_data = unread_count > 0; std::lock_guard lock(internalMutex_); ConditionalScopedLock clock(conditionMutex_, conditionVariable_); @@ -163,11 +163,11 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds std::atomic_bool data_; std::atomic_bool deadline_changes_; - eprosima::fastrtps::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::atomic_bool liveliness_changes_; - eprosima::fastrtps::LivelinessChangedStatus liveliness_changed_status_ + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 7d851814d..a123a304e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -25,9 +25,12 @@ namespace rmw_fastrtps_shared_cpp { -// The Entities Publisher and Subscriber are created with the Participant -// Endpoint SubEntities as DataWriter and DataReader are created with -// create_publisher and create_subscription +// This method will create a DDS DomainParticipant along with +// a DDS Publisher and a DDS Subscriber +// For the creation of DDS DataWriter see method create_publisher +// For the creation of DDS DataReader see method create_subscription +// Note that ROS 2 Publishers and Subscriptions correspond with DDS DataWriters +// and DataReaders respectively and not with DDS Publishers and Subscribers. RMW_FASTRTPS_SHARED_CPP_PUBLIC CustomParticipantInfo * create_participant( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index fad8b2538..c70ba0892 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -16,9 +16,7 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ -#include #include -#include #include #include @@ -49,12 +47,10 @@ get_topic_qos( eprosima::fastdds::dds::TopicQos & topicQos); /* - * Converts the low-level QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. - * Since DataWriterQos or DataReaderQos does not have information about history and depth, these values are not set - * by this function. + * Converts the DDS QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. * * \param[in] dds_qos of type DataWriterQos or DataReaderQos - * \param[out] qos the equivalent of the data in DataWriterQos or DataReaderQos in rmw_qos_profile_t + * \param[out] qos the equivalent of the data in dds_qos as a rmw_qos_profile_t */ template void @@ -63,10 +59,10 @@ dds_qos_to_rmw_qos( rmw_qos_profile_t * qos) { switch (dds_qos.reliability().kind) { - case eprosima::fastdds::dds::ReliabilityQosPolicyKind::BEST_EFFORT_RELIABILITY_QOS: + case eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; - case eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS: + case eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; break; default: @@ -121,13 +117,21 @@ dds_qos_to_rmw_qos( qos->depth = static_cast(dds_qos.history().depth); } -template +/* + * Converts the RTPS QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. + * Since WriterQos or ReaderQos do not have information about history and depth, + * these values are not set by this function. + * + * \param[in] rtps_qos of type WriterQos or ReaderQos + * \param[out] qos the equivalent of the data in rtps_qos as a rmw_qos_profile_t + */ +template void rtps_qos_to_rmw_qos( - const DDSQoSPolicyT & dds_qos, + const RTPSQoSPolicyT & rtps_qos, rmw_qos_profile_t * qos) { - switch (dds_qos.m_reliability.kind) { + switch (rtps_qos.m_reliability.kind) { case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; break; @@ -139,7 +143,7 @@ rtps_qos_to_rmw_qos( break; } - switch (dds_qos.m_durability.kind) { + switch (rtps_qos.m_durability.kind) { case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; break; @@ -151,13 +155,13 @@ rtps_qos_to_rmw_qos( break; } - qos->deadline.sec = dds_qos.m_deadline.period.seconds; - qos->deadline.nsec = dds_qos.m_deadline.period.nanosec; + qos->deadline.sec = rtps_qos.m_deadline.period.seconds; + qos->deadline.nsec = rtps_qos.m_deadline.period.nanosec; - qos->lifespan.sec = dds_qos.m_lifespan.duration.seconds; - qos->lifespan.nsec = dds_qos.m_lifespan.duration.nanosec; + qos->lifespan.sec = rtps_qos.m_lifespan.duration.seconds; + qos->lifespan.nsec = rtps_qos.m_lifespan.duration.nanosec; - switch (dds_qos.m_liveliness.kind) { + switch (rtps_qos.m_liveliness.kind) { case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; break; @@ -168,8 +172,8 @@ rtps_qos_to_rmw_qos( qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; break; } - qos->liveliness_lease_duration.sec = dds_qos.m_liveliness.lease_duration.seconds; - qos->liveliness_lease_duration.nsec = dds_qos.m_liveliness.lease_duration.nanosec; + qos->liveliness_lease_duration.sec = rtps_qos.m_liveliness.lease_duration.seconds; + qos->liveliness_lease_duration.nsec = rtps_qos.m_liveliness.lease_duration.nanosec; } extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 08c5b4354..6b12c0bce 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,9 +18,8 @@ #include #include -#include "fastdds/dds/domain/DomainParticipant.hpp" #include "fastdds/dds/topic/TopicDescription.hpp" -#include "fastdds/dds/topic/qos/TopicQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" #include "fastrtps/types/TypesBase.h" diff --git a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp index 72ffb3087..49b90492b 100644 --- a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/Guid.h" #include "rmw/types.h" diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index f910daca9..33e6c13f8 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "fastdds/dds/core/status/BaseStatus.hpp" +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" #include "types/event_types.hpp" EventListenerInterface * @@ -42,7 +44,7 @@ PubListener::on_offered_deadline_missed( void PubListener::on_liveliness_lost( eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastrtps::LivelinessLostStatus & status) + const eprosima::fastdds::dds::LivelinessLostStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 8af36904a..d9eb011d1 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -13,6 +13,10 @@ // limitations under the License. #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" + +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" + #include "types/event_types.hpp" EventListenerInterface * @@ -24,7 +28,7 @@ CustomSubscriberInfo::getListener() const void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader * /* reader */, - const eprosima::fastrtps::RequestedDeadlineMissedStatus & status) + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -42,7 +46,7 @@ SubListener::on_requested_deadline_missed( void SubListener::on_liveliness_changed( eprosima::fastdds::dds::DataReader * /* reader */, - const eprosima::fastrtps::LivelinessChangedStatus & status) + const eprosima::fastdds::dds::LivelinessChangedStatus & status) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 304cb90f7..dbb8031c4 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -16,10 +16,13 @@ #include #include +#include "fastdds/dds/core/status/StatusMask.hpp" #include "fastdds/dds/domain/DomainParticipantFactory.hpp" #include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" #include "fastdds/dds/publisher/qos/PublisherQos.hpp" #include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" +#include "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/common/Property.h" #include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" #include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" @@ -73,17 +76,15 @@ get_security_file_paths( #endif // Private function to create Participant with QoS -CustomParticipantInfo * +static CustomParticipantInfo * __create_participant( const char * identifier, - eprosima::fastdds::dds::DomainParticipantQos domainParticipantQos, + const eprosima::fastdds::dds::DomainParticipantQos & domainParticipantQos, bool leave_middleware_default_qos, publishing_mode_t publishing_mode, rmw_dds_common::Context * common_context, size_t domain_id) { - ///// - // Declare everything before beginning to create things. CustomParticipantInfo * participant_info = nullptr; ///// @@ -92,6 +93,7 @@ __create_participant( participant_info = new CustomParticipantInfo(); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("__create_participant failed to allocate CustomParticipantInfo struct"); + return nullptr; } // lambda to delete participant info auto cleanup_participant_info = rcpputils::make_scope_exit( @@ -106,6 +108,7 @@ __create_participant( identifier, common_context); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); + return nullptr; } // lambda to delete listener auto cleanup_listener = rcpputils::make_scope_exit( @@ -113,21 +116,12 @@ __create_participant( delete participant_info->listener_; }); - ///// - // Force to load XML configuration file - if (ReturnCode_t::RETCODE_OK != - eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles()) - { - RMW_SET_ERROR_MSG("__create_participant failed to load FastDDS XML configuration file"); - } - ///// // Create Participant - // Create Participant Listener mask so it only uses the domain - // participant listener non inheritated callbacks - // Without this mask, callback of DataReaders on_data_available is never called - // Publisher and subscriber dont need masks as they do not have listeners + // As the participant listener is only used for discovery related callbacks, which are + // Fast DDS extensions to the DDS standard DomainParticipantListener interface, an empty + // mask should be used to let child entities handle standard DDS events. eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); participant_info->participant_ = @@ -222,7 +216,7 @@ rmw_fastrtps_shared_cpp::create_participant( // Add SHM transport if available auto shm_transport = std::make_shared(); - domainParticipantQos.transport().user_transports.push_back(udp_transport); + domainParticipantQos.transport().user_transports.push_back(shm_transport); } size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 69913bcb8..8cd4748f6 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" - #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -25,9 +22,6 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; - namespace rmw_fastrtps_shared_cpp { rmw_ret_t @@ -52,7 +46,7 @@ destroy_publisher( // Delete DataWriter ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -64,7 +58,7 @@ destroy_publisher( // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); - // Delete PublisherInfo structure + // Delete CustomPublisherInfo structure delete info; } else { ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 8fd526a99..3ecdd9b33 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -16,10 +16,9 @@ #include "rmw_fastrtps_shared_cpp/qos.hpp" -#include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" -#include "fastdds/dds/subscriber/DataReader.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" #include "fastdds/rtps/common/Time_t.h" #include "rmw/error_handling.h" @@ -42,7 +41,8 @@ is_time_default(const rmw_time_t & time) return time.sec == 0 && time.nsec == 0; } -// Private function to encapsulate DataReader and DataWriter together with TopicQos fill entities +// Private function to encapsulate DataReader and DataWriter, together with Topic, filling +// entities DDS QoS from the RMW QoS profile. template bool fill_entity_qos_from_profile( const rmw_qos_profile_t & qos_policies, @@ -50,10 +50,10 @@ bool fill_entity_qos_from_profile( { switch (qos_policies.history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - entity_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - entity_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS; break; case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: break; @@ -64,10 +64,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.durability) { case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - entity_qos.durability().kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_VOLATILE: - entity_qos.durability().kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + entity_qos.durability().kind = eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS; break; case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: break; @@ -78,10 +78,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.reliability) { case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - entity_qos.reliability().kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - entity_qos.reliability().kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + entity_qos.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS; break; case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: break; @@ -91,7 +91,7 @@ bool fill_entity_qos_from_profile( } // ensure the history depth is at least the requested queue size - assert(entity_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? + assert(entity_qos.history().depth >= 0); if ( qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && static_cast(entity_qos.history().depth) < qos_policies.depth) @@ -114,10 +114,10 @@ bool fill_entity_qos_from_profile( switch (qos_policies.liveliness) { case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: - entity_qos.liveliness().kind = eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: - entity_qos.liveliness().kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + entity_qos.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; break; case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: break; @@ -161,35 +161,7 @@ get_topic_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastdds::dds::TopicQos & topic_qos) { - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - topic_qos.history().kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - topic_qos.history().kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - // ensure the history depth is at least the requested queue size - assert(topic_qos.history().depth >= 0); // TODO(eprosima) should not be after initialization? - if ( - qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && - static_cast(topic_qos.history().depth) < qos_policies.depth) - { - if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - topic_qos.history().depth = static_cast(qos_policies.depth); - } - - return true; + return fill_entity_qos_from_profile(qos_policies, topic_qos);; } bool diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index ba1ba8e14..1a5046c57 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -90,7 +90,7 @@ __rmw_destroy_client( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -103,7 +103,7 @@ __rmw_destroy_client( remove_topic_and_type(participant_info, request_topic, info->request_type_support_); remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - // Delete ClientInfo structure + // Delete CustomClientInfo structure delete info; } else { final_ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp index 60e974894..db6b86a74 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp @@ -23,11 +23,12 @@ namespace rmw_fastrtps_shared_cpp { -using eprosima::fastdds::dds::Log; rmw_ret_t __rmw_set_log_severity(rmw_log_severity_t severity) { + using eprosima::fastdds::dds::Log; + Log::Kind log_kind; switch (severity) { @@ -49,7 +50,7 @@ __rmw_set_log_severity(rmw_log_severity_t severity) return RMW_RET_ERROR; } - eprosima::fastdds::dds::Log::SetVerbosity(log_kind); + Log::SetVerbosity(log_kind); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index b16195269..8a1bd6a0d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -21,7 +21,7 @@ #include "rmw/rmw.h" #include "fastdds/dds/publisher/DataWriter.hpp" -#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" @@ -125,9 +125,8 @@ __rmw_publisher_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(publisher->data); - eprosima::fastdds::dds::DataWriter * fastrtps_pub = info->data_writer_; - const eprosima::fastdds::dds::DataWriterQos & dds_qos = - fastrtps_pub->get_qos(); + eprosima::fastdds::dds::DataWriter * fastdds_dw = info->data_writer_; + const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastdds_dw->get_qos(); dds_qos_to_rmw_qos(dds_qos, qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index c89bea3c9..73ce6c534 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -17,6 +17,8 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" +#include "fastdds/rtps/common/WriteParams.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/impl/cpp/macros.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index e913c354d..1766e4bbd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -16,7 +16,7 @@ #include "fastcdr/Cdr.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "fastdds/rtps/common/WriteParams.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp index 339d70ba8..0d09c442a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_security_logging.cpp @@ -12,20 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" + #include #include #include #include +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + #include "fastrtps/config.h" + #include "rcutils/filesystem.h" #include "rcutils/get_env.h" + #include "rmw/error_handling.h" #include "rmw/qos_profiles.h" #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" - #if HAVE_SECURITY namespace @@ -44,7 +49,7 @@ const char verbosity_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.logging const char distribute_enable_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.distribute"; -// Fast RTPS supports the following verbosities: +// Fast DDS supports the following verbosities: // - EMERGENCY_LEVEL // - ALERT_LEVEL // - CRITICAL_LEVEL @@ -211,8 +216,8 @@ bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPoli #else (void)policy; RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); + "This Fast DDS version doesn't have the security libraries\n" + "Please compile Fast DDS using the -DSECURITY=ON CMake option"); return false; #endif } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 32f85ff7a..e38d780e2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -103,7 +103,7 @@ __rmw_destroy_service( // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->response_writer_); if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG("Fail in delete datareader"); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); return rmw_fastrtps_shared_cpp::cast_error_dds_to_rmw(ret); } @@ -116,7 +116,7 @@ __rmw_destroy_service( remove_topic_and_type(participant_info, request_topic, info->request_type_support_); remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - // Delete ServiceInfo structure + // Delete CustomServiceInfo structure delete info; } else { final_ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index ab9bf5416..98f42159f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -21,7 +21,7 @@ #include "rmw/rmw.h" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -99,9 +99,8 @@ __rmw_subscription_get_actual_qos( rmw_qos_profile_t * qos) { auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader * fastrtps_sub = info->data_reader_; - const eprosima::fastdds::dds::DataReaderQos & dds_qos = - fastrtps_sub->get_qos(); + eprosima::fastdds::dds::DataReader * fastdds_dr = info->data_reader_; + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->get_qos(); dds_qos_to_rmw_qos(dds_qos, qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 5fd17b58a..ec75d5f3e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -79,7 +79,7 @@ _take( data.impl = info->type_support_impl_; if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->data_reader_); + info->listener_->update_has_data(info->data_reader_); if (sinfo.valid_data) { if (message_info) { @@ -287,7 +287,7 @@ _take_serialized_message( if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener - info->listener_->update_unread_count(info->data_reader_); + info->listener_->update_has_data(info->data_reader_); if (sinfo.valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index df35f97d8..d3cfa24dc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/subscriber/Subscriber.h" - #include "rcutils/macros.h" #include "rmw/error_handling.h" diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index f58acc562..e070c92a5 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -64,7 +64,7 @@ destroy_subscription( // Delete topic and unregister type remove_topic_and_type(participant_info, topic, info->type_support_); - // Delete SubscriberInfo structure + // Delete CustomSubscriberInfo structure delete info; } else { ret = RMW_RET_INVALID_ARGUMENT; diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index f94ea65b1..31acf8e15 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -16,9 +16,14 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" -#include "rmw/rmw.h" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + #include "fastrtps/types/TypesBase.h" +#include "rmw/rmw.h" + using ReturnCode_t = eprosima::fastrtps::types::ReturnCode_t; namespace rmw_fastrtps_shared_cpp @@ -30,7 +35,7 @@ rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) if (code == ReturnCode_t::RETCODE_OK) { return RMW_RET_OK; } else if (code == ReturnCode_t::RETCODE_ERROR) { - // repeats the error to avoid too much if comparations + // repeats the error to avoid too many 'if' comparisons return RMW_RET_ERROR; } else if (code == ReturnCode_t::RETCODE_TIMEOUT) { return RMW_RET_TIMEOUT; diff --git a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp index 588da9ecd..3bba2a29c 100644 --- a/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_guid_utils.cpp @@ -14,7 +14,9 @@ #include "gtest/gtest.h" -#include "fastrtps/rtps/common/Guid.h" +#include "fastdds/rtps/common/EntityId_t.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/GuidPrefix_t.hpp" #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" diff --git a/rmw_fastrtps_shared_cpp/test/test_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_logging.cpp index 73365e05b..e2b158035 100644 --- a/rmw_fastrtps_shared_cpp/test/test_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_logging.cpp @@ -14,7 +14,7 @@ #include "gtest/gtest.h" -#include "fastrtps/log/Log.h" +#include "fastdds/dds/log/Log.hpp" #include "rmw/rmw.h" #include "rmw/error_handling.h" @@ -23,21 +23,23 @@ TEST(TestLogging, rmw_logging) { + using eprosima::fastdds::dds::Log; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Info, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Warning, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_FATAL); EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_EQ(eprosima::fastrtps::Log::Kind::Error, eprosima::fastrtps::Log::GetVerbosity()); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); } TEST(TestLogging, rmw_logging_bad_verbosity) diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 7dedd050f..6000cc4e9 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -17,8 +17,9 @@ #include "gtest/gtest.h" -#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/core/policy/QosPolicies.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" @@ -78,13 +79,13 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); EXPECT_EQ( - eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, subscriber_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, subscriber_qos_.durability().kind); EXPECT_EQ( - eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, subscriber_qos_.liveliness().kind); EXPECT_EQ(0, subscriber_qos_.lifespan().duration.seconds); EXPECT_EQ(500000000u, subscriber_qos_.lifespan().duration.nanosec); @@ -93,7 +94,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { EXPECT_EQ(10, subscriber_qos_.liveliness().lease_duration.seconds); EXPECT_EQ(0u, subscriber_qos_.liveliness().lease_duration.nanosec); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, subscriber_qos_.history().kind); EXPECT_GE(10, subscriber_qos_.history().depth); } @@ -176,13 +177,13 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( - eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS, + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, publisher_qos_.reliability().kind); EXPECT_EQ( - eprosima::fastrtps::VOLATILE_DURABILITY_QOS, + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, publisher_qos_.durability().kind); EXPECT_EQ( - eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS, + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, publisher_qos_.liveliness().kind); EXPECT_EQ(0, publisher_qos_.lifespan().duration.seconds); EXPECT_EQ(500000000u, publisher_qos_.lifespan().duration.nanosec); @@ -191,7 +192,7 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { EXPECT_EQ(10, publisher_qos_.liveliness().lease_duration.seconds); EXPECT_EQ(0u, publisher_qos_.liveliness().lease_duration.nanosec); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, publisher_qos_.history().kind); EXPECT_GE(10, publisher_qos_.history().depth); } @@ -204,7 +205,7 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); EXPECT_EQ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS, + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, publisher_qos_.history().kind); EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); diff --git a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp index 81fb7cbc8..e1188e2cf 100644 --- a/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_security_logging.cpp @@ -16,6 +16,9 @@ #include #include +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + #include "fastrtps/config.h" #include "rcutils/filesystem.h" #include "rmw/error_handling.h" @@ -266,7 +269,7 @@ TEST_F(SecurityLoggingTest, test_apply_logging_fails) eprosima::fastrtps::rtps::PropertyPolicy policy; EXPECT_FALSE(apply_security_logging_configuration(policy)); EXPECT_TRUE(rmw_error_is_set()); - EXPECT_THAT(rmw_get_error_string().str, HasSubstr("Please compile Fast-RTPS")); + EXPECT_THAT(rmw_get_error_string().str, HasSubstr("Please compile Fast DDS")); } #endif