From 8f9b0e863db90f22ac6ad78a2eb7d0f9f6420902 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 24 Jun 2022 11:15:28 +0200 Subject: [PATCH 01/34] Initial commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../include/rmw_fastrtps_cpp/subscription.hpp | 3 +- .../src/init_rmw_context_impl.cpp | 8 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 11 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 11 +- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 3 +- rmw_fastrtps_cpp/src/subscription.cpp | 15 +- .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 3 +- .../src/init_rmw_context_impl.cpp | 3 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 10 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 10 +- .../src/rmw_subscription.cpp | 3 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 27 +-- .../custom_client_info.hpp | 7 +- .../custom_event_info.hpp | 2 + .../custom_publisher_info.hpp | 3 + .../custom_service_info.hpp | 7 +- .../custom_subscriber_info.hpp | 50 ++---- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 2 - .../src/custom_publisher_info.cpp | 5 + .../src/custom_subscriber_info.cpp | 15 +- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 7 +- .../src/rmw_guard_condition.cpp | 6 +- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 2 + rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 2 + rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 9 +- .../src/rmw_subscription.cpp | 5 +- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 7 +- .../src/rmw_trigger_guard_condition.cpp | 7 +- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 166 +++++++----------- rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp | 18 +- rmw_fastrtps_shared_cpp/src/subscription.cpp | 3 - .../src/types/custom_wait_set_info.hpp | 27 --- .../src/types/guard_condition.hpp | 87 --------- rmw_fastrtps_shared_cpp/src/utils.cpp | 7 +- 34 files changed, 153 insertions(+), 398 deletions(-) delete mode 100644 rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp delete mode 100644 rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp index 3413cea4c..32370bc62 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -30,8 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, - bool create_subscription_listener); + bool keyed); } // namespace rmw_fastrtps_cpp #endif // RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 51138919e..ec5e7db01 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -119,8 +119,7 @@ init_context_impl(rmw_context_t * context) "ros_discovery_info", &qos, &subscription_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), + false), // our fastrtps typesupport doesn't support keyed topics [&](rmw_subscription_t * sub) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( eprosima_fastrtps_identifier, @@ -160,11 +159,6 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) { - return ret; - } - common_context->graph_cache.set_on_change_callback( [guard_condition = graph_guard_condition.get()]() { rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index d376bdc2e..dba37b5eb 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -195,7 +195,6 @@ rmw_create_client( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; - delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -246,13 +245,6 @@ rmw_create_client( info->response_type_support_ = response_fastdds_type; ///// - // Create Listeners - info->listener_ = new (std::nothrow) ClientListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); - return nullptr; - } - info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); @@ -328,8 +320,7 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - reader_qos, - info->listener_); + reader_qos); if (!info->response_reader_) { RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index a14d609cf..75a99e0b0 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -199,7 +199,6 @@ rmw_create_service( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; - delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -248,13 +247,6 @@ rmw_create_service( info->response_type_support_ = response_fastdds_type; ///// - // Create Listeners - info->listener_ = new (std::nothrow) ServiceListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); - return nullptr; - } - info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); @@ -327,8 +319,7 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - reader_qos, - info->listener_); + reader_qos); if (!info->request_reader_) { RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 41172d2c8..de2345f3f 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -89,8 +89,7 @@ rmw_create_subscription( topic_name, &adapted_qos_policies, subscription_options, - false, // use no keyed topic - true); // create subscription listener + false); // use no keyed topic if (!subscription) { return nullptr; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index e416c1095..a91a319a0 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -65,8 +65,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, - bool create_subscription_listener) + bool keyed) { ///// // Check input parameters @@ -165,7 +164,6 @@ create_subscription( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { - delete info->listener_; if (info->type_support_) { dds_participant->unregister_type(info->type_support_.get_type_name()); } @@ -206,16 +204,6 @@ create_subscription( return nullptr; } - ///// - // Create Listener - if (create_subscription_listener) { - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - } - ///// // Create and register Topic eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); @@ -290,7 +278,6 @@ create_subscription( subscription_options, subscriber, des_topic, - info->listener_, &info->data_reader_)) { RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp index 0db879040..6401254c1 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,8 +30,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, - bool create_subscription_listener); + bool keyed); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index aaa658fda..7e66e759c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -119,8 +119,7 @@ init_context_impl(rmw_context_t * context) "ros_discovery_info", &qos, &subscription_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), + false), // our fastrtps typesupport doesn't support keyed topics [&](rmw_subscription_t * sub) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index e759cd075..8f1c7863a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -202,7 +202,6 @@ rmw_create_client( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; - delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -278,12 +277,6 @@ rmw_create_client( ///// // Create Listeners - info->listener_ = new (std::nothrow) ClientListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); - return nullptr; - } - info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); @@ -359,8 +352,7 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - reader_qos, - info->listener_); + reader_qos); if (!info->response_reader_) { RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 1d0fce778..c66247313 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -206,7 +206,6 @@ rmw_create_service( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; - delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -280,12 +279,6 @@ rmw_create_service( ///// // Create Listeners - info->listener_ = new (std::nothrow) ServiceListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); - return nullptr; - } - info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); @@ -358,8 +351,7 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - reader_qos, - info->listener_); + reader_qos); if (!info->request_reader_) { RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index 5a9933a9d..55a6ba980 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -92,8 +92,7 @@ rmw_create_subscription( topic_name, &adapted_qos_policies, subscription_options, - false, - true); + false); if (!subscription) { return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 2637097fd..4fefe7dfe 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -65,8 +65,7 @@ create_subscription( const char * topic_name, const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options, - bool keyed, - bool create_subscription_listener) + bool keyed) { RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); @@ -165,7 +164,6 @@ create_subscription( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { - delete info->listener_; if (info->type_support_) { dds_participant->unregister_type(info->type_support_.get_type_name()); } @@ -211,17 +209,6 @@ create_subscription( info->type_support_ = fastdds_type; - ///// - // Create Listener - if (create_subscription_listener) { - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - } - ///// // Create and register Topic eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); @@ -266,12 +253,6 @@ create_subscription( return nullptr; } - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - return nullptr; - } - eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; switch (subscription_options->require_unique_network_flow_endpoints) { default: @@ -296,16 +277,14 @@ create_subscription( // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, - reader_qos, - info->listener_); + reader_qos); if (!info->data_reader_ && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { info->data_reader_ = subscriber->create_datareader( des_topic, - original_qos, - info->listener_); + original_qos); } if (!info->data_reader_) { 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 6a4b4d515..cd329f89f 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 @@ -31,7 +31,6 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" @@ -46,7 +45,6 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -class ClientListener; class ClientPubListener; typedef struct CustomClientInfo @@ -61,7 +59,6 @@ typedef struct CustomClientInfo std::string request_topic_; std::string response_topic_; - ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; @@ -78,6 +75,7 @@ typedef struct CustomClientResponse eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; +/* class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: @@ -175,7 +173,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener } void on_subscription_matched( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { if (info_ == nullptr) { @@ -239,6 +237,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener std::mutex on_new_response_m_; uint64_t unread_count_ = 0; }; +*/ class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index d7df46611..bd3ed3342 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -23,6 +23,7 @@ #include #include "fastcdr/FastBuffer.h" +#include "fastdds/dds/core/condition/StatusCondition.hpp" #include "rmw/event.h" #include "rmw/event_callback_type.h" @@ -102,6 +103,7 @@ class EventListenerInterface::ConditionalScopedLock struct CustomEventInfo { + virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; virtual EventListenerInterface * getListener() const = 0; }; 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 37db4ca2c..6e2bbcd27 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 @@ -50,6 +50,9 @@ typedef struct CustomPublisherInfo : public CustomEventInfo rmw_gid_t publisher_gid{}; const char * typesupport_identifier_{nullptr}; + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; + RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * getListener() const final; 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 9d50c5a3d..4aa7e7d7e 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 @@ -29,7 +29,6 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -#include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" @@ -45,7 +44,6 @@ #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -class ServiceListener; class ServicePubListener; enum class client_present_t @@ -65,7 +63,6 @@ typedef struct CustomServiceInfo eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; - ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; const char * typesupport_identifier_{nullptr}; @@ -180,6 +177,7 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener std::condition_variable cv_; }; +/* class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: @@ -191,7 +189,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener void on_subscription_matched( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final { if (info.current_count_change == -1) { @@ -342,5 +340,6 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener std::mutex on_new_request_m_; uint64_t unread_count_ = 0; }; +*/ #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ 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 a130f2cf6..00b713ca0 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 @@ -76,11 +76,22 @@ struct CustomSubscriberInfo : public CustomEventInfo eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic_ {nullptr}; eprosima::fastdds::dds::DataReaderQos datareader_qos_; + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; + RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * - getListener() const final; + getListener() const final + { + return nullptr; + } +}; + +class SubListener +{ }; +/* class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: @@ -167,42 +178,6 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds bool takeNextEvent(rmw_event_type_t event_type, void * event_info) final; - // SubListener API - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasData() const - { - return data_.load(std::memory_order_relaxed); - } - - void - update_has_data(eprosima::fastdds::dds::DataReader * reader) - { - // Make sure to call into Fast DDS before taking the lock to avoid an - // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. - auto unread_count = reader->get_unread_count(); - bool has_data = unread_count > 0; - - std::lock_guard lock(internalMutex_); - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - data_.store(has_data, std::memory_order_relaxed); - } - size_t publisherCount() { std::lock_guard lock(internalMutex_); @@ -264,5 +239,6 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds size_t qos_depth_; size_t new_data_unread_count_ = 0; }; +*/ #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_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 88b8e183a..0c7e3197e 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 @@ -168,7 +168,6 @@ create_content_filtered_topic( * \param[in] subscription_options Options of the subscription. * \param[in] subscriber A subsciber to create the data reader. * \param[in] des_topic TopicDescription returned by find_and_check_topic_and_type. -* \param[in] listener The listener of the data reader. * \param[out] data_reader Will hold the pointer to the data reader. * * \return true when the data reader was created @@ -181,7 +180,6 @@ create_datareader( const rmw_subscription_options_t * subscription_options, eprosima::fastdds::dds::Subscriber * subscriber, eprosima::fastdds::dds::TopicDescription * des_topic, - SubListener * listener, eprosima::fastdds::dds::DataReader ** data_reader); } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 9fc6e4009..501d80f2c 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -20,6 +20,11 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" +eprosima::fastdds::dds::StatusCondition& CustomPublisherInfo::get_statuscondition() const +{ + return data_writer_->get_statuscondition(); +} + EventListenerInterface * CustomPublisherInfo::getListener() const { diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 6c24c358f..0844000d3 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -20,6 +20,12 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" +eprosima::fastdds::dds::StatusCondition& CustomSubscriberInfo::get_statuscondition() const +{ + return data_reader_->get_statuscondition(); +} + +/* EventListenerInterface * CustomSubscriberInfo::getListener() const { @@ -28,7 +34,7 @@ CustomSubscriberInfo::getListener() const void SubListener::on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { std::lock_guard lock(internalMutex_); @@ -54,7 +60,7 @@ SubListener::on_requested_deadline_missed( } void SubListener::on_liveliness_changed( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::LivelinessChangedStatus & status) { std::lock_guard lock(internalMutex_); @@ -82,7 +88,7 @@ void SubListener::on_liveliness_changed( } void SubListener::on_sample_lost( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::SampleLostStatus & status) { std::lock_guard lock(internalMutex_); @@ -100,7 +106,7 @@ void SubListener::on_sample_lost( } void SubListener::on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader * /* reader */, + eprosima::fastdds::dds::DataReader * , const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) { std::lock_guard lock(internalMutex_); @@ -209,3 +215,4 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) } return true; } +*/ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index dfeb1747c..fcb6b19e2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -93,11 +93,6 @@ __rmw_destroy_client( info->response_reader_->set_listener(nullptr); } - // Delete DataReader listener - if (nullptr != info->listener_) { - delete info->listener_; - } - // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { @@ -156,9 +151,11 @@ __rmw_client_set_on_new_response_callback( const void * user_data) { auto custom_client_info = static_cast(rmw_client->data); + /* TODO custom_client_info->listener_->set_on_new_response_callback( user_data, callback); + */ return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp index cdcac523c..8aeedc2d0 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp @@ -17,7 +17,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "types/guard_condition.hpp" +#include "fastdds/dds/core/condition/GuardCondition.hpp" namespace rmw_fastrtps_shared_cpp { @@ -28,7 +28,7 @@ __rmw_create_guard_condition(const char * identifier) rmw_guard_condition_t * guard_condition_handle = new rmw_guard_condition_t; guard_condition_handle->implementation_identifier = identifier; - guard_condition_handle->data = new GuardCondition(); + guard_condition_handle->data = new eprosima::fastdds::dds::GuardCondition(); return guard_condition_handle; } @@ -38,7 +38,7 @@ __rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) rmw_ret_t ret = RMW_RET_ERROR; if (guard_condition) { - delete static_cast(guard_condition->data); + delete static_cast(guard_condition->data); delete guard_condition; ret = RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 73ce6c534..a574eb77f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -91,6 +91,7 @@ __rmw_take_request( auto info = static_cast(service->data); assert(info); + /* TODO CustomServiceRequest request = info->listener_->getRequest(); if (request.buffer_ != nullptr) { @@ -115,6 +116,7 @@ __rmw_take_request( delete request.buffer_; } + */ return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 1766e4bbd..fc9d52846 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -54,6 +54,7 @@ __rmw_take_response( CustomClientResponse response; + /* TODO if (info->listener_->getResponse(response)) { auto raw_type_support = dynamic_cast( info->response_type_support_.get()); @@ -73,6 +74,7 @@ __rmw_take_response( *taken = true; } } + */ return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 92bd790be..b363e660a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -102,12 +102,6 @@ __rmw_destroy_service( show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datareader"); final_ret = RMW_RET_ERROR; - info->request_reader_->set_listener(nullptr); - } - - // Delete DataReader listener - if (nullptr != info->listener_) { - delete info->listener_; } // Delete DataWriter @@ -116,7 +110,6 @@ __rmw_destroy_service( show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datawriter"); final_ret = RMW_RET_ERROR; - info->response_writer_->set_listener(nullptr); } // Delete DataWriter listener @@ -168,9 +161,11 @@ __rmw_service_set_on_new_request_callback( const void * user_data) { auto custom_service_info = static_cast(rmw_service->data); + /* custom_service_info->listener_->set_on_new_request_callback( user_data, callback); + */ return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index a521b1a69..325ea029f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -92,7 +92,9 @@ __rmw_subscription_count_matched_publishers( { auto info = static_cast(subscription->data); + /* TODO *publisher_count = info->listener_->publisherCount(); + */ return RMW_RET_OK; } @@ -183,7 +185,6 @@ __rmw_subscription_set_content_filter( subscription_options, subscriber, des_topic, - info->listener_, &info->data_reader_)) { RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); @@ -267,9 +268,11 @@ __rmw_subscription_set_on_new_message_callback( const void * user_data) { auto custom_subscriber_info = static_cast(rmw_subscription->data); + /* TODO custom_subscriber_info->listener_->set_on_new_message_callback( user_data, callback); + */ return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index e4e3dfb6c..ff0c77946 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -91,6 +91,7 @@ _take( data.data = ros_message; data.impl = info->type_support_impl_; + /* TODO while (0 < info->data_reader_->get_unread_count()) { if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener @@ -115,6 +116,7 @@ _take( } } } + */ TRACEPOINT( rmw_take, @@ -318,6 +320,7 @@ _take_serialized_message( data.data = &buffer; data.impl = nullptr; // not used when is_cdr_buffer is true + /* TODO if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { // Update hasData from listener info->listener_->update_has_data(info->data_reader_); @@ -339,6 +342,7 @@ _take_serialized_message( *taken = true; } } + */ return RMW_RET_OK; } @@ -481,6 +485,7 @@ __rmw_take_loaned_message_internal( auto item = std::make_unique(); + /* TODO while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) { if (item->info_seq[0].valid_data) { if (nullptr != message_info) { @@ -498,10 +503,10 @@ __rmw_take_loaned_message_internal( // Should return loan before taking again info->data_reader_->return_loan(item->data_seq, item->info_seq); } + */ // No data available, return loan information. *taken = false; - info->listener_->update_has_data(info->data_reader_); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp index ac3c72013..45b5287bc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp @@ -18,7 +18,8 @@ #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "types/guard_condition.hpp" + +#include "fastdds/dds/core/condition/GuardCondition.hpp" namespace rmw_fastrtps_shared_cpp { @@ -34,8 +35,8 @@ __rmw_trigger_guard_condition( return RMW_RET_ERROR; } - auto guard_condition = static_cast(guard_condition_handle->data); - guard_condition->trigger(); + auto guard_condition = static_cast(guard_condition_handle->data); + guard_condition->set_trigger_value(true); return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 4275e4ffc..f7beb2879 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -21,70 +21,10 @@ #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "types/custom_wait_set_info.hpp" -#include "types/guard_condition.hpp" - -// helper function for wait -bool -check_wait_set_for_data( - const rmw_subscriptions_t * subscriptions, - const rmw_guard_conditions_t * guard_conditions, - const rmw_services_t * services, - const rmw_clients_t * clients, - const rmw_events_t * events) -{ - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - // Short circuiting out of this function is possible - if (custom_subscriber_info && custom_subscriber_info->listener_->hasData()) { - return true; - } - } - } - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - auto custom_client_info = static_cast(data); - if (custom_client_info && custom_client_info->listener_->hasData()) { - return true; - } - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - if (custom_service_info && custom_service_info->listener_->hasData()) { - return true; - } - } - } - - if (events) { - for (size_t i = 0; i < events->event_count; ++i) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - if (custom_event_info->getListener()->hasEvent(event->event_type)) { - return true; - } - } - } - - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - if (guard_condition && guard_condition->hasTriggered()) { - return true; - } - } - } - return false; -} +#include "fastdds/dds/core/condition/WaitSet.hpp" +#include "fastdds/dds/core/condition/GuardCondition.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" namespace rmw_fastrtps_shared_cpp { @@ -114,15 +54,13 @@ __rmw_wait( // error. // - Heap is corrupt. // In all three cases, it's better if this crashes soon enough. - auto wait_set_info = static_cast(wait_set->data); - std::mutex * conditionMutex = &wait_set_info->condition_mutex; - std::condition_variable * conditionVariable = &wait_set_info->condition; + auto fastdds_wait_set = static_cast(wait_set->data); if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); - custom_subscriber_info->listener_->attachCondition(conditionMutex, conditionVariable); + fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); } } @@ -130,7 +68,7 @@ __rmw_wait( for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; auto custom_client_info = static_cast(data); - custom_client_info->listener_->attachCondition(conditionMutex, conditionVariable); + fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); } } @@ -138,7 +76,7 @@ __rmw_wait( for (size_t i = 0; i < services->service_count; ++i) { void * data = services->services[i]; auto custom_service_info = static_cast(data); - custom_service_info->listener_->attachCondition(conditionMutex, conditionVariable); + fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); } } @@ -146,62 +84,82 @@ __rmw_wait( for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); auto custom_event_info = static_cast(event->data); - custom_event_info->getListener()->attachCondition(conditionMutex, conditionVariable); + fastdds_wait_set->attach_condition(custom_event_info->get_statuscondition()); } } if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - guard_condition->attachCondition(conditionMutex, conditionVariable); + auto guard_condition = static_cast(data); + fastdds_wait_set->attach_condition(*guard_condition); } } - // This mutex prevents any of the listeners - // to change the internal state and notify the condition - // between the call to hasData() / hasTriggered() and wait() - // otherwise the decision to wait might be incorrect - std::unique_lock lock(*conditionMutex); + eprosima::fastdds::dds::ConditionSeq triggered_coditions; + ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, + (wait_timeout && (wait_timeout->sec > 0 || wait_timeout->nsec > 0)) ? + Duration_t{wait_timeout->sec, wait_timeout->nsec} : eprosima::fastrtps::c_TimeInfinite + ); - bool hasData = check_wait_set_for_data( - subscriptions, guard_conditions, services, clients, events); - auto predicate = [subscriptions, guard_conditions, services, clients, events]() { - return check_wait_set_for_data(subscriptions, guard_conditions, services, clients, events); - }; + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + fastdds_wait_set->detach_condition(*guard_condition); + } + } - bool timeout = false; - if (!hasData) { - if (!wait_timeout) { - conditionVariable->wait(lock, predicate); - } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { - auto n = std::chrono::duration_cast( - std::chrono::seconds(wait_timeout->sec)); - n += std::chrono::nanoseconds(wait_timeout->nsec); - timeout = !conditionVariable->wait_for(lock, n, predicate); - } else { - timeout = true; + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->detach_condition(custom_event_info->get_statuscondition()); + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + fastdds_wait_set->detach_condition(custom_service_info->request_reader_->get_statuscondition()); } } - // Unlock the condition variable mutex to prevent deadlocks that can occur if - // a listener triggers while the condition variable is being detached. - // Listeners will no longer be prevented from changing their internal state, - // but that should not cause issues (if a listener has data / has triggered - // after we check, it will be caught on the next call to this function). - lock.unlock(); + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + fastdds_wait_set->detach_condition(custom_client_info->response_reader_->get_statuscondition()); + } + } if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); - custom_subscriber_info->listener_->detachCondition(); - if (!custom_subscriber_info->listener_->hasData()) { - subscriptions->subscribers[i] = 0; + eprosima::fastdds::dds::StatusCondition& condition = custom_subscriber_info->data_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(condition); + } + } + + if (ReturnCode_t::RETCODE_OK == ret_code) + { + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + custom_subscriber_info->listener_->detachCondition(); + if (!custom_subscriber_info->listener_->hasData()) { + subscriptions->subscribers[i] = 0; + } + } } - } } + + + if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; @@ -245,7 +203,9 @@ __rmw_wait( } } } + */ + bool timeout = false; return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp index c29137fe2..8c127418a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp @@ -21,7 +21,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "types/custom_wait_set_info.hpp" +#include "fastdds/dds/core/condition/WaitSet.hpp" namespace rmw_fastrtps_shared_cpp { @@ -40,26 +40,26 @@ __rmw_create_wait_set(const char * identifier, rmw_context_t * context, size_t m (void)max_conditions; // From here onward, error results in unrolling in the goto fail block. - CustomWaitsetInfo * wait_set_info = nullptr; + eprosima::fastdds::dds::WaitSet * fastdds_wait_set = nullptr; rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); if (!wait_set) { RMW_SET_ERROR_MSG("failed to allocate wait set"); goto fail; } wait_set->implementation_identifier = identifier; - wait_set->data = rmw_allocate(sizeof(CustomWaitsetInfo)); + wait_set->data = rmw_allocate(sizeof(eprosima::fastdds::dds::WaitSet)); if (!wait_set->data) { RMW_SET_ERROR_MSG("failed to allocate wait set info"); goto fail; } // This should default-construct the fields of CustomWaitsetInfo RMW_TRY_PLACEMENT_NEW( - wait_set_info, + fastdds_wait_set, wait_set->data, goto fail, // cppcheck-suppress syntaxError - CustomWaitsetInfo, ); - (void) wait_set_info; + eprosima::fastdds::dds::WaitSet, ); + (void) fastdds_wait_set; return wait_set; @@ -89,12 +89,12 @@ __rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set) // error. // - Heap is corrupt. // In all three cases, it's better if this crashes soon enough. - auto wait_set_info = static_cast(wait_set->data); + auto fastdds_wait_set = static_cast(wait_set->data); if (wait_set->data) { - if (wait_set_info) { + if (fastdds_wait_set) { RMW_TRY_DESTRUCTOR( - wait_set_info->~CustomWaitsetInfo(), wait_set_info, result = RMW_RET_ERROR) + fastdds_wait_set->eprosima::fastdds::dds::WaitSet::~WaitSet(), fastdds_wait_set, result = RMW_RET_ERROR) } rmw_free(wait_set->data); } diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index bfb625459..6c798c4db 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -65,9 +65,6 @@ destroy_subscription( return RMW_RET_OK; } - // Delete DataReader listener - delete info->listener_; - // Delete topic and unregister type remove_topic_and_type(participant_info, info->topic_, info->type_support_); diff --git a/rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp b/rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp deleted file mode 100644 index 1ebdd5716..000000000 --- a/rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TYPES__CUSTOM_WAIT_SET_INFO_HPP_ -#define TYPES__CUSTOM_WAIT_SET_INFO_HPP_ - -#include -#include - -typedef struct CustomWaitsetInfo -{ - std::condition_variable condition; - std::mutex condition_mutex; -} CustomWaitsetInfo; - -#endif // TYPES__CUSTOM_WAIT_SET_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp deleted file mode 100644 index ab5076522..000000000 --- a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TYPES__GUARD_CONDITION_HPP_ -#define TYPES__GUARD_CONDITION_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "rcpputils/thread_safety_annotations.hpp" - -class GuardCondition -{ -public: - GuardCondition() - : hasTriggered_(false), - conditionMutex_(nullptr), conditionVariable_(nullptr) {} - - void - trigger() - { - std::lock_guard lock(internalMutex_); - - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); - // the change to hasTriggered_ needs to be mutually exclusive with - // rmw_wait() which checks hasTriggered() and decides if wait() needs to - // be called - hasTriggered_ = true; - clock.unlock(); - conditionVariable_->notify_one(); - } else { - hasTriggered_ = true; - } - } - - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasTriggered() - { - return hasTriggered_; - } - - bool - getHasTriggered() - { - return hasTriggered_.exchange(false); - } - -private: - std::mutex internalMutex_; - std::atomic_bool hasTriggered_; - std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); -}; - -#endif // TYPES__GUARD_CONDITION_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index f652abed4..cab0265cf 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -165,7 +165,6 @@ create_datareader( const rmw_subscription_options_t * subscription_options, eprosima::fastdds::dds::Subscriber * subscriber, eprosima::fastdds::dds::TopicDescription * des_topic, - SubListener * listener, eprosima::fastdds::dds::DataReader ** data_reader ) { @@ -194,16 +193,14 @@ create_datareader( // Creates DataReader (with subscriber name to not change name policy) *data_reader = subscriber->create_datareader( des_topic, - updated_qos, - listener); + updated_qos); if (!data_reader && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { *data_reader = subscriber->create_datareader( des_topic, - datareader_qos, - listener); + datareader_qos); } return true; } From 6532c29b955077427596480f7f5716ef403592f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Mon, 27 Jun 2022 08:31:15 +0200 Subject: [PATCH 02/34] Working subscriptions and services MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_cpp/src/rmw_client.cpp | 12 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 12 +- rmw_fastrtps_cpp/src/subscription.cpp | 13 ++ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 11 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 11 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 27 +++- .../custom_client_info.hpp | 5 +- .../custom_event_info.hpp | 1 + .../custom_service_info.hpp | 5 +- .../custom_subscriber_info.hpp | 46 ++++-- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 2 + .../src/custom_subscriber_info.cpp | 2 - rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 38 +++++ rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 30 +++- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 20 ++- .../src/rmw_subscription.cpp | 1 + rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 12 -- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 151 ++++++++++-------- rmw_fastrtps_shared_cpp/src/subscription.cpp | 3 + .../src/types/event_types.hpp | 5 + rmw_fastrtps_shared_cpp/src/utils.cpp | 9 +- 21 files changed, 309 insertions(+), 107 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index dba37b5eb..bd3e2a27f 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -195,6 +195,7 @@ rmw_create_client( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -245,6 +246,13 @@ rmw_create_client( info->response_type_support_ = response_fastdds_type; ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); + return nullptr; + } + info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); @@ -320,7 +328,9 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - reader_qos); + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!info->response_reader_) { RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 75a99e0b0..d6dc70729 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -199,6 +199,7 @@ rmw_create_service( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -247,6 +248,13 @@ rmw_create_service( info->response_type_support_ = response_fastdds_type; ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); + return nullptr; + } + info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); @@ -319,7 +327,9 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - reader_qos); + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!info->request_reader_) { RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index a91a319a0..669fee038 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -164,6 +164,7 @@ create_subscription( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { dds_participant->unregister_type(info->type_support_.get_type_name()); } @@ -204,6 +205,14 @@ create_subscription( return nullptr; } + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + ///// // Create and register Topic eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); @@ -278,12 +287,16 @@ create_subscription( subscription_options, subscriber, des_topic, + info->listener_, &info->data_reader_)) { RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); return nullptr; } + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 8f1c7863a..f96f27adf 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -202,6 +202,7 @@ rmw_create_client( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -277,6 +278,12 @@ rmw_create_client( ///// // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); + return nullptr; + } + info->pub_listener_ = new (std::nothrow) ClientPubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); @@ -352,7 +359,9 @@ rmw_create_client( // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, - reader_qos); + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!info->response_reader_) { RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index c66247313..b78376e77 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -206,6 +206,7 @@ rmw_create_service( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { delete info->pub_listener_; + delete info->listener_; if (info->response_type_support_) { dds_participant->unregister_type(info->response_type_support_.get_type_name()); } @@ -279,6 +280,12 @@ rmw_create_service( ///// // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); + return nullptr; + } + info->pub_listener_ = new (std::nothrow) ServicePubListener(info); if (!info->pub_listener_) { RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); @@ -351,7 +358,9 @@ rmw_create_service( // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, - reader_qos); + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!info->request_reader_) { RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 4fefe7dfe..8e6cf06c7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -164,6 +164,7 @@ create_subscription( auto cleanup_info = rcpputils::make_scope_exit( [info, dds_participant]() { + delete info->listener_; if (info->type_support_) { dds_participant->unregister_type(info->type_support_.get_type_name()); } @@ -209,6 +210,15 @@ create_subscription( info->type_support_ = fastdds_type; + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); + + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + ///// // Create and register Topic eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); @@ -253,6 +263,12 @@ create_subscription( return nullptr; } + info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + return nullptr; + } + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; switch (subscription_options->require_unique_network_flow_endpoints) { default: @@ -277,14 +293,18 @@ create_subscription( // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( des_topic, - reader_qos); + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::none()); if (!info->data_reader_ && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { info->data_reader_ = subscriber->create_datareader( des_topic, - original_qos); + original_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::none()); } if (!info->data_reader_) { @@ -292,6 +312,9 @@ create_subscription( return nullptr; } + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { 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 cd329f89f..45ec9d3a2 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 @@ -31,6 +31,7 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" @@ -45,6 +46,7 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +class ClientListener; class ClientPubListener; typedef struct CustomClientInfo @@ -59,6 +61,7 @@ typedef struct CustomClientInfo std::string request_topic_; std::string response_topic_; + ClientListener * listener_{nullptr}; eprosima::fastrtps::rtps::GUID_t writer_guid_; eprosima::fastrtps::rtps::GUID_t reader_guid_; @@ -75,7 +78,6 @@ typedef struct CustomClientResponse eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; -/* class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: @@ -237,7 +239,6 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener std::mutex on_new_response_m_; uint64_t unread_count_ = 0; }; -*/ class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index bd3ed3342..d68e4b88d 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -104,6 +104,7 @@ class EventListenerInterface::ConditionalScopedLock struct CustomEventInfo { virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; + virtual EventListenerInterface * getListener() const = 0; }; 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 4aa7e7d7e..146eb33ad 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 @@ -29,6 +29,7 @@ #include "fastdds/dds/publisher/DataWriter.hpp" #include "fastdds/dds/publisher/DataWriterListener.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" #include "fastdds/dds/subscriber/SampleInfo.hpp" #include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" #include "fastdds/dds/topic/TypeSupport.hpp" @@ -44,6 +45,7 @@ #include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +class ServiceListener; class ServicePubListener; enum class client_present_t @@ -63,6 +65,7 @@ typedef struct CustomServiceInfo eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; + ServiceListener * listener_{nullptr}; ServicePubListener * pub_listener_{nullptr}; const char * typesupport_identifier_{nullptr}; @@ -177,7 +180,6 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener std::condition_variable cv_; }; -/* class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: @@ -340,6 +342,5 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener std::mutex on_new_request_m_; uint64_t unread_count_ = 0; }; -*/ #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ 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 00b713ca0..80fe1bd24 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 @@ -81,17 +81,10 @@ struct CustomSubscriberInfo : public CustomEventInfo RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * - getListener() const final - { - return nullptr; - } + getListener() const final; }; -class SubListener -{ -}; -/* class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: @@ -178,6 +171,42 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds bool takeNextEvent(rmw_event_type_t event_type, void * event_info) final; + // SubListener API + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + bool + hasData() const + { + return data_.load(std::memory_order_relaxed); + } + + void + update_has_data(eprosima::fastdds::dds::DataReader * reader) + { + // Make sure to call into Fast DDS before taking the lock to avoid an + // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. + auto unread_count = reader->get_unread_count(); + bool has_data = unread_count > 0; + + std::lock_guard lock(internalMutex_); + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + data_.store(has_data, std::memory_order_relaxed); + } + size_t publisherCount() { std::lock_guard lock(internalMutex_); @@ -239,6 +268,5 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds size_t qos_depth_; size_t new_data_unread_count_ = 0; }; -*/ #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_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 0c7e3197e..88b8e183a 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 @@ -168,6 +168,7 @@ create_content_filtered_topic( * \param[in] subscription_options Options of the subscription. * \param[in] subscriber A subsciber to create the data reader. * \param[in] des_topic TopicDescription returned by find_and_check_topic_and_type. +* \param[in] listener The listener of the data reader. * \param[out] data_reader Will hold the pointer to the data reader. * * \return true when the data reader was created @@ -180,6 +181,7 @@ create_datareader( const rmw_subscription_options_t * subscription_options, eprosima::fastdds::dds::Subscriber * subscriber, eprosima::fastdds::dds::TopicDescription * des_topic, + SubListener * listener, eprosima::fastdds::dds::DataReader ** data_reader); } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 0844000d3..6500d2fc7 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -25,7 +25,6 @@ eprosima::fastdds::dds::StatusCondition& CustomSubscriberInfo::get_statusconditi return data_reader_->get_statuscondition(); } -/* EventListenerInterface * CustomSubscriberInfo::getListener() const { @@ -215,4 +214,3 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) } return true; } -*/ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index ccc53753c..94bd3896f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -36,6 +36,40 @@ namespace rmw_fastrtps_shared_cpp namespace internal { +eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( + const rmw_event_type_t event_type) +{ + eprosima::fastdds::dds::StatusMask ret_statusmask = eprosima::fastdds::dds::StatusMask::none(); + switch (event_type) + { + case RMW_EVENT_LIVELINESS_CHANGED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_changed(); + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_deadline_missed(); + break; + case RMW_EVENT_LIVELINESS_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_lost(); + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_deadline_missed(); + break; + case RMW_EVENT_MESSAGE_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::sample_lost(); + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_incompatible_qos(); + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_incompatible_qos(); + break; + default: + break; + } + + return ret_statusmask; +} + bool is_event_supported(rmw_event_type_t event_type) { return g_rmw_event_type_set.count(event_type) == 1; @@ -91,6 +125,10 @@ __rmw_init_event( rmw_event->implementation_identifier = topic_endpoint_impl_identifier; rmw_event->data = data; rmw_event->event_type = event_type; + CustomEventInfo* event = static_cast(rmw_event->data); + eprosima::fastdds::dds::StatusMask statusmask = event->get_statuscondition().get_enabled_statuses(); + statusmask << internal::rmw_event_to_dds_statusmask(event_type); + event->get_statuscondition().set_enabled_statuses(statusmask); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index a574eb77f..d3706fe9e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -91,10 +91,31 @@ __rmw_take_request( auto info = static_cast(service->data); assert(info); - /* TODO - CustomServiceRequest request = info->listener_->getRequest(); + CustomServiceRequest request; + + + request.buffer_ = new eprosima::fastcdr::FastBuffer(); if (request.buffer_ != nullptr) { + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->request_reader_->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (request.sample_info_.valid_data) { + request.sample_identity_ = request.sample_info_.sample_identity; + // Use response subscriber guid (on related_sample_identity) when present. + const eprosima::fastrtps::rtps::GUID_t & reader_guid = + request.sample_info_.related_sample_identity.writer_guid(); + if (reader_guid != eprosima::fastrtps::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 = + request.sample_info_.sample_identity.writer_guid(); + info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); + auto raw_type_support = dynamic_cast( info->response_type_support_.get()); eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, @@ -114,9 +135,12 @@ __rmw_take_request( *taken = true; } + } + } + delete request.buffer_; } - */ + return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index fc9d52846..21fb355ed 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -54,8 +54,21 @@ __rmw_take_response( CustomClientResponse response; - /* TODO if (info->listener_->getResponse(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.is_cdr_buffer = true; + data.data = response.buffer_.get(); + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->response_reader_->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (response.sample_info_.valid_data) { + response.sample_identity_ = response.sample_info_.related_sample_identity; + + if (response.sample_identity_.writer_guid() == info->reader_guid_ || + response.sample_identity_.writer_guid() == info->writer_guid_) + { auto raw_type_support = dynamic_cast( info->response_type_support_.get()); eprosima::fastcdr::Cdr deser( @@ -73,8 +86,9 @@ __rmw_take_response( *taken = true; } - } - */ + } + } + } return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 325ea029f..986033ffa 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -185,6 +185,7 @@ __rmw_subscription_set_content_filter( subscription_options, subscriber, des_topic, + info->listener_, &info->data_reader_)) { RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index ff0c77946..b5453475d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -91,11 +91,8 @@ _take( data.data = ros_message; data.impl = info->type_support_impl_; - /* TODO while (0 < info->data_reader_->get_unread_count()) { if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - // Update hasData from listener - info->listener_->update_has_data(info->data_reader_); if (subscription->options.ignore_local_publications) { auto sample_writer_guid = @@ -116,7 +113,6 @@ _take( } } } - */ TRACEPOINT( rmw_take, @@ -320,11 +316,7 @@ _take_serialized_message( data.data = &buffer; data.impl = nullptr; // not used when is_cdr_buffer is true - /* TODO if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - // Update hasData from listener - info->listener_->update_has_data(info->data_reader_); - if (sinfo.valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); if (serialized_message->buffer_capacity < buffer_size) { @@ -342,7 +334,6 @@ _take_serialized_message( *taken = true; } } - */ return RMW_RET_OK; } @@ -485,7 +476,6 @@ __rmw_take_loaned_message_internal( auto item = std::make_unique(); - /* TODO while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) { if (item->info_seq[0].valid_data) { if (nullptr != message_info) { @@ -493,7 +483,6 @@ __rmw_take_loaned_message_internal( } *loaned_message = item->data_seq.buffer()[0]; *taken = true; - info->listener_->update_has_data(info->data_reader_); info->loan_manager_->add_item(std::move(item)); @@ -503,7 +492,6 @@ __rmw_take_loaned_message_internal( // Should return loan before taking again info->data_reader_->return_loan(item->data_seq, item->info_seq); } - */ // No data available, return loan information. *taken = false; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index f7beb2879..499b8187d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -21,6 +21,7 @@ #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/event_types.hpp" #include "fastdds/dds/core/condition/WaitSet.hpp" #include "fastdds/dds/core/condition/GuardCondition.hpp" @@ -102,70 +103,57 @@ __rmw_wait( Duration_t{wait_timeout->sec, wait_timeout->nsec} : eprosima::fastrtps::c_TimeInfinite ); - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->detach_condition(*guard_condition); - } - } - - if (events) { - for (size_t i = 0; i < events->event_count; ++i) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->detach_condition(custom_event_info->get_statuscondition()); - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - fastdds_wait_set->detach_condition(custom_service_info->request_reader_->get_statuscondition()); - } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - auto custom_client_info = static_cast(data); - fastdds_wait_set->detach_condition(custom_client_info->response_reader_->get_statuscondition()); - } - } - if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& condition = custom_subscriber_info->data_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(condition); - } - } - - if (ReturnCode_t::RETCODE_OK == ret_code) - { - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - custom_subscriber_info->listener_->detachCondition(); - if (!custom_subscriber_info->listener_->hasData()) { - subscriptions->subscribers[i] = 0; - } - } + eprosima::fastdds::dds::StatusCondition& status_condition = custom_subscriber_info->data_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + subscriptions->subscribers[i] = 0; + } + } + else + { + subscriptions->subscribers[i] = 0; } + } } - - - if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; auto custom_client_info = static_cast(data); - custom_client_info->listener_->detachCondition(); - if (!custom_client_info->listener_->hasData()) { + eprosima::fastdds::dds::StatusCondition& status_condition = custom_client_info->response_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + clients->clients[i] = 0; + } + } + else + { clients->clients[i] = 0; } } @@ -175,8 +163,25 @@ __rmw_wait( for (size_t i = 0; i < services->service_count; ++i) { void * data = services->services[i]; auto custom_service_info = static_cast(data); - custom_service_info->listener_->detachCondition(); - if (!custom_service_info->listener_->hasData()) { + eprosima::fastdds::dds::StatusCondition& status_condition = custom_service_info->request_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + services->services[i] = 0; + } + } + else + { services->services[i] = 0; } } @@ -186,9 +191,27 @@ __rmw_wait( for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); auto custom_event_info = static_cast(event->data); - custom_event_info->getListener()->detachCondition(); - if (!custom_event_info->getListener()->hasEvent(event->event_type)) { - events->events[i] = nullptr; + fastdds_wait_set->detach_condition(custom_event_info->get_statuscondition()); + eprosima::fastdds::dds::StatusCondition& status_condition = custom_event_info->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event->event_type))) + { + events->events[i] = 0; + } + } + else + { + events->events[i] = 0; } } } @@ -196,16 +219,12 @@ __rmw_wait( if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - guard_condition->detachCondition(); - if (!guard_condition->getHasTriggered()) { - guard_conditions->guard_conditions[i] = 0; - } + auto guard_condition = static_cast(data); + fastdds_wait_set->detach_condition(*guard_condition); + guard_condition->set_trigger_value(false); } } - */ - bool timeout = false; - return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; + return ReturnCode_t::RETCODE_OK == ret_code ? RMW_RET_OK : RMW_RET_TIMEOUT; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index 6c798c4db..bfb625459 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -65,6 +65,9 @@ destroy_subscription( return RMW_RET_OK; } + // Delete DataReader listener + delete info->listener_; + // Delete topic and unregister type remove_topic_and_type(participant_info, info->topic_, info->type_support_); diff --git a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp index 8eb9d68da..0cc3225d6 100644 --- a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp +++ b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp @@ -17,6 +17,8 @@ #include "rmw/event.h" +#include "fastdds/dds/core/status/StatusMask.hpp" + namespace rmw_fastrtps_shared_cpp { namespace internal @@ -24,6 +26,9 @@ namespace internal bool is_event_supported(rmw_event_type_t event_type); +eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( + const rmw_event_type_t event_type); + } // namespace internal } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index cab0265cf..b7148b786 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -165,6 +165,7 @@ create_datareader( const rmw_subscription_options_t * subscription_options, eprosima::fastdds::dds::Subscriber * subscriber, eprosima::fastdds::dds::TopicDescription * des_topic, + SubListener * listener, eprosima::fastdds::dds::DataReader ** data_reader ) { @@ -193,14 +194,18 @@ create_datareader( // Creates DataReader (with subscriber name to not change name policy) *data_reader = subscriber->create_datareader( des_topic, - updated_qos); + updated_qos, + listener, + eprosima::fastdds::dds::StatusMask::none()); if (!data_reader && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) { *data_reader = subscriber->create_datareader( des_topic, - datareader_qos); + datareader_qos, + listener, + eprosima::fastdds::dds::StatusMask::none()); } return true; } From db2ef410cfc91e7a1f03e474f4d6677168074bd8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Mon, 27 Jun 2022 15:48:56 +0200 Subject: [PATCH 03/34] Add event support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_cpp/src/publisher.cpp | 5 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 7 ++- rmw_fastrtps_cpp/src/rmw_service.cpp | 7 ++- .../src/init_rmw_context_impl.cpp | 5 -- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 5 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 7 ++- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 7 ++- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 4 +- .../custom_event_info.hpp | 2 + .../custom_publisher_info.hpp | 3 + .../custom_subscriber_info.hpp | 3 + .../src/custom_publisher_info.cpp | 46 ++++++++++++++ .../src/custom_subscriber_info.cpp | 61 +++++++++++++++++++ .../src/init_rmw_context_impl.cpp | 5 -- .../src/rmw_subscription.cpp | 2 - rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 2 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 4 +- 17 files changed, 152 insertions(+), 23 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index efd27d4d4..1880117c7 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -273,13 +273,16 @@ rmw_fastrtps_cpp::create_publisher( info->data_writer_ = publisher->create_datawriter( topic.topic, writer_qos, - info->listener_); + info->listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->data_writer_) { RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } + info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index bd3e2a27f..1973ff126 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -337,6 +337,8 @@ rmw_create_client( return nullptr; } + info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { @@ -380,13 +382,16 @@ rmw_create_client( info->request_writer_ = publisher->create_datawriter( request_topic.topic, writer_qos, - info->pub_listener_); + info->pub_listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->request_writer_) { RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); return nullptr; } + info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index d6dc70729..263d68bff 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -336,6 +336,8 @@ rmw_create_service( return nullptr; } + info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { @@ -383,13 +385,16 @@ rmw_create_service( info->response_writer_ = publisher->create_datawriter( response_topic.topic, writer_qos, - info->pub_listener_); + info->pub_listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->response_writer_) { RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); return nullptr; } + info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 7e66e759c..e7daf769d 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -159,11 +159,6 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) { - return ret; - } - common_context->graph_cache.set_on_change_callback( [guard_condition = graph_guard_condition.get()]() { rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 6235cf776..9fa07547f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -278,13 +278,16 @@ rmw_fastrtps_dynamic_cpp::create_publisher( info->data_writer_ = publisher->create_datawriter( topic.topic, writer_qos, - info->listener_); + info->listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->data_writer_) { RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); return nullptr; } + info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index f96f27adf..956450843 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -368,6 +368,8 @@ rmw_create_client( return nullptr; } + info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { @@ -411,13 +413,16 @@ rmw_create_client( info->request_writer_ = publisher->create_datawriter( request_topic.topic, writer_qos, - info->pub_listener_); + info->pub_listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->request_writer_) { RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); return nullptr; } + info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index b78376e77..49985c55b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -367,6 +367,8 @@ rmw_create_service( return nullptr; } + info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() { @@ -414,13 +416,16 @@ rmw_create_service( info->response_writer_ = publisher->create_datawriter( response_topic.topic, writer_qos, - info->pub_listener_); + info->pub_listener_, + eprosima::fastdds::dds::StatusMask::publication_matched()); if (!info->response_writer_) { RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); return nullptr; } + info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( [publisher, info]() { diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 8e6cf06c7..8b70517aa 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -295,7 +295,7 @@ create_subscription( des_topic, reader_qos, info->listener_, - eprosima::fastdds::dds::StatusMask::none()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!info->data_reader_ && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) @@ -304,7 +304,7 @@ create_subscription( des_topic, original_qos, info->listener_, - eprosima::fastdds::dds::StatusMask::none()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); } if (!info->data_reader_) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index d68e4b88d..441e6bf7f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -106,6 +106,8 @@ struct CustomEventInfo virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; virtual EventListenerInterface * getListener() const = 0; + + virtual bool take_event(rmw_event_type_t event_type, void * event_info) const = 0; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ 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 6e2bbcd27..6973c0d48 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 @@ -56,6 +56,9 @@ typedef struct CustomPublisherInfo : public CustomEventInfo RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * getListener() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event(rmw_event_type_t event_type, void * event_info) const final; } CustomPublisherInfo; class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener 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 80fe1bd24..cf96f8ce7 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 @@ -82,6 +82,9 @@ struct CustomSubscriberInfo : public CustomEventInfo RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * getListener() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event(rmw_event_type_t event_type, void * event_info) const final; }; diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 501d80f2c..24d7b93db 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -31,6 +31,52 @@ CustomPublisherInfo::getListener() const return listener_; } +bool CustomPublisherInfo::take_event(rmw_event_type_t event_type, void * event_info) const +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; + data_writer_->get_liveliness_lost_status(liveliness_lost_status); + rmw_data->total_count = liveliness_lost_status.total_count; + rmw_data->total_count_change = liveliness_lost_status.total_count_change; + //liveliness_lost_status_.total_count_change = 0; + //liveliness_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; + data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status); + rmw_data->total_count = offered_deadline_missed_status.total_count; + rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; + //offered_deadline_missed_status_.total_count_change = 0; + //deadline_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; + data_writer_->get_offered_incompatible_qos_status(offered_incompatible_qos_status); + rmw_data->total_count = offered_incompatible_qos_status.total_count; + rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + offered_incompatible_qos_status.last_policy_id); + //incompatible_qos_status_.total_count_change = 0; + //incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + break; + default: + return false; + } + return true; +} + void PubListener::on_offered_deadline_missed( eprosima::fastdds::dds::DataWriter * /* writer */, diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 6500d2fc7..35262d2ca 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -31,6 +31,67 @@ CustomSubscriberInfo::getListener() const return listener_; } +bool CustomSubscriberInfo::take_event(rmw_event_type_t event_type, void * event_info) const +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; + data_reader_->get_liveliness_changed_status(liveliness_changed_status); + rmw_data->alive_count = liveliness_changed_status.alive_count; + rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; + //liveliness_changed_status_.alive_count_change = 0; + //liveliness_changed_status_.not_alive_count_change = 0; + //liveliness_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; + data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status); + rmw_data->total_count = requested_deadline_missed_status.total_count; + rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; + //requested_deadline_missed_status_.total_count_change = 0; + //deadline_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::SampleLostStatus sample_lost_status; + data_reader_->get_sample_lost_status(sample_lost_status); + rmw_data->total_count = sample_lost_status.total_count; + rmw_data->total_count_change = sample_lost_status.total_count_change; + //sample_lost_status_.total_count_change = 0; + //sample_lost_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + eprosima::fastdds::dds::RequestedIncompatibleQosStatus requested_qos_incompatible_qos_status; + data_reader_->get_requested_incompatible_qos_status(requested_qos_incompatible_qos_status); + rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; + rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + requested_qos_incompatible_qos_status.last_policy_id); + //incompatible_qos_status_.total_count_change = 0; + //incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + break; + default: + return false; + } + + return true; +} + void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader * , diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index fc3b0db48..9d3dc21c4 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -47,11 +47,6 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex rmw_ret_t ret = RMW_RET_OK; rmw_error_string_t error_string; - ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); - if (RMW_RET_OK != ret) { - return ret; - } - auto common_context = static_cast(context->impl->common); auto participant_info = static_cast(context->impl->participant_info); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 986033ffa..238d5d6eb 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -92,9 +92,7 @@ __rmw_subscription_count_matched_publishers( { auto info = static_cast(subscription->data); - /* TODO *publisher_count = info->listener_->publisherCount(); - */ return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index b5453475d..ce259e973 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -194,7 +194,7 @@ __rmw_take_event( return RMW_RET_ERROR); auto event = static_cast(event_handle->data); - if (event->getListener()->takeNextEvent(event_handle->event_type, event_info)) { + if (event->take_event(event_handle->event_type, event_info)) { *taken = true; return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index b7148b786..f9111588e 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -196,7 +196,7 @@ create_datareader( des_topic, updated_qos, listener, - eprosima::fastdds::dds::StatusMask::none()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); if (!data_reader && (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == subscription_options->require_unique_network_flow_endpoints)) @@ -205,7 +205,7 @@ create_datareader( des_topic, datareader_qos, listener, - eprosima::fastdds::dds::StatusMask::none()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); } return true; } From 905863ec389c8f1bd6e33005f70ebd718c865ff6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Tue, 28 Jun 2022 11:38:39 +0200 Subject: [PATCH 04/34] Initial new event callback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_event_info.hpp | 134 +-- .../custom_publisher_info.hpp | 236 +++--- .../custom_subscriber_info.hpp | 413 +++++----- .../src/custom_publisher_info.cpp | 354 ++++---- .../src/custom_subscriber_info.cpp | 449 +++++----- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 137 ++-- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 766 +++++++++--------- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 373 +++++---- 8 files changed, 1512 insertions(+), 1350 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index 441e6bf7f..d5d289395 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -34,80 +34,96 @@ class EventListenerInterface { protected: - class ConditionalScopedLock; + + class ConditionalScopedLock; public: - /// Connect a condition variable so a waiter can be notified of new data. - virtual void attachCondition( - std::mutex * conditionMutex, - std::condition_variable * conditionVariable) = 0; - - /// Unset the information from attachCondition. - virtual void detachCondition() = 0; - - /// Check if there is new data available for a specific event type. - /** - * \param event_type The event type to check on. - * \return `true` if new data is available. - */ - virtual bool hasEvent(rmw_event_type_t event_type) const = 0; - - /// Take ready data for an event type. - /** - * \param event_type The event type to get data for. - * \param event_info A preallocated event information (from rmw/types.h) to fill with data - * \return `true` if data was successfully taken. - * \return `false` if data was not available, in this case nothing was written to event_info. - */ - virtual bool takeNextEvent(rmw_event_type_t event_type, void * event_info) = 0; - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - virtual void set_on_new_event_callback( - const void * user_data, - rmw_event_callback_t callback) = 0; - - rmw_event_callback_t on_new_event_cb_{nullptr}; - const void * user_data_{nullptr}; - uint64_t unread_events_count_ = 0; - std::mutex on_new_event_m_; + + virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; + + + //TODO(richiwre) review + /// Connect a condition variable so a waiter can be notified of new data. + virtual void attachCondition( + std::mutex* conditionMutex, + std::condition_variable* conditionVariable) = 0; + + //TODO(richiwre) review + /// Unset the information from attachCondition. + virtual void detachCondition() = 0; + + /// Check if there is new data available for a specific event type. + /** + * \param event_type The event type to check on. + * \return `true` if new data is available. + */ + virtual bool has_event( + rmw_event_type_t event_type) const = 0; + + /// Take ready data for an event type. + /** + * \param event_type The event type to get data for. + * \param event_info A preallocated event information (from rmw/types.h) to fill with data + * \return `true` if data was successfully taken. + * \return `false` if data was not available, in this case nothing was written to event_info. + */ + virtual bool take_event( + rmw_event_type_t event_type, + void* event_info) = 0; + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + virtual void set_on_new_event_callback( + rmw_event_type_t event_type, + const void* user_data, + rmw_event_callback_t callback) = 0; + +protected: + + rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; + + const void* user_data_[RMW_EVENT_INVALID] = {nullptr}; + + std::mutex on_new_event_m_; }; class EventListenerInterface::ConditionalScopedLock { public: - ConditionalScopedLock( - std::mutex * mutex, - std::condition_variable * condition_variable = nullptr) - : mutex_(mutex), cv_(condition_variable) - { - if (nullptr != mutex_) { - mutex_->lock(); + + ConditionalScopedLock( + std::mutex* mutex, + std::condition_variable* condition_variable = nullptr) + : mutex_(mutex) + , cv_(condition_variable) + { + if (nullptr != mutex_) + { + mutex_->lock(); + } } - } - - ~ConditionalScopedLock() - { - if (nullptr != mutex_) { - mutex_->unlock(); - if (nullptr != cv_) { - cv_->notify_all(); - } + + ~ConditionalScopedLock() + { + if (nullptr != mutex_) + { + mutex_->unlock(); + if (nullptr != cv_) + { + cv_->notify_all(); + } + } } - } private: - std::mutex * mutex_; - std::condition_variable * cv_; + + std::mutex* mutex_; + std::condition_variable* cv_; }; struct CustomEventInfo { - virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; - - virtual EventListenerInterface * getListener() const = 0; - - virtual bool take_event(rmw_event_type_t event_type, void * event_info) const = 0; + virtual EventListenerInterface* get_listener() const = 0; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ 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 6973c0d48..3e162761c 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 @@ -41,129 +41,145 @@ class PubListener; typedef struct CustomPublisherInfo : public CustomEventInfo { - virtual ~CustomPublisherInfo() = default; - - eprosima::fastdds::dds::DataWriter * data_writer_{nullptr}; - PubListener * listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void * type_support_impl_{nullptr}; - rmw_gid_t publisher_gid{}; - const char * typesupport_identifier_{nullptr}; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface * - getListener() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event(rmw_event_type_t event_type, void * event_info) const final; + virtual ~CustomPublisherInfo() = default; + + eprosima::fastdds::dds::DataWriter* data_writer_{nullptr}; + PubListener* listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void* type_support_impl_{nullptr}; + rmw_gid_t publisher_gid{}; + const char* typesupport_identifier_{nullptr}; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface* + get_listener() const final; } CustomPublisherInfo; class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener { public: - explicit PubListener(CustomPublisherInfo * info) - : deadline_changes_(false), - liveliness_changes_(false), - incompatible_qos_changes_(false), - conditionMutex_(nullptr), - conditionVariable_(nullptr) - { - (void) info; - } - - // DataWriterListener implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus & info) final - { - std::lock_guard lock(internalMutex_); - if (info.current_count_change == 1) { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } else if (info.current_count_change == -1) { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + + explicit PubListener( + CustomPublisherInfo* info) + : publisher_info_(info) + , deadline_changes_(false) + , liveliness_changes_(false) + , incompatible_qos_changes_(false) + , conditionMutex_(nullptr) + , conditionVariable_(nullptr) + { + } + + // DataWriterListener implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus& info) final + { + std::lock_guard lock(internalMutex_); + if (info.current_count_change == 1) + { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + else if (info.current_count_change == -1) + { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_deadline_missed( + eprosima::fastdds::dds::DataWriter* writer, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_lost( + eprosima::fastdds::dds::DataWriter* writer, + const eprosima::fastdds::dds::LivelinessLostStatus& status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter*, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus&) final; + + // EventListenerInterface implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + has_event( + rmw_event_type_t event_type) const final; + + // PubListener API + size_t subscriptionCount() + { + std::lock_guard lock(internalMutex_); + return subscriptions_.size(); } - } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter * writer, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_lost( - eprosima::fastdds::dds::DataWriter * writer, - const eprosima::fastdds::dds::LivelinessLostStatus & status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter *, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus &) final; - - // EventListenerInterface implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - hasEvent(rmw_event_type_t event_type) const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - const void * user_data, - rmw_event_callback_t callback) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - takeNextEvent(rmw_event_type_t event_type, void * event_info) final; - - // PubListener API - size_t subscriptionCount() - { - std::lock_guard lock(internalMutex_); - return subscriptions_.size(); - } - - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } + + void + attachCondition( + std::mutex* conditionMutex, + std::condition_variable* conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void* event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void* user_data, + rmw_event_callback_t callback) final; private: - mutable std::mutex internalMutex_; - std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + CustomPublisherInfo* publisher_info_ = nullptr; + + mutable std::mutex internalMutex_; + + std::set subscriptions_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool deadline_changes_; - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool deadline_changes_; + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool incompatible_qos_changes_; - eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool incompatible_qos_changes_; + eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::mutex* conditionMutex_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); + std::condition_variable* conditionVariable_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ 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 cf96f8ce7..496f52e4a 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 @@ -49,227 +49,254 @@ class SubListener; -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { struct LoanManager; } // namespace rmw_fastrtps_shared_cpp struct CustomSubscriberInfo : public CustomEventInfo { - virtual ~CustomSubscriberInfo() = default; - - eprosima::fastdds::dds::DataReader * data_reader_ {nullptr}; - SubListener * listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void * type_support_impl_{nullptr}; - rmw_gid_t subscription_gid_{}; - const char * typesupport_identifier_{nullptr}; - std::shared_ptr loan_manager_; - - // for re-create or delete content filtered topic - const rmw_node_t * node_ {nullptr}; - rmw_dds_common::Context * common_context_ {nullptr}; - eprosima::fastdds::dds::DomainParticipant * dds_participant_ {nullptr}; - eprosima::fastdds::dds::Subscriber * subscriber_ {nullptr}; - std::string topic_name_mangled_; - eprosima::fastdds::dds::TopicDescription * topic_ {nullptr}; - eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic_ {nullptr}; - eprosima::fastdds::dds::DataReaderQos datareader_qos_; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface * - getListener() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event(rmw_event_type_t event_type, void * event_info) const final; + virtual ~CustomSubscriberInfo() = default; + + eprosima::fastdds::dds::DataReader* data_reader_ {nullptr}; + SubListener* listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void* type_support_impl_{nullptr}; + rmw_gid_t subscription_gid_{}; + const char* typesupport_identifier_{nullptr}; + std::shared_ptr loan_manager_; + + // for re-create or delete content filtered topic + const rmw_node_t* node_ {nullptr}; + rmw_dds_common::Context* common_context_ {nullptr}; + eprosima::fastdds::dds::DomainParticipant* dds_participant_ {nullptr}; + eprosima::fastdds::dds::Subscriber* subscriber_ {nullptr}; + std::string topic_name_mangled_; + eprosima::fastdds::dds::TopicDescription* topic_ {nullptr}; + eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic_ {nullptr}; + eprosima::fastdds::dds::DataReaderQos datareader_qos_; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface* + get_listener() const final; }; class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: - explicit SubListener(CustomSubscriberInfo * info, size_t qos_depth) - : data_(false), - deadline_changes_(false), - liveliness_changes_(false), - sample_lost_changes_(false), - incompatible_qos_changes_(false), - conditionMutex_(nullptr), - conditionVariable_(nullptr) - { - qos_depth_ = (qos_depth > 0) ? qos_depth : std::numeric_limits::max(); - // Field is not used right now - (void)info; - } - - // DataReaderListener implementation - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader * reader, - const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final - { + + explicit SubListener( + CustomSubscriberInfo* info, + size_t qos_depth) + : subscriber_info_(info) + , data_(false) + , deadline_changes_(false) + , liveliness_changes_(false) + , sample_lost_changes_(false) + , incompatible_qos_changes_(false) + , conditionMutex_(nullptr) + , conditionVariable_(nullptr) + { + //TODO(richiware) Review + qos_depth_ = (qos_depth > 0) ? qos_depth : std::numeric_limits::max(); + } + + // DataReaderListener implementation + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader* reader, + const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final + { + { + std::lock_guard lock(internalMutex_); + if (info.current_count_change == 1) + { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + else if (info.current_count_change == -1) + { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + } + update_has_data(reader); + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader* reader) final + { + update_has_data(reader); + + std::unique_lock lock_mutex(on_new_message_m_); + + if (on_new_message_cb_) + { + on_new_message_cb_(user_data_, 1); + } + else + { + new_data_unread_count_++; + } + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastrtps::RequestedDeadlineMissedStatus&) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_changed( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastrtps::LivelinessChangedStatus&) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_sample_lost( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::SampleLostStatus&) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus&) final; + + // EventListenerInterface implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + has_event( + rmw_event_type_t event_type) const final; + + // SubListener API + void + attachCondition( + std::mutex* conditionMutex, + std::condition_variable* conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() { - std::lock_guard lock(internalMutex_); - if (info.current_count_change == 1) { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } else if (info.current_count_change == -1) { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; } - update_has_data(reader); - } - void - on_data_available(eprosima::fastdds::dds::DataReader * reader) final - { - update_has_data(reader); + bool + hasData() const + { + return data_.load(std::memory_order_relaxed); + } - std::unique_lock lock_mutex(on_new_message_m_); + void + update_has_data( + eprosima::fastdds::dds::DataReader* reader) + { + // Make sure to call into Fast DDS before taking the lock to avoid an + // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. + auto unread_count = reader->get_unread_count(); + bool has_data = unread_count > 0; + + std::lock_guard lock(internalMutex_); + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + data_.store(has_data, std::memory_order_relaxed); + } - if (on_new_message_cb_) { - on_new_message_cb_(user_data_, 1); - } else { - new_data_unread_count_++; + size_t publisherCount() + { + std::lock_guard lock(internalMutex_); + return publishers_.size(); } - } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader *, - const eprosima::fastrtps::RequestedDeadlineMissedStatus &) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_changed( - eprosima::fastdds::dds::DataReader *, - const eprosima::fastrtps::LivelinessChangedStatus &) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_sample_lost( - eprosima::fastdds::dds::DataReader *, - const eprosima::fastdds::dds::SampleLostStatus &) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader *, - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus &) final; - - // EventListenerInterface implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - hasEvent(rmw_event_type_t event_type) const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - const void * user_data, - rmw_event_callback_t callback) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - takeNextEvent(rmw_event_type_t event_type, void * event_info) final; - - // SubListener API - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasData() const - { - return data_.load(std::memory_order_relaxed); - } - - void - update_has_data(eprosima::fastdds::dds::DataReader * reader) - { - // Make sure to call into Fast DDS before taking the lock to avoid an - // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. - auto unread_count = reader->get_unread_count(); - bool has_data = unread_count > 0; - - std::lock_guard lock(internalMutex_); - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - data_.store(has_data, std::memory_order_relaxed); - } - - size_t publisherCount() - { - std::lock_guard lock(internalMutex_); - return publishers_.size(); - } - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_message_callback( - const void * user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_message_m_); - - if (callback) { - // Push events arrived before setting the executor's callback - if (new_data_unread_count_) { - auto unread_count = std::min(new_data_unread_count_, qos_depth_); - callback(user_data, unread_count); - new_data_unread_count_ = 0; - } - user_data_ = user_data; - on_new_message_cb_ = callback; - } else { - user_data_ = nullptr; - on_new_message_cb_ = nullptr; + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_message_callback( + const void* user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_message_m_); + + if (callback) + { + // Push events arrived before setting the executor's callback + if (new_data_unread_count_) + { + auto unread_count = std::min(new_data_unread_count_, qos_depth_); + callback(user_data, unread_count); + new_data_unread_count_ = 0; + } + //TODO(richiware) + //user_data_ = user_data; + on_new_message_cb_ = callback; + } + else + { + //user_data_ = nullptr; + on_new_message_cb_ = nullptr; + } } - } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void* event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void* user_data, + rmw_event_callback_t callback) final; private: - mutable std::mutex internalMutex_; - std::atomic_bool data_; + CustomSubscriberInfo* subscriber_info_ = nullptr; + + mutable std::mutex internalMutex_; + + std::atomic_bool data_; - std::atomic_bool deadline_changes_; - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool deadline_changes_; + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool sample_lost_changes_; - eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool sample_lost_changes_; + eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::atomic_bool incompatible_qos_changes_; - eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool incompatible_qos_changes_; + eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::mutex* conditionMutex_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); + std::condition_variable* conditionVariable_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - std::set publishers_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - rmw_event_callback_t on_new_message_cb_{nullptr}; - std::mutex on_new_message_m_; - size_t qos_depth_; - size_t new_data_unread_count_ = 0; + rmw_event_callback_t on_new_message_cb_{nullptr}; + std::mutex on_new_message_m_; + size_t qos_depth_; + size_t new_data_unread_count_ = 0; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 24d7b93db..34da4c457 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -20,208 +20,222 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -eprosima::fastdds::dds::StatusCondition& CustomPublisherInfo::get_statuscondition() const +EventListenerInterface* +CustomPublisherInfo::get_listener() const { - return data_writer_->get_statuscondition(); + return listener_; } -EventListenerInterface * -CustomPublisherInfo::getListener() const +eprosima::fastdds::dds::StatusCondition& PubListener::get_statuscondition() const { - return listener_; + return publisher_info_->data_writer_->get_statuscondition(); } -bool CustomPublisherInfo::take_event(rmw_event_type_t event_type, void * event_info) const +bool PubListener::take_event( + rmw_event_type_t event_type, + void* event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type) { - case RMW_EVENT_LIVELINESS_LOST: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; - data_writer_->get_liveliness_lost_status(liveliness_lost_status); - rmw_data->total_count = liveliness_lost_status.total_count; - rmw_data->total_count_change = liveliness_lost_status.total_count_change; - //liveliness_lost_status_.total_count_change = 0; - //liveliness_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; - data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status); - rmw_data->total_count = offered_deadline_missed_status.total_count; - rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; - //offered_deadline_missed_status_.total_count_change = 0; - //deadline_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; - data_writer_->get_offered_incompatible_qos_status(offered_incompatible_qos_status); - rmw_data->total_count = offered_incompatible_qos_status.total_count; - rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - offered_incompatible_qos_status.last_policy_id); - //incompatible_qos_status_.total_count_change = 0; - //incompatible_qos_changes_.store(false, std::memory_order_relaxed); - } - break; - default: - return false; - } - return true; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type){ + case RMW_EVENT_LIVELINESS_LOST: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = liveliness_lost_status_.total_count; + rmw_data->total_count_change = liveliness_lost_status_.total_count_change; + liveliness_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status); + rmw_data->total_count = liveliness_lost_status.total_count; + rmw_data->total_count_change = liveliness_lost_status.total_count_change; + } + liveliness_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = offered_deadline_missed_status_.total_count; + rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; + deadline_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; + publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status); + rmw_data->total_count = offered_deadline_missed_status.total_count; + rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; + } + offered_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; + publisher_info_->data_writer_->get_offered_incompatible_qos_status(offered_incompatible_qos_status); + rmw_data->total_count = offered_incompatible_qos_status.total_count; + rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + offered_incompatible_qos_status.last_policy_id); + } + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + return true; +} + +void PubListener::set_on_new_event_callback( + rmw_event_type_t event_type, + const void* user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_event_m_); + + if (callback) + { + size_t last_total_count = 0; + + switch (event_type) + { + case RMW_EVENT_LIVELINESS_LOST: + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status_); + callback(user_data, liveliness_lost_status_.total_count_change); + liveliness_lost_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + last_total_count = offered_deadline_missed_status_.total_count; + publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status_); + callback(user_data, + offered_deadline_missed_status_.total_count_change); + offered_deadline_missed_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + last_total_count = incompatible_qos_status_.total_count; + publisher_info_->data_writer_->get_offered_incompatible_qos_status(incompatible_qos_status_); + callback(user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; + break; + default: + break; + } + + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; + + eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } + else + { + eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } void PubListener::on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) { - std::lock_guard lock(internalMutex_); - - // the change to liveliness_lost_count_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - - // Assign absolute values - offered_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - offered_deadline_missed_status_.total_count_change += status.total_count_change; + std::unique_lock lock_mutex(on_new_event_m_); - deadline_changes_.store(true, std::memory_order_relaxed); + // Assign absolute values + offered_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + offered_deadline_missed_status_.total_count_change += status.total_count_change; - std::unique_lock lock_mutex(on_new_event_m_); + deadline_changes_.store(true, std::memory_order_relaxed); - if (on_new_event_cb_) { - on_new_event_cb_(user_data_, 1); - } else { - unread_events_count_++; - } + if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) + { + on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED], 1); + } } void PubListener::on_liveliness_lost( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::LivelinessLostStatus & status) + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::LivelinessLostStatus& status) { - std::lock_guard lock(internalMutex_); - - // the change to liveliness_lost_count_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - liveliness_lost_status_.total_count = status.total_count; - // Accumulate deltas - liveliness_lost_status_.total_count_change += status.total_count_change; + // Assign absolute values + liveliness_lost_status_.total_count = status.total_count; + // Accumulate deltas + liveliness_lost_status_.total_count_change += status.total_count_change; - liveliness_changes_.store(true, std::memory_order_relaxed); + liveliness_changes_.store(true, std::memory_order_relaxed); - std::unique_lock lock_mutex(on_new_event_m_); - if (on_new_event_cb_) { - on_new_event_cb_(user_data_, 1); - } else { - unread_events_count_++; - } + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) + { + on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); + } } void PubListener::on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status) + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus& status) { - std::lock_guard lock(internalMutex_); - - // the change to incompatible_qos_status_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; - - incompatible_qos_changes_.store(true, std::memory_order_relaxed); -} + std::unique_lock lock_mutex(on_new_event_m_); -bool PubListener::hasEvent(rmw_event_type_t event_type) const -{ - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type) { - case RMW_EVENT_LIVELINESS_LOST: - return liveliness_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - return deadline_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - return incompatible_qos_changes_.load(std::memory_order_relaxed); - default: - break; - } - return false; -} + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; -void PubListener::set_on_new_event_callback( - const void * user_data, - rmw_event_callback_t callback) -{ - std::unique_lock lock_mutex(on_new_event_m_); + incompatible_qos_changes_.store(true, std::memory_order_relaxed); - if (callback) { - // Push events arrived before setting the executor's callback - if (unread_events_count_) { - callback(user_data, unread_events_count_); - unread_events_count_ = 0; + if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) + { + on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); } - user_data_ = user_data; - on_new_event_cb_ = callback; - } else { - user_data_ = nullptr; - on_new_event_cb_ = nullptr; - } } -bool PubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) +bool PubListener::has_event( + rmw_event_type_t event_type) const { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - std::lock_guard lock(internalMutex_); - switch (event_type) { - case RMW_EVENT_LIVELINESS_LOST: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = liveliness_lost_status_.total_count; - rmw_data->total_count_change = liveliness_lost_status_.total_count_change; - liveliness_lost_status_.total_count_change = 0; - liveliness_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = offered_deadline_missed_status_.total_count; - rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; - offered_deadline_missed_status_.total_count_change = 0; - deadline_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_status_.total_count_change = 0; - incompatible_qos_changes_.store(false, std::memory_order_relaxed); - } - break; - default: - return false; - } - return true; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type){ + case RMW_EVENT_LIVELINESS_LOST: + return liveliness_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + return deadline_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + return incompatible_qos_changes_.load(std::memory_order_relaxed); + default: + break; + } + return false; } diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 35262d2ca..104890181 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -20,258 +20,283 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -eprosima::fastdds::dds::StatusCondition& CustomSubscriberInfo::get_statuscondition() const +EventListenerInterface* +CustomSubscriberInfo::get_listener() const { - return data_reader_->get_statuscondition(); + return listener_; } -EventListenerInterface * -CustomSubscriberInfo::getListener() const +eprosima::fastdds::dds::StatusCondition& SubListener::get_statuscondition() const { - return listener_; + return subscriber_info_->data_reader_->get_statuscondition(); } -bool CustomSubscriberInfo::take_event(rmw_event_type_t event_type, void * event_info) const +bool SubListener::take_event( + rmw_event_type_t event_type, + void* event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type) { - case RMW_EVENT_LIVELINESS_CHANGED: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; - data_reader_->get_liveliness_changed_status(liveliness_changed_status); - rmw_data->alive_count = liveliness_changed_status.alive_count; - rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; - //liveliness_changed_status_.alive_count_change = 0; - //liveliness_changed_status_.not_alive_count_change = 0; - //liveliness_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; - data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status); - rmw_data->total_count = requested_deadline_missed_status.total_count; - rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; - //requested_deadline_missed_status_.total_count_change = 0; - //deadline_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_MESSAGE_LOST: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::SampleLostStatus sample_lost_status; - data_reader_->get_sample_lost_status(sample_lost_status); - rmw_data->total_count = sample_lost_status.total_count; - rmw_data->total_count_change = sample_lost_status.total_count_change; - //sample_lost_status_.total_count_change = 0; - //sample_lost_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - eprosima::fastdds::dds::RequestedIncompatibleQosStatus requested_qos_incompatible_qos_status; - data_reader_->get_requested_incompatible_qos_status(requested_qos_incompatible_qos_status); - rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; - rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - requested_qos_incompatible_qos_status.last_policy_id); - //incompatible_qos_status_.total_count_change = 0; - //incompatible_qos_changes_.store(false, std::memory_order_relaxed); - } - break; - default: - return false; - } - - return true; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type){ + case RMW_EVENT_LIVELINESS_CHANGED: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_.load(std::memory_order_relaxed)) + { + rmw_data->alive_count = liveliness_changed_status_.alive_count; + rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; + liveliness_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status); + rmw_data->alive_count = liveliness_changed_status.alive_count; + rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; + } + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = requested_deadline_missed_status_.total_count; + rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; + deadline_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; + subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status); + rmw_data->total_count = requested_deadline_missed_status.total_count; + rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; + } + requested_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + auto rmw_data = static_cast(event_info); + if (sample_lost_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = sample_lost_status_.total_count; + rmw_data->total_count_change = sample_lost_status_.total_count_change; + sample_lost_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::SampleLostStatus sample_lost_status; + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status); + rmw_data->total_count = sample_lost_status.total_count; + rmw_data->total_count_change = sample_lost_status.total_count_change; + } + sample_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_.load(std::memory_order_relaxed)) + { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + else + { + eprosima::fastdds::dds::RequestedIncompatibleQosStatus requested_qos_incompatible_qos_status; + subscriber_info_->data_reader_->get_requested_incompatible_qos_status( + requested_qos_incompatible_qos_status); + rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; + rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + requested_qos_incompatible_qos_status.last_policy_id); + } + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + + return true; } -void -SubListener::on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) +void SubListener::set_on_new_event_callback( + rmw_event_type_t event_type, + const void* user_data, + rmw_event_callback_t callback) { - std::lock_guard lock(internalMutex_); + std::unique_lock lock_mutex(on_new_event_m_); - // the change to liveliness_lost_count_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + if (callback) + { + size_t last_total_count = 0; - // Assign absolute values - requested_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - requested_deadline_missed_status_.total_count_change += status.total_count_change; + switch (event_type){ + case RMW_EVENT_LIVELINESS_CHANGED: + { + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status_); + callback(user_data, liveliness_changed_status_.alive_count_change + + liveliness_changed_status_.not_alive_count_change); + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status_); + callback(user_data, + requested_deadline_missed_status_.total_count_change); + requested_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status_); + callback(user_data, + sample_lost_status_.total_count_change); + sample_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + subscriber_info_->data_reader_->get_requested_incompatible_qos_status(incompatible_qos_status_); + callback(user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + break; + } - deadline_changes_.store(true, std::memory_order_relaxed); + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; - std::unique_lock lock_mutex(on_new_event_m_); + eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } + else + { + eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); - if (on_new_event_cb_) { - on_new_event_cb_(user_data_, 1); - } else { - unread_events_count_++; - } + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } -void SubListener::on_liveliness_changed( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::LivelinessChangedStatus & status) +void +SubListener::on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus& status) { - std::lock_guard lock(internalMutex_); - - // the change to liveliness_lost_count_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - - // Assign absolute values - liveliness_changed_status_.alive_count = status.alive_count; - liveliness_changed_status_.not_alive_count = status.not_alive_count; - // Accumulate deltas - liveliness_changed_status_.alive_count_change += status.alive_count_change; - liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; + std::unique_lock lock_mutex(on_new_event_m_); - liveliness_changes_.store(true, std::memory_order_relaxed); + // Assign absolute values + requested_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + requested_deadline_missed_status_.total_count_change += status.total_count_change; - std::unique_lock lock_mutex(on_new_event_m_); + deadline_changes_.store(true, std::memory_order_relaxed); - if (on_new_event_cb_) { - on_new_event_cb_(user_data_, 1); - } else { - unread_events_count_++; - } + if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) + { + on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); + } } -void SubListener::on_sample_lost( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::SampleLostStatus & status) +void SubListener::on_liveliness_changed( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::LivelinessChangedStatus& status) { - std::lock_guard lock(internalMutex_); + std::unique_lock lock_mutex(on_new_event_m_); - // the change to sample_lost_status_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + // Assign absolute values + liveliness_changed_status_.alive_count = status.alive_count; + liveliness_changed_status_.not_alive_count = status.not_alive_count; + // Accumulate deltas + liveliness_changed_status_.alive_count_change += status.alive_count_change; + liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; - // Assign absolute values - sample_lost_status_.total_count = status.total_count; - // Accumulate deltas - sample_lost_status_.total_count_change += status.total_count_change; + liveliness_changes_.store(true, std::memory_order_relaxed); - sample_lost_changes_.store(true, std::memory_order_relaxed); + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) + { + on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); + } } -void SubListener::on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) +void SubListener::on_sample_lost( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::SampleLostStatus& status) { - std::lock_guard lock(internalMutex_); + std::lock_guard lock_mutex(on_new_event_m_); - // the change to incompatible_qos_status_ needs to be mutually exclusive with - // rmw_wait() which checks hasEvent() and decides if wait() needs to be called - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + // Assign absolute values + sample_lost_status_.total_count = status.total_count; + // Accumulate deltas + sample_lost_status_.total_count_change += status.total_count_change; - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; + sample_lost_changes_.store(true, std::memory_order_relaxed); - incompatible_qos_changes_.store(true, std::memory_order_relaxed); + if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) + { + on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); + } } -bool SubListener::hasEvent(rmw_event_type_t event_type) const +void SubListener::on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus& status) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type) { - case RMW_EVENT_LIVELINESS_CHANGED: - return liveliness_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - return deadline_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_MESSAGE_LOST: - return sample_lost_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - return incompatible_qos_changes_.load(std::memory_order_relaxed); - default: - break; - } - return false; -} + std::lock_guard lock_mutex(on_new_event_m_); -void SubListener::set_on_new_event_callback( - const void * user_data, - rmw_event_callback_t callback) -{ - std::unique_lock lock_mutex(on_new_event_m_); + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; + + incompatible_qos_changes_.store(true, std::memory_order_relaxed); - if (callback) { - // Push events arrived before setting the executor's callback - if (unread_events_count_) { - callback(user_data, unread_events_count_); - unread_events_count_ = 0; + if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) + { + on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); } - user_data_ = user_data; - on_new_event_cb_ = callback; - } else { - user_data_ = nullptr; - on_new_event_cb_ = nullptr; - return; - } } -bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) +bool SubListener::has_event( + rmw_event_type_t event_type) const { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - std::lock_guard lock(internalMutex_); - switch (event_type) { - case RMW_EVENT_LIVELINESS_CHANGED: - { - auto rmw_data = static_cast(event_info); - rmw_data->alive_count = liveliness_changed_status_.alive_count; - rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; - liveliness_changed_status_.alive_count_change = 0; - liveliness_changed_status_.not_alive_count_change = 0; - liveliness_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = requested_deadline_missed_status_.total_count; - rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; - requested_deadline_missed_status_.total_count_change = 0; - deadline_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_MESSAGE_LOST: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = sample_lost_status_.total_count; - rmw_data->total_count_change = sample_lost_status_.total_count_change; - sample_lost_status_.total_count_change = 0; - sample_lost_changes_.store(false, std::memory_order_relaxed); - } - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_status_.total_count_change = 0; - incompatible_qos_changes_.store(false, std::memory_order_relaxed); - } - break; - default: - return false; - } - return true; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type){ + case RMW_EVENT_LIVELINESS_CHANGED: + return liveliness_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + return deadline_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_MESSAGE_LOST: + return sample_lost_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + return incompatible_qos_changes_.load(std::memory_order_relaxed); + default: + break; + } + return false; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 94bd3896f..b5d45b5e6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -22,19 +22,17 @@ #include "types/event_types.hpp" static const std::unordered_set g_rmw_event_type_set{ - RMW_EVENT_LIVELINESS_CHANGED, - RMW_EVENT_REQUESTED_DEADLINE_MISSED, - RMW_EVENT_LIVELINESS_LOST, - RMW_EVENT_OFFERED_DEADLINE_MISSED, - RMW_EVENT_MESSAGE_LOST, - RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, - RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE + RMW_EVENT_LIVELINESS_CHANGED, + RMW_EVENT_REQUESTED_DEADLINE_MISSED, + RMW_EVENT_LIVELINESS_LOST, + RMW_EVENT_OFFERED_DEADLINE_MISSED, + RMW_EVENT_MESSAGE_LOST, + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE }; -namespace rmw_fastrtps_shared_cpp -{ -namespace internal -{ +namespace rmw_fastrtps_shared_cpp { +namespace internal { eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( const rmw_event_type_t event_type) @@ -70,80 +68,83 @@ eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( return ret_statusmask; } -bool is_event_supported(rmw_event_type_t event_type) +bool is_event_supported( + rmw_event_type_t event_type) { - return g_rmw_event_type_set.count(event_type) == 1; + return g_rmw_event_type_set.count(event_type) == 1; } rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( - eprosima::fastdds::dds::QosPolicyId_t policy_id) + eprosima::fastdds::dds::QosPolicyId_t policy_id) { - using eprosima::fastdds::dds::QosPolicyId_t; - - switch (policy_id) { - case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_DURABILITY; - case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: - return RMW_QOS_POLICY_DEADLINE; - case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIVELINESS; - case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_RELIABILITY; - case QosPolicyId_t::HISTORY_QOS_POLICY_ID: - return RMW_QOS_POLICY_HISTORY; - case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIFESPAN; - default: - return RMW_QOS_POLICY_INVALID; - } + using eprosima::fastdds::dds::QosPolicyId_t; + + switch (policy_id){ + case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_DURABILITY; + case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: + return RMW_QOS_POLICY_DEADLINE; + case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIVELINESS; + case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_RELIABILITY; + case QosPolicyId_t::HISTORY_QOS_POLICY_ID: + return RMW_QOS_POLICY_HISTORY; + case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIFESPAN; + default: + return RMW_QOS_POLICY_INVALID; + } } } // namespace internal rmw_ret_t __rmw_init_event( - const char * identifier, - rmw_event_t * rmw_event, - const char * topic_endpoint_impl_identifier, - void * data, - rmw_event_type_t event_type) + const char* identifier, + rmw_event_t* rmw_event, + const char* topic_endpoint_impl_identifier, + void* data, + rmw_event_type_t event_type) { - RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - topic endpoint, - topic_endpoint_impl_identifier, - identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!internal::is_event_supported(event_type)) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); - return RMW_RET_UNSUPPORTED; - } - - rmw_event->implementation_identifier = topic_endpoint_impl_identifier; - rmw_event->data = data; - rmw_event->event_type = event_type; - CustomEventInfo* event = static_cast(rmw_event->data); - eprosima::fastdds::dds::StatusMask statusmask = event->get_statuscondition().get_enabled_statuses(); - statusmask << internal::rmw_event_to_dds_statusmask(event_type); - event->get_statuscondition().set_enabled_statuses(statusmask); - - return RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + topic endpoint, + topic_endpoint_impl_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!internal::is_event_supported(event_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = topic_endpoint_impl_identifier; + rmw_event->data = data; + rmw_event->event_type = event_type; + CustomEventInfo* event = static_cast(rmw_event->data); + eprosima::fastdds::dds::StatusMask statusmask = event->get_listener()->get_statuscondition().get_enabled_statuses(); + statusmask << internal::rmw_event_to_dds_statusmask(event_type); + event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); + + return RMW_RET_OK; } rmw_ret_t __rmw_event_set_callback( - rmw_event_t * rmw_event, - rmw_event_callback_t callback, - const void * user_data) + rmw_event_t* rmw_event, + rmw_event_callback_t callback, + const void* user_data) { - auto custom_event_info = static_cast(rmw_event->data); - custom_event_info->getListener()->set_on_new_event_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_event_info = static_cast(rmw_event->data); + custom_event_info->get_listener()->set_on_new_event_callback( + rmw_event->event_type, + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index ce259e973..8405c1b6d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -36,497 +36,533 @@ #include "tracetools/tracetools.h" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { using DataSharingKind = eprosima::fastdds::dds::DataSharingKind; void _assign_message_info( - const char * identifier, - rmw_message_info_t * message_info, - const eprosima::fastdds::dds::SampleInfo * sinfo) + const char* identifier, + rmw_message_info_t* message_info, + const eprosima::fastdds::dds::SampleInfo* sinfo) { - message_info->source_timestamp = sinfo->source_timestamp.to_ns(); - message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); - auto fastdds_sn = sinfo->sample_identity.sequence_number(); - message_info->publication_sequence_number = - (static_cast(fastdds_sn.high) << 32) | - static_cast(fastdds_sn.low); - message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; - rmw_gid_t * sender_gid = &message_info->publisher_gid; - 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( - sinfo->sample_identity.writer_guid(), - sender_gid->data); + message_info->source_timestamp = sinfo->source_timestamp.to_ns(); + message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); + auto fastdds_sn = sinfo->sample_identity.sequence_number(); + message_info->publication_sequence_number = + (static_cast(fastdds_sn.high) << 32) | + static_cast(fastdds_sn.low); + message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; + rmw_gid_t* sender_gid = &message_info->publisher_gid; + 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( + sinfo->sample_identity.writer_guid(), + sender_gid->data); } rmw_ret_t _take( - const char * identifier, - const rmw_subscription_t * subscription, - void * ros_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + void* ros_message, + bool* taken, + rmw_message_info_t* message_info, + rmw_subscription_allocation_t* allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - - data.is_cdr_buffer = false; - data.data = ros_message; - data.impl = info->type_support_impl_; - - while (0 < info->data_reader_->get_unread_count()) { - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - - if (subscription->options.ignore_local_publications) { - auto sample_writer_guid = - eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); - - if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { - // This is a local publication. Ignore it - continue; + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + + data.is_cdr_buffer = false; + data.data = ros_message; + data.impl = info->type_support_impl_; + + while (0 < info->data_reader_->get_unread_count()) + { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) + { + + if (subscription->options.ignore_local_publications) + { + auto sample_writer_guid = + eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); + + if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) + { + // This is a local publication. Ignore it + continue; + } + } + + if (sinfo.valid_data) + { + if (message_info) + { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + break; + } } - } - - if (sinfo.valid_data) { - if (message_info) { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; - break; - } } - } - - TRACEPOINT( - rmw_take, - static_cast(subscription), - static_cast(ros_message), - (message_info ? message_info->source_timestamp : 0LL), - *taken); - return RMW_RET_OK; + + TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + (message_info ? message_info->source_timestamp : 0LL), + *taken); + return RMW_RET_OK; } rmw_ret_t _take_sequence( - const char * identifier, - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + size_t count, + rmw_message_sequence_t* message_sequence, + rmw_message_info_sequence_t* message_info_sequence, + size_t* taken, + rmw_subscription_allocation_t* allocation) { - *taken = 0; - bool taken_flag = false; - rmw_ret_t ret = RMW_RET_OK; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - // Limit the upper bound of reads to the number unread at the beginning. - // This prevents any samples that are added after the beginning of the - // _take_sequence call from being read. - auto unread_count = info->data_reader_->get_unread_count(); - if (unread_count < count) { - count = unread_count; - } - - for (size_t ii = 0; ii < count; ++ii) { - taken_flag = false; - ret = _take( - identifier, subscription, message_sequence->data[*taken], - &taken_flag, &message_info_sequence->data[*taken], allocation); - - if (ret != RMW_RET_OK) { - break; + *taken = 0; + bool taken_flag = false; + rmw_ret_t ret = RMW_RET_OK; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + // Limit the upper bound of reads to the number unread at the beginning. + // This prevents any samples that are added after the beginning of the + // _take_sequence call from being read. + auto unread_count = info->data_reader_->get_unread_count(); + if (unread_count < count) + { + count = unread_count; } - if (taken_flag) { - (*taken)++; + for (size_t ii = 0; ii < count; ++ii) + { + taken_flag = false; + ret = _take( + identifier, subscription, message_sequence->data[*taken], + &taken_flag, &message_info_sequence->data[*taken], allocation); + + if (ret != RMW_RET_OK) + { + break; + } + + if (taken_flag) + { + (*taken)++; + } } - } - message_sequence->size = *taken; - message_info_sequence->size = *taken; + message_sequence->size = *taken; + message_info_sequence->size = *taken; - return ret; + return ret; } rmw_ret_t __rmw_take_event( - const char * identifier, - const rmw_event_t * event_handle, - void * event_info, - bool * taken) + const char* identifier, + const rmw_event_t* event_handle, + void* event_info, + bool* taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - event handle, - event_handle->implementation_identifier, - identifier, - return RMW_RET_ERROR); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + event handle, + event_handle->implementation_identifier, + identifier, + return RMW_RET_ERROR); - auto event = static_cast(event_handle->data); - if (event->take_event(event_handle->event_type, event_info)) { - *taken = true; - return RMW_RET_OK; - } + auto event = static_cast(event_handle->data); + if (event->get_listener()->take_event(event_handle->event_type, event_info)) + { + *taken = true; + return RMW_RET_OK; + } - return RMW_RET_ERROR; + return RMW_RET_ERROR; } rmw_ret_t __rmw_take( - const char * identifier, - const rmw_subscription_t * subscription, - void * ros_message, - bool * taken, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + void* ros_message, + bool* taken, + rmw_subscription_allocation_t* allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, nullptr, allocation); + return _take(identifier, subscription, ros_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_sequence( - const char * identifier, - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + size_t count, + rmw_message_sequence_t* message_sequence, + rmw_message_info_sequence_t* message_info_sequence, + size_t* taken, + rmw_subscription_allocation_t* allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - if (0u == count) { - RMW_SET_ERROR_MSG("count cannot be 0"); - return RMW_RET_INVALID_ARGUMENT; - } + if (0u == count) + { + RMW_SET_ERROR_MSG("count cannot be 0"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_sequence->capacity) { - RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_sequence->capacity) + { + RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_info_sequence->capacity) { - RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_info_sequence->capacity) + { + RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - return _take_sequence( - identifier, subscription, count, message_sequence, message_info_sequence, - taken, allocation); + return _take_sequence( + identifier, subscription, count, message_sequence, message_info_sequence, + taken, allocation); } rmw_ret_t __rmw_take_with_info( - const char * identifier, - const rmw_subscription_t * subscription, - void * ros_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + void* ros_message, + bool* taken, + rmw_message_info_t* message_info, + rmw_subscription_allocation_t* allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, message_info, allocation); + return _take(identifier, subscription, ros_message, taken, message_info, allocation); } rmw_ret_t _take_serialized_message( - const char * identifier, - const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + rmw_serialized_message_t* serialized_message, + bool* taken, + rmw_message_info_t* message_info, + rmw_subscription_allocation_t* allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastcdr::FastBuffer buffer; - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = &buffer; - data.impl = nullptr; // not used when is_cdr_buffer is true - - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - if (sinfo.valid_data) { - auto buffer_size = static_cast(buffer.getBufferSize()); - if (serialized_message->buffer_capacity < buffer_size) { - auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); - if (ret != RMW_RET_OK) { - return ret; // Error message already set + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer; + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &buffer; + data.impl = nullptr; // not used when is_cdr_buffer is true + + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) + { + if (sinfo.valid_data) + { + auto buffer_size = static_cast(buffer.getBufferSize()); + if (serialized_message->buffer_capacity < buffer_size) + { + auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); + if (ret != RMW_RET_OK) + { + return ret; // Error message already set + } + } + serialized_message->buffer_length = buffer_size; + memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); + + if (message_info) + { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; } - } - serialized_message->buffer_length = buffer_size; - memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); - - if (message_info) { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; } - } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_take_serialized_message( - const char * identifier, - const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, - bool * taken, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + rmw_serialized_message_t* serialized_message, + bool* taken, + rmw_subscription_allocation_t* allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, nullptr, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_serialized_message_with_info( - const char * identifier, - const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) + const char* identifier, + const rmw_subscription_t* subscription, + rmw_serialized_message_t* serialized_message, + bool* taken, + rmw_message_info_t* message_info, + rmw_subscription_allocation_t* allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, message_info, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, message_info, allocation); } // ----------------- Loans related code ------------------------- // struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection { - GenericSequence() = default; + GenericSequence() = default; + + void resize( + size_type /*new_length*/) override + { + // This kind of collection should only be used with loans + throw std::bad_alloc(); + } - void resize(size_type /*new_length*/) override - { - // This kind of collection should only be used with loans - throw std::bad_alloc(); - } }; struct LoanManager { - struct Item - { - GenericSequence data_seq{}; - eprosima::fastdds::dds::SampleInfoSeq info_seq{}; - }; - - explicit LoanManager(const eprosima::fastrtps::ResourceLimitedContainerConfig & items_cfg) - : items(items_cfg) - { - } - - void add_item(std::unique_ptr item) - { - std::lock_guard guard(mtx); - items.push_back(std::move(item)); - } - - std::unique_ptr erase_item(void * loaned_message) - { - std::unique_ptr ret{nullptr}; - - std::lock_guard guard(mtx); - for (auto it = items.begin(); it != items.end(); ++it) { - if (loaned_message == (*it)->data_seq.buffer()[0]) { - ret = std::move(*it); - items.erase(it); - break; - } + struct Item + { + GenericSequence data_seq{}; + eprosima::fastdds::dds::SampleInfoSeq info_seq{}; + }; + + explicit LoanManager( + const eprosima::fastrtps::ResourceLimitedContainerConfig& items_cfg) + : items(items_cfg) + { } - return ret; - } + void add_item( + std::unique_ptr item) + { + std::lock_guard guard(mtx); + items.push_back(std::move(item)); + } + + std::unique_ptr erase_item( + void* loaned_message) + { + std::unique_ptr ret{nullptr}; + + std::lock_guard guard(mtx); + for (auto it = items.begin(); it != items.end(); ++it) + { + if (loaned_message == (*it)->data_seq.buffer()[0]) + { + ret = std::move(*it); + items.erase(it); + break; + } + } + + return ret; + } private: - std::mutex mtx; - using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); + + std::mutex mtx; + using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; + ItemVector items RCPPUTILS_TSA_GUARDED_BY( + mtx); }; void __init_subscription_for_loans( - rmw_subscription_t * subscription) + rmw_subscription_t* subscription) { - auto info = static_cast(subscription->data); - const auto & qos = info->data_reader_->get_qos(); - bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); - subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); - if (subscription->can_loan_messages) { - const auto & allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; - info->loan_manager_ = std::make_shared(allocation_qos); - } + auto info = static_cast(subscription->data); + const auto& qos = info->data_reader_->get_qos(); + bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); + subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); + if (subscription->can_loan_messages) + { + const auto& allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; + info->loan_manager_ = std::make_shared(allocation_qos); + } } rmw_ret_t __rmw_take_loaned_message_internal( - const char * identifier, - const rmw_subscription_t * subscription, - void ** loaned_message, - bool * taken, - rmw_message_info_t * message_info) + const char* identifier, + const rmw_subscription_t* subscription, + void** loaned_message, + bool* taken, + rmw_message_info_t* message_info) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; - } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) + { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - auto info = static_cast(subscription->data); + auto info = static_cast(subscription->data); - auto item = std::make_unique(); + auto item = std::make_unique(); - while (ReturnCode_t::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]); - } - *loaned_message = item->data_seq.buffer()[0]; - *taken = true; + while (ReturnCode_t::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]); + } + *loaned_message = item->data_seq.buffer()[0]; + *taken = true; - info->loan_manager_->add_item(std::move(item)); + info->loan_manager_->add_item(std::move(item)); - return RMW_RET_OK; - } + return RMW_RET_OK; + } - // Should return loan before taking again - info->data_reader_->return_loan(item->data_seq, item->info_seq); - } + // Should return loan before taking again + info->data_reader_->return_loan(item->data_seq, item->info_seq); + } - // No data available, return loan information. - *taken = false; - return RMW_RET_OK; + // No data available, return loan information. + *taken = false; + return RMW_RET_OK; } rmw_ret_t __rmw_return_loaned_message_from_subscription( - const char * identifier, - const rmw_subscription_t * subscription, - void * loaned_message) + const char* identifier, + const rmw_subscription_t* subscription, + void* loaned_message) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; - } - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - - auto info = static_cast(subscription->data); - 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)) { - RMW_SET_ERROR_MSG("Error returning loan"); - return RMW_RET_ERROR; + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) + { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; } + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + 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)) + { + RMW_SET_ERROR_MSG("Error returning loan"); + return RMW_RET_ERROR; + } - return RMW_RET_OK; - } + return RMW_RET_OK; + } - RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); + return RMW_RET_ERROR; } + } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 499b8187d..51668f129 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -27,204 +27,231 @@ #include "fastdds/dds/core/condition/GuardCondition.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_wait( - const char * identifier, - rmw_subscriptions_t * subscriptions, - rmw_guard_conditions_t * guard_conditions, - rmw_services_t * services, - rmw_clients_t * clients, - rmw_events_t * events, - rmw_wait_set_t * wait_set, - const rmw_time_t * wait_timeout) + const char* identifier, + rmw_subscriptions_t* subscriptions, + rmw_guard_conditions_t* guard_conditions, + rmw_services_t* services, + rmw_clients_t* clients, + rmw_events_t* events, + rmw_wait_set_t* wait_set, + const rmw_time_t* wait_timeout) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - wait set handle, - wait_set->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - // If wait_set_info is ever nullptr, it can only mean one of three things: - // - Wait set is invalid. Caller did not respect preconditions. - // - Implementation is logically broken. Definitely not something we want to treat as a normal - // error. - // - Heap is corrupt. - // In all three cases, it's better if this crashes soon enough. - auto fastdds_wait_set = static_cast(wait_set->data); - - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + // If wait_set_info is ever nullptr, it can only mean one of three things: + // - Wait set is invalid. Caller did not respect preconditions. + // - Implementation is logically broken. Definitely not something we want to treat as a normal + // error. + // - Heap is corrupt. + // In all three cases, it's better if this crashes soon enough. + auto fastdds_wait_set = static_cast(wait_set->data); + + if (subscriptions) + { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) + { + void* data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); + } } - } - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - auto custom_client_info = static_cast(data); - fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); + if (clients) + { + for (size_t i = 0; i < clients->client_count; ++i) + { + void* data = clients->clients[i]; + auto custom_client_info = static_cast(data); + fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); + } } - } - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); + if (services) + { + for (size_t i = 0; i < services->service_count; ++i) + { + void* data = services->services[i]; + auto custom_service_info = static_cast(data); + fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); + } } - } - if (events) { - for (size_t i = 0; i < events->event_count; ++i) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->attach_condition(custom_event_info->get_statuscondition()); + if (events) + { + for (size_t i = 0; i < events->event_count; ++i) + { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->attach_condition(custom_event_info->get_listener()->get_statuscondition()); + } } - } - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->attach_condition(*guard_condition); + if (guard_conditions) + { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) + { + void* data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + fastdds_wait_set->attach_condition(*guard_condition); + } } - } - - eprosima::fastdds::dds::ConditionSeq triggered_coditions; - ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, - (wait_timeout && (wait_timeout->sec > 0 || wait_timeout->nsec > 0)) ? - Duration_t{wait_timeout->sec, wait_timeout->nsec} : eprosima::fastrtps::c_TimeInfinite - ); - - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = custom_subscriber_info->data_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + + eprosima::fastdds::dds::ConditionSeq triggered_coditions; + ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, + (wait_timeout && (wait_timeout->sec > 0 || wait_timeout->nsec > 0)) ? + Duration_t{static_cast(wait_timeout->sec), static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite + ); + + if (subscriptions) + { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - subscriptions->subscribers[i] = 0; + void* data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition& status_condition = + custom_subscriber_info->data_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + subscriptions->subscribers[i] = 0; + } + } + else + { + subscriptions->subscribers[i] = 0; + } } - } - else - { - subscriptions->subscribers[i] = 0; - } } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - auto custom_client_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = custom_client_info->response_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + + if (clients) + { + for (size_t i = 0; i < clients->client_count; ++i) { - clients->clients[i] = 0; + void* data = clients->clients[i]; + auto custom_client_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition& status_condition = + custom_client_info->response_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + clients->clients[i] = 0; + } + } + else + { + clients->clients[i] = 0; + } } - } - else - { - clients->clients[i] = 0; - } } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = custom_service_info->request_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + + if (services) + { + for (size_t i = 0; i < services->service_count; ++i) { - services->services[i] = 0; + void* data = services->services[i]; + auto custom_service_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition& status_condition = + custom_service_info->request_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) + { + services->services[i] = 0; + } + } + else + { + services->services[i] = 0; + } } - } - else - { - services->services[i] = 0; - } } - } - - if (events) { - for (size_t i = 0; i < events->event_count; ++i) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->detach_condition(custom_event_info->get_statuscondition()); - eprosima::fastdds::dds::StatusCondition& status_condition = custom_event_info->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event->event_type))) + + if (events) + { + for (size_t i = 0; i < events->event_count; ++i) { - events->events[i] = 0; + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->detach_condition(custom_event_info->get_listener()->get_statuscondition()); + eprosima::fastdds::dds::StatusCondition& status_condition = + custom_event_info->get_listener()->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition* condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!custom_event_info->get_listener()->has_event(event->event_type) && + !changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event-> + event_type))) + { + events->events[i] = 0; + } + } + else + { + events->events[i] = 0; + } } - } - else - { - events->events[i] = 0; - } } - } - - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->detach_condition(*guard_condition); - guard_condition->set_trigger_value(false); + + if (guard_conditions) + { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) + { + void* data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + fastdds_wait_set->detach_condition(*guard_condition); + guard_condition->set_trigger_value(false); + } } - } - return ReturnCode_t::RETCODE_OK == ret_code ? RMW_RET_OK : RMW_RET_TIMEOUT; + return ReturnCode_t::RETCODE_OK == ret_code ? RMW_RET_OK : RMW_RET_TIMEOUT; } + } // namespace rmw_fastrtps_shared_cpp From 3413abe6f6f77141daac91432e81ef0beff294ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Tue, 28 Jun 2022 12:17:59 +0200 Subject: [PATCH 05/34] Use guard_condition with event listeners MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_event_info.hpp | 12 ++--- .../custom_publisher_info.hpp | 13 ++--- .../custom_subscriber_info.hpp | 14 ++--- .../src/custom_publisher_info.cpp | 45 ++++++---------- .../src/custom_subscriber_info.cpp | 52 +++++++------------ rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 29 ++++++++--- 6 files changed, 69 insertions(+), 96 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index d5d289395..0eda78002 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -24,6 +24,7 @@ #include "fastcdr/FastBuffer.h" #include "fastdds/dds/core/condition/StatusCondition.hpp" +#include "fastdds/dds/core/condition/GuardCondition.hpp" #include "rmw/event.h" #include "rmw/event_callback_type.h" @@ -52,14 +53,6 @@ class EventListenerInterface /// Unset the information from attachCondition. virtual void detachCondition() = 0; - /// Check if there is new data available for a specific event type. - /** - * \param event_type The event type to check on. - * \return `true` if new data is available. - */ - virtual bool has_event( - rmw_event_type_t event_type) const = 0; - /// Take ready data for an event type. /** * \param event_type The event type to get data for. @@ -78,6 +71,9 @@ class EventListenerInterface const void* user_data, rmw_event_callback_t callback) = 0; + + eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; + protected: rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; 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 3e162761c..dc1c961f4 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 @@ -15,7 +15,6 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ -#include #include #include #include @@ -106,12 +105,6 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds eprosima::fastdds::dds::DataWriter*, const eprosima::fastdds::dds::OfferedIncompatibleQosStatus&) final; - // EventListenerInterface implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - has_event( - rmw_event_type_t event_type) const final; - // PubListener API size_t subscriptionCount() { @@ -161,17 +154,17 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool deadline_changes_; + bool deadline_changes_; eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool liveliness_changes_; + bool liveliness_changes_; eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool incompatible_qos_changes_; + bool incompatible_qos_changes_; eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 496f52e4a..00364e533 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -163,12 +163,6 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds eprosima::fastdds::dds::DataReader*, const eprosima::fastdds::dds::RequestedIncompatibleQosStatus&) final; - // EventListenerInterface implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool - has_event( - rmw_event_type_t event_type) const final; - // SubListener API void attachCondition( @@ -265,22 +259,22 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds std::atomic_bool data_; - std::atomic_bool deadline_changes_; + bool deadline_changes_; eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool liveliness_changes_; + bool liveliness_changes_; eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool sample_lost_changes_; + bool sample_lost_changes_; eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); - std::atomic_bool incompatible_qos_changes_; + bool incompatible_qos_changes_; eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 34da4c457..997314de7 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -43,11 +43,11 @@ bool PubListener::take_event( case RMW_EVENT_LIVELINESS_LOST: { auto rmw_data = static_cast(event_info); - if (liveliness_changes_.load(std::memory_order_relaxed)) + if (liveliness_changes_) { rmw_data->total_count = liveliness_lost_status_.total_count; rmw_data->total_count_change = liveliness_lost_status_.total_count_change; - liveliness_changes_.store(false, std::memory_order_relaxed); + liveliness_changes_ = false; } else { @@ -62,11 +62,11 @@ bool PubListener::take_event( case RMW_EVENT_OFFERED_DEADLINE_MISSED: { auto rmw_data = static_cast(event_info); - if (deadline_changes_.load(std::memory_order_relaxed)) + if (deadline_changes_) { rmw_data->total_count = offered_deadline_missed_status_.total_count; rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; - deadline_changes_.store(false, std::memory_order_relaxed); + deadline_changes_ = false; } else { @@ -81,14 +81,14 @@ bool PubListener::take_event( case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: { auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_.load(std::memory_order_relaxed)) + if (incompatible_qos_changes_) { rmw_data->total_count = incompatible_qos_status_.total_count; rmw_data->total_count_change = incompatible_qos_status_.total_count_change; rmw_data->last_policy_kind = rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_.store(false, std::memory_order_relaxed); + incompatible_qos_changes_ = false; } else { @@ -106,6 +106,7 @@ bool PubListener::take_event( default: return false; } + event_guard[event_type].set_trigger_value(false); return true; } @@ -118,8 +119,6 @@ void PubListener::set_on_new_event_callback( if (callback) { - size_t last_total_count = 0; - switch (event_type) { case RMW_EVENT_LIVELINESS_LOST: @@ -128,14 +127,12 @@ void PubListener::set_on_new_event_callback( liveliness_lost_status_.total_count_change = 0; break; case RMW_EVENT_OFFERED_DEADLINE_MISSED: - last_total_count = offered_deadline_missed_status_.total_count; publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status_); callback(user_data, offered_deadline_missed_status_.total_count_change); offered_deadline_missed_status_.total_count_change = 0; break; case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - last_total_count = incompatible_qos_status_.total_count; publisher_info_->data_writer_->get_offered_incompatible_qos_status(incompatible_qos_status_); callback(user_data, incompatible_qos_status_.total_count_change); @@ -175,12 +172,14 @@ PubListener::on_offered_deadline_missed( // Accumulate deltas offered_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_.store(true, std::memory_order_relaxed); + deadline_changes_ = true; if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) { on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED], 1); } + + event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); } void PubListener::on_liveliness_lost( @@ -194,13 +193,14 @@ void PubListener::on_liveliness_lost( // Accumulate deltas liveliness_lost_status_.total_count_change += status.total_count_change; - liveliness_changes_.store(true, std::memory_order_relaxed); - + liveliness_changes_ = true; if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) { on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); } + + event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); } void PubListener::on_offered_incompatible_qos( @@ -215,27 +215,12 @@ void PubListener::on_offered_incompatible_qos( // Accumulate deltas incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_.store(true, std::memory_order_relaxed); + incompatible_qos_changes_ = true; if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) { on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); } -} -bool PubListener::has_event( - rmw_event_type_t event_type) const -{ - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type){ - case RMW_EVENT_LIVELINESS_LOST: - return liveliness_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - return deadline_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - return incompatible_qos_changes_.load(std::memory_order_relaxed); - default: - break; - } - return false; + event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 104890181..e3298ccc6 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -43,13 +43,13 @@ bool SubListener::take_event( case RMW_EVENT_LIVELINESS_CHANGED: { auto rmw_data = static_cast(event_info); - if (liveliness_changes_.load(std::memory_order_relaxed)) + if (liveliness_changes_) { rmw_data->alive_count = liveliness_changed_status_.alive_count; rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; - liveliness_changes_.store(false, std::memory_order_relaxed); + liveliness_changes_ = false; } else { @@ -67,11 +67,11 @@ bool SubListener::take_event( case RMW_EVENT_REQUESTED_DEADLINE_MISSED: { auto rmw_data = static_cast(event_info); - if (deadline_changes_.load(std::memory_order_relaxed)) + if (deadline_changes_) { rmw_data->total_count = requested_deadline_missed_status_.total_count; rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; - deadline_changes_.store(false, std::memory_order_relaxed); + deadline_changes_ = false; } else { @@ -86,11 +86,11 @@ bool SubListener::take_event( case RMW_EVENT_MESSAGE_LOST: { auto rmw_data = static_cast(event_info); - if (sample_lost_changes_.load(std::memory_order_relaxed)) + if (sample_lost_changes_) { rmw_data->total_count = sample_lost_status_.total_count; rmw_data->total_count_change = sample_lost_status_.total_count_change; - sample_lost_changes_.store(false, std::memory_order_relaxed); + sample_lost_changes_ = false; } else { @@ -105,14 +105,14 @@ bool SubListener::take_event( case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: { auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_.load(std::memory_order_relaxed)) + if (incompatible_qos_changes_) { rmw_data->total_count = incompatible_qos_status_.total_count; rmw_data->total_count_change = incompatible_qos_status_.total_count_change; rmw_data->last_policy_kind = rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_.store(false, std::memory_order_relaxed); + incompatible_qos_changes_ = false; } else { @@ -132,6 +132,7 @@ bool SubListener::take_event( return false; } + event_guard[event_type].set_trigger_value(false); return true; } @@ -144,8 +145,6 @@ void SubListener::set_on_new_event_callback( if (callback) { - size_t last_total_count = 0; - switch (event_type){ case RMW_EVENT_LIVELINESS_CHANGED: { @@ -214,12 +213,14 @@ SubListener::on_requested_deadline_missed( // Accumulate deltas requested_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_.store(true, std::memory_order_relaxed); + deadline_changes_ = true; if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) { on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); } + + event_guard[RMW_EVENT_REQUESTED_DEADLINE_MISSED].set_trigger_value(true); } void SubListener::on_liveliness_changed( @@ -235,12 +236,14 @@ void SubListener::on_liveliness_changed( liveliness_changed_status_.alive_count_change += status.alive_count_change; liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; - liveliness_changes_.store(true, std::memory_order_relaxed); + liveliness_changes_ = true; if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) { on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); } + + event_guard[RMW_EVENT_LIVELINESS_CHANGED].set_trigger_value(true); } void SubListener::on_sample_lost( @@ -254,12 +257,14 @@ void SubListener::on_sample_lost( // Accumulate deltas sample_lost_status_.total_count_change += status.total_count_change; - sample_lost_changes_.store(true, std::memory_order_relaxed); + sample_lost_changes_ = true; if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) { on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); } + + event_guard[RMW_EVENT_MESSAGE_LOST].set_trigger_value(true); } void SubListener::on_requested_incompatible_qos( @@ -274,29 +279,12 @@ void SubListener::on_requested_incompatible_qos( // Accumulate deltas incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_.store(true, std::memory_order_relaxed); + incompatible_qos_changes_ = true; if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) { on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); } -} -bool SubListener::has_event( - rmw_event_type_t event_type) const -{ - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - switch (event_type){ - case RMW_EVENT_LIVELINESS_CHANGED: - return liveliness_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - return deadline_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_MESSAGE_LOST: - return sample_lost_changes_.load(std::memory_order_relaxed); - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - return incompatible_qos_changes_.load(std::memory_order_relaxed); - default: - break; - } - return false; + event_guard[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 51668f129..23b7178cc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -93,6 +93,7 @@ __rmw_wait( auto event = static_cast(events->events[i]); auto custom_event_info = static_cast(event->data); fastdds_wait_set->attach_condition(custom_event_info->get_listener()->get_statuscondition()); + fastdds_wait_set->attach_condition(custom_event_info->get_listener()->event_guard[event->event_type]); } } @@ -216,6 +217,9 @@ __rmw_wait( custom_event_info->get_listener()->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::Condition* condition = &status_condition; + eprosima::fastdds::dds::GuardCondition* guard_condition = + &custom_event_info->get_listener()->event_guard[event->event_type]; + bool active = false; if (ReturnCode_t::RETCODE_OK == ret_code && triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), [condition](const eprosima::fastdds::dds::Condition* c) @@ -225,15 +229,28 @@ __rmw_wait( { eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!custom_event_info->get_listener()->has_event(event->event_type) && - !changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event-> - event_type))) + if (changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event->event_type))) { - events->events[i] = 0; + active = true; } } - else + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [guard_condition](const eprosima::fastdds::dds::Condition* c) + { + return c == guard_condition; + })) + { + if (guard_condition->get_trigger_value()) + { + active = true; + guard_condition->set_trigger_value(false); + } + } + + + if (!active) { events->events[i] = 0; } From 7b9ca575ed9c029b45672ec05d9a9cbcfb30b34f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Tue, 28 Jun 2022 14:03:49 +0200 Subject: [PATCH 06/34] Remove unused functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_cpp/src/subscription.cpp | 583 +++++++++-------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 616 +++++++++--------- .../custom_client_info.hpp | 323 ++++----- .../custom_event_info.hpp | 53 -- .../custom_publisher_info.hpp | 41 +- .../custom_service_info.hpp | 434 ++++++------ .../custom_subscriber_info.hpp | 121 +--- .../src/custom_subscriber_info.cpp | 46 ++ rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 211 +++--- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 221 ++++--- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 207 +++--- .../src/rmw_subscription.cpp | 387 +++++------ 12 files changed, 1536 insertions(+), 1707 deletions(-) diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 669fee038..ff3d7814b 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -55,294 +55,319 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_cpp -{ +namespace rmw_fastrtps_cpp { -rmw_subscription_t * +rmw_subscription_t* create_subscription( - const CustomParticipantInfo * participant_info, - const rosidl_message_type_support_t * type_supports, - const char * topic_name, - const rmw_qos_profile_t * qos_policies, - const rmw_subscription_options_t * subscription_options, - bool keyed) + const CustomParticipantInfo* participant_info, + const rosidl_message_type_support_t* type_supports, + const char* topic_name, + const rmw_qos_profile_t* qos_policies, + const rmw_subscription_options_t* subscription_options, + bool keyed) { - ///// - // Check input parameters - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); - return nullptr; - } - RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); - if (!qos_policies->avoid_ros_namespace_conventions) { - int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); - if (RMW_RET_OK != ret) { - return nullptr; + ///// + // Check input parameters + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) + { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) + { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) + { + return nullptr; + } + 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( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; + } + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) + { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) + { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) + { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called for existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() + { + delete info->listener_; + if (info->type_support_) + { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = callbacks; + + ///// + // Create the Type Support struct + if (!fastdds_type) + { + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!tsupport) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; + } + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) + { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(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; + } + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) + { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) + { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + info->dds_participant_ = dds_participant; + info->subscriber_ = subscriber; + info->topic_name_mangled_ = topic_name_mangled; + info->topic_ = topic.desc; + des_topic = topic.desc; + + // Create ContentFilteredTopic + if (subscription_options->content_filter_options) + { + rmw_subscription_content_filter_options_t* options = + subscription_options->content_filter_options; + if (nullptr != options->filter_expression) + { + eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, des_topic, + topic_name_mangled, options, &filtered_topic)) + { + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + return nullptr; + } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; + } } - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) + { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); } - } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; + + if (!get_datareader_qos(*qos_policies, reader_qos)) + { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; } - } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called for existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; - } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() { - delete info->listener_; - if (info->type_support_) { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = callbacks; - - ///// - // Create the Type Support struct - if (!fastdds_type) { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!tsupport) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); - return nullptr; + + info->datareader_qos_ = reader_qos; + + // create_datareader + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return nullptr; } - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(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; - } - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; - } - - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - info->dds_participant_ = dds_participant; - info->subscriber_ = subscriber; - info->topic_name_mangled_ = topic_name_mangled; - info->topic_ = topic.desc; - des_topic = topic.desc; - - // Create ContentFilteredTopic - if (subscription_options->content_filter_options) { - rmw_subscription_content_filter_options_t * options = - subscription_options->content_filter_options; - if (nullptr != options->filter_expression) { - eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, des_topic, - topic_name_mangled, options, &filtered_topic)) - { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() + { + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + ///// + // Allocate subscription + rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); return nullptr; - } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; } - } - - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->datareader_qos_ = reader_qos; - - // create_datareader - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) - { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); - return nullptr; - } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - ///// - // Allocate subscription - rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_subscription = rcpputils::make_scope_exit( - [rmw_subscription]() { - rmw_free(const_cast(rmw_subscription->topic_name)); - rmw_subscription_free(rmw_subscription); - }); - - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - - rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - cleanup_info.cancel(); - - TRACEPOINT( - rmw_subscription_init, - static_cast(rmw_subscription), - info->subscription_gid_.data); - return rmw_subscription; + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() + { + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + + rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); + if (!rmw_subscription->topic_name) + { + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + cleanup_info.cancel(); + + TRACEPOINT( + rmw_subscription_init, + static_cast(rmw_subscription), + info->subscription_gid_.data); + return rmw_subscription; } + } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 8b70517aa..8d54cdf61 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -55,309 +55,335 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_dynamic_cpp -{ +namespace rmw_fastrtps_dynamic_cpp { -rmw_subscription_t * +rmw_subscription_t* create_subscription( - const CustomParticipantInfo * participant_info, - const rosidl_message_type_support_t * type_supports, - const char * topic_name, - const rmw_qos_profile_t * qos_policies, - const rmw_subscription_options_t * subscription_options, - bool keyed) + const CustomParticipantInfo* participant_info, + const rosidl_message_type_support_t* type_supports, + const char* topic_name, + const rmw_qos_profile_t* qos_policies, + const rmw_subscription_options_t* subscription_options, + bool keyed) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - ///// - // Check input parameters - RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); - return nullptr; - } - RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); - if (!qos_policies->avoid_ros_namespace_conventions) { - int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); - if (RMW_RET_OK != ret) { - return nullptr; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + ///// + // Check input parameters + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) + { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) + { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) + { + return nullptr; + } + 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( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; + } + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) + { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; } - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) + { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) + { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } } - } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription* des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; } - } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - std::string type_name = _create_type_name( - type_support->data, type_support->typesupport_identifier); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription * des_topic; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called with existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; - } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() { - delete info->listener_; - if (info->type_support_) { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - ///// - // Create the Type Support struct - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto type_support_impl = type_registry.get_message_type_support(type_support); - if (!type_support_impl) { - RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); - return nullptr; - } - auto return_type_support = rcpputils::make_scope_exit( - [&type_registry, type_support]() { - type_registry.return_message_type_support(type_support); - }); - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support_impl; - - if (!fastdds_type) { - auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); - if (!tsupport) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); - return nullptr; + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() + { + delete info->listener_; + if (info->type_support_) + { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + ///// + // Create the Type Support struct + TypeSupportRegistry& type_registry = TypeSupportRegistry::get_instance(); + auto type_support_impl = type_registry.get_message_type_support(type_support); + if (!type_support_impl) + { + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); + return nullptr; + } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() + { + type_registry.return_message_type_support(type_support); + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support_impl; + + if (!fastdds_type) + { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!tsupport) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); + return nullptr; + } + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) + { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + + if (!info->listener_) + { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) + { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; } - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { - RMW_SET_ERROR_MSG("create_subscription() failed to register type"); - return nullptr; - } - - info->type_support_ = fastdds_type; - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; - } - - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - des_topic = topic.desc; - - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - return nullptr; - } - - eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; - switch (subscription_options->require_unique_network_flow_endpoints) { - default: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: - // Unique network flow endpoints not required. We leave the decission to the XML profile. - break; - - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: - // Ensure we request unique network flow endpoints - if (nullptr == - PropertyPolicyHelper::find_property( - reader_qos.properties(), - "fastdds.unique_network_flows")) - { - reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); - } - break; - } - - // Creates DataReader (with subscriber name to not change name policy) - info->data_reader_ = subscriber->create_datareader( - des_topic, - reader_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - if (!info->data_reader_ && - (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == - subscription_options->require_unique_network_flow_endpoints)) - { + des_topic = topic.desc; + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) + { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) + { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) + { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + return nullptr; + } + + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints){ + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + + // Creates DataReader (with subscriber name to not change name policy) info->data_reader_ = subscriber->create_datareader( - des_topic, - original_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - } - - if (!info->data_reader_) { - RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); - return nullptr; - } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_subscription = rcpputils::make_scope_exit( - [rmw_subscription]() { - rmw_free(const_cast(rmw_subscription->topic_name)); - rmw_subscription_free(rmw_subscription); - }); - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed - rmw_subscription->is_cft_enabled = false; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - return_type_support.cancel(); - cleanup_info.cancel(); - return rmw_subscription; + des_topic, + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + } + + if (!info->data_reader_) + { + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); + return nullptr; + } + + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() + { + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) + { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() + { + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + if (!rmw_subscription->topic_name) + { + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed + rmw_subscription->is_cft_enabled = false; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + return_type_support.cancel(); + cleanup_info.cancel(); + return rmw_subscription; } + } // namespace rmw_fastrtps_dynamic_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 45ec9d3a2..1478424e3 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 @@ -15,9 +15,6 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ -#include -#include -#include #include #include #include @@ -51,223 +48,163 @@ class ClientPubListener; typedef struct CustomClientInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void * request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void * response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader * response_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter * request_writer_{nullptr}; - - std::string request_topic_; - std::string response_topic_; - - ClientListener * listener_{nullptr}; - eprosima::fastrtps::rtps::GUID_t writer_guid_; - eprosima::fastrtps::rtps::GUID_t reader_guid_; - - const char * typesupport_identifier_{nullptr}; - ClientPubListener * pub_listener_{nullptr}; - std::atomic_size_t response_subscriber_matched_count_; - std::atomic_size_t request_publisher_matched_count_; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void* request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void* response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader* response_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter* request_writer_{nullptr}; + + std::string request_topic_; + std::string response_topic_; + + ClientListener* listener_{nullptr}; + eprosima::fastrtps::rtps::GUID_t writer_guid_; + eprosima::fastrtps::rtps::GUID_t reader_guid_; + + const char* typesupport_identifier_{nullptr}; + ClientPubListener* pub_listener_{nullptr}; + std::atomic_size_t response_subscriber_matched_count_; + std::atomic_size_t request_publisher_matched_count_; } CustomClientInfo; typedef struct CustomClientResponse { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - std::unique_ptr buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + std::unique_ptr buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: - explicit ClientListener(CustomClientInfo * info) - : info_(info), list_has_data_(false), - conditionMutex_(nullptr), conditionVariable_(nullptr) {} - - - void - on_data_available(eprosima::fastdds::dds::DataReader * reader) - { - assert(reader); - - CustomClientResponse 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.is_cdr_buffer = true; - data.data = response.buffer_.get(); - data.impl = nullptr; // not used when is_cdr_buffer is true - while (reader->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (response.sample_info_.valid_data) { - response.sample_identity_ = response.sample_info_.related_sample_identity; - - if (response.sample_identity_.writer_guid() == info_->reader_guid_ || - response.sample_identity_.writer_guid() == info_->writer_guid_) + + explicit ClientListener( + CustomClientInfo* info) + : info_(info) + { + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader*) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (on_new_response_cb_) { - std::lock_guard lock(internalMutex_); - const eprosima::fastrtps::HistoryQosPolicy & history = reader->get_qos().history(); - if (eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == history.kind) { - while (list.size() >= static_cast(history.depth)) { - list.pop_front(); - } - } - - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); - list.emplace_back(std::move(response)); - // the change to list_has_data_ needs to be mutually exclusive with - // rmw_wait() which checks hasData() and decides if wait() needs to - // be called - list_has_data_.store(true); - clock.unlock(); - conditionVariable_->notify_one(); - } else { - list.emplace_back(std::move(response)); - list_has_data_.store(true); - } - - std::unique_lock lock_mutex(on_new_response_m_); - - if (on_new_response_cb_) { on_new_response_cb_(user_data_, 1); - } else { - unread_count_++; - } } - } } - } - - bool - getResponse(CustomClientResponse & response) - { - std::lock_guard lock(internalMutex_); - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); - return popResponse(response); - } - return popResponse(response); - } - - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasData() - { - return list_has_data_.load(); - } - - void on_subscription_matched( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final - { - if (info_ == nullptr) { - return; - } - if (info.current_count_change == 1) { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } else if (info.current_count_change == -1) { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } else { - return; + void on_subscription_matched( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final + { + if (info_ == nullptr) + { + return; + } + if (info.current_count_change == 1) + { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + else if (info.current_count_change == -1) + { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + else + { + return; + } + info_->response_subscriber_matched_count_.store(publishers_.size()); } - info_->response_subscriber_matched_count_.store(publishers_.size()); - } - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_response_callback( - const void * user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_response_m_); - - if (callback) { - // Push events arrived before setting the the executor callback - if (unread_count_) { - callback(user_data, unread_count_); - unread_count_ = 0; - } - user_data_ = user_data; - on_new_response_cb_ = callback; - } else { - user_data_ = nullptr; - on_new_response_cb_ = nullptr; + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_response_callback( + const void* user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (callback) + { + auto unread_count = info_->response_reader_->get_unread_count(); + // Push events arrived before setting the the executor callback + if (unread_count) + { + callback(user_data, unread_count); + } + user_data_ = user_data; + on_new_response_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener(this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } + else + { + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener(this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_response_cb_ = nullptr; + } } - } private: - bool popResponse(CustomClientResponse & response) RCPPUTILS_TSA_REQUIRES(internalMutex_) - { - if (!list.empty()) { - response = std::move(list.front()); - list.pop_front(); - list_has_data_.store(!list.empty()); - return true; - } - return false; - }; - - CustomClientInfo * info_; - std::mutex internalMutex_; - std::list list RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::atomic_bool list_has_data_; - std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::set publishers_; - - rmw_event_callback_t on_new_response_cb_{nullptr}; - const void * user_data_{nullptr}; - std::mutex on_new_response_m_; - uint64_t unread_count_ = 0; + + CustomClientInfo* info_; + + std::set publishers_; + + rmw_event_callback_t on_new_response_cb_{nullptr}; + + const void* user_data_{nullptr}; + + std::mutex on_new_response_m_; }; class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { public: - explicit ClientPubListener(CustomClientInfo * info) - : info_(info) - { - } - - void on_publication_matched( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus & info) final - { - if (info_ == nullptr) { - return; + + explicit ClientPubListener( + CustomClientInfo* info) + : info_(info) + { } - if (info.current_count_change == 1) { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } else if (info.current_count_change == -1) { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } else { - return; + + void on_publication_matched( + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus& info) final + { + if (info_ == nullptr) + { + return; + } + if (info.current_count_change == 1) + { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + else if (info.current_count_change == -1) + { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + else + { + return; + } + info_->request_publisher_matched_count_.store(subscriptions_.size()); } - info_->request_publisher_matched_count_.store(subscriptions_.size()); - } private: - CustomClientInfo * info_; - std::set subscriptions_; + + CustomClientInfo* info_; + 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_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index 0eda78002..45d631a69 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -15,10 +15,6 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ -#include -#include -#include -#include #include #include @@ -34,25 +30,10 @@ class EventListenerInterface { -protected: - - class ConditionalScopedLock; - public: virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; - - //TODO(richiwre) review - /// Connect a condition variable so a waiter can be notified of new data. - virtual void attachCondition( - std::mutex* conditionMutex, - std::condition_variable* conditionVariable) = 0; - - //TODO(richiwre) review - /// Unset the information from attachCondition. - virtual void detachCondition() = 0; - /// Take ready data for an event type. /** * \param event_type The event type to get data for. @@ -83,40 +64,6 @@ class EventListenerInterface std::mutex on_new_event_m_; }; -class EventListenerInterface::ConditionalScopedLock -{ -public: - - ConditionalScopedLock( - std::mutex* mutex, - std::condition_variable* condition_variable = nullptr) - : mutex_(mutex) - , cv_(condition_variable) - { - if (nullptr != mutex_) - { - mutex_->lock(); - } - } - - ~ConditionalScopedLock() - { - if (nullptr != mutex_) - { - mutex_->unlock(); - if (nullptr != cv_) - { - cv_->notify_all(); - } - } - } - -private: - - std::mutex* mutex_; - std::condition_variable* cv_; -}; - struct CustomEventInfo { virtual EventListenerInterface* get_listener() const = 0; 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 dc1c961f4..bb344d733 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 @@ -64,8 +64,6 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds , deadline_changes_(false) , liveliness_changes_(false) , incompatible_qos_changes_(false) - , conditionMutex_(nullptr) - , conditionVariable_(nullptr) { } @@ -76,7 +74,7 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds eprosima::fastdds::dds::DataWriter* /* writer */, const eprosima::fastdds::dds::PublicationMatchedStatus& info) final { - std::lock_guard lock(internalMutex_); + std::lock_guard lock(discovery_m_); if (info.current_count_change == 1) { subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); @@ -108,28 +106,10 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds // PubListener API size_t subscriptionCount() { - std::lock_guard lock(internalMutex_); + std::lock_guard lock(discovery_m_); return subscriptions_.size(); } - void - attachCondition( - std::mutex* conditionMutex, - std::condition_variable* conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - RMW_FASTRTPS_SHARED_CPP_PUBLIC eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; @@ -148,31 +128,26 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds CustomPublisherInfo* publisher_info_ = nullptr; - mutable std::mutex internalMutex_; - std::set subscriptions_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + discovery_m_); + + std::mutex discovery_m_; bool deadline_changes_; eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); bool liveliness_changes_; eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); bool incompatible_qos_changes_; eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); - - std::mutex* conditionMutex_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); - std::condition_variable* conditionVariable_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 146eb33ad..9805efbda 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 @@ -15,10 +15,8 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ -#include -#include -#include #include +#include #include #include @@ -50,297 +48,233 @@ class ServicePubListener; enum class client_present_t { - FAILURE, // an error occurred when checking - MAYBE, // reader not matched, writer still present - YES, // reader matched - GONE // neither reader nor writer + FAILURE, // an error occurred when checking + MAYBE, // reader not matched, writer still present + YES, // reader matched + GONE // neither reader nor writer }; typedef struct CustomServiceInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void * request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void * response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void* request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void* response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader* request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter* response_writer_{nullptr}; - ServiceListener * listener_{nullptr}; - ServicePubListener * pub_listener_{nullptr}; + ServiceListener* listener_{nullptr}; + ServicePubListener* pub_listener_{nullptr}; - const char * typesupport_identifier_{nullptr}; + const char* typesupport_identifier_{nullptr}; } CustomServiceInfo; typedef struct CustomServiceRequest { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - eprosima::fastcdr::FastBuffer * buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastcdr::FastBuffer* buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; + + CustomServiceRequest() + : buffer_(nullptr) + { + } - CustomServiceRequest() - : buffer_(nullptr) {} } CustomServiceRequest; class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { - using subscriptions_set_t = - std::unordered_set; - using clients_endpoints_map_t = - std::unordered_map; + using subscriptions_set_t = + std::unordered_set; + using clients_endpoints_map_t = + std::unordered_map; public: - explicit ServicePubListener(CustomServiceInfo * info) - { - (void) info; - } - - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter * /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus & info) final - { - std::lock_guard lock(mutex_); - if (info.current_count_change == 1) { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } else if (info.current_count_change == -1) { - eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); - subscriptions_.erase(erase_endpoint_guid); - auto endpoint = clients_endpoints_.find(erase_endpoint_guid); - if (endpoint != clients_endpoints_.end()) { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(erase_endpoint_guid); - } - } else { - return; - } - cv_.notify_all(); - } - - template - bool - wait_for_subscription( - const eprosima::fastrtps::rtps::GUID_t & guid, - const std::chrono::duration & rel_time) - { - auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool + + explicit ServicePubListener( + CustomServiceInfo* info) { - return subscriptions_.find(guid) != subscriptions_.end(); - }; + (void) info; + } - std::unique_lock lock(mutex_); - return cv_.wait_for(lock, rel_time, guid_is_present); - } + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter* /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus& info) final + { + std::lock_guard lock(mutex_); + if (info.current_count_change == 1) + { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + else if (info.current_count_change == -1) + { + eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + subscriptions_.erase(erase_endpoint_guid); + auto endpoint = clients_endpoints_.find(erase_endpoint_guid); + if (endpoint != clients_endpoints_.end()) + { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(erase_endpoint_guid); + } + } + else + { + return; + } + cv_.notify_all(); + } - client_present_t - check_for_subscription( - const eprosima::fastrtps::rtps::GUID_t & guid) - { + template + bool + wait_for_subscription( + const eprosima::fastrtps::rtps::GUID_t& guid, + const std::chrono::duration& rel_time) { - std::lock_guard lock(mutex_); - // Check if the guid is still in the map - if (clients_endpoints_.find(guid) == clients_endpoints_.end()) { - // Client is gone - return client_present_t::GONE; - } + auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool + { + return subscriptions_.find(guid) != subscriptions_.end(); + }; + + std::unique_lock lock(mutex_); + return cv_.wait_for(lock, rel_time, guid_is_present); } - // Wait for subscription - if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) { - return client_present_t::MAYBE; + + client_present_t + check_for_subscription( + const eprosima::fastrtps::rtps::GUID_t& guid) + { + { + std::lock_guard lock(mutex_); + // Check if the guid is still in the map + if (clients_endpoints_.find(guid) == clients_endpoints_.end()) + { + // Client is gone + return client_present_t::GONE; + } + } + // Wait for subscription + if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) + { + return client_present_t::MAYBE; + } + return client_present_t::YES; } - return client_present_t::YES; - } - - void endpoint_erase_if_exists(const eprosima::fastrtps::rtps::GUID_t & endpointGuid) - { - std::lock_guard lock(mutex_); - auto endpoint = clients_endpoints_.find(endpointGuid); - if (endpoint != clients_endpoints_.end()) { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(endpointGuid); + + void endpoint_erase_if_exists( + const eprosima::fastrtps::rtps::GUID_t& endpointGuid) + { + std::lock_guard lock(mutex_); + auto endpoint = clients_endpoints_.find(endpointGuid); + if (endpoint != clients_endpoints_.end()) + { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(endpointGuid); + } } - } - void endpoint_add_reader_and_writer( - const eprosima::fastrtps::rtps::GUID_t & readerGuid, - const eprosima::fastrtps::rtps::GUID_t & writerGuid) - { - std::lock_guard lock(mutex_); - clients_endpoints_.emplace(readerGuid, writerGuid); - clients_endpoints_.emplace(writerGuid, readerGuid); - } + void endpoint_add_reader_and_writer( + const eprosima::fastrtps::rtps::GUID_t& readerGuid, + const eprosima::fastrtps::rtps::GUID_t& writerGuid) + { + std::lock_guard lock(mutex_); + clients_endpoints_.emplace(readerGuid, writerGuid); + clients_endpoints_.emplace(writerGuid, readerGuid); + } private: - std::mutex mutex_; - subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - std::condition_variable cv_; + + std::mutex mutex_; + subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + std::condition_variable cv_; }; class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: - explicit ServiceListener(CustomServiceInfo * info) - : info_(info), list_has_data_(false), - conditionMutex_(nullptr), conditionVariable_(nullptr) - { - } - - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader * , - const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final - { - if (info.current_count_change == -1) { - info_->pub_listener_->endpoint_erase_if_exists( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - } - - void - on_data_available(eprosima::fastdds::dds::DataReader * reader) final - { - assert(reader); - - CustomServiceRequest request; - request.buffer_ = new eprosima::fastcdr::FastBuffer(); - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = request.buffer_; - data.impl = nullptr; // not used when is_cdr_buffer is true - while (reader->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (request.sample_info_.valid_data) { - request.sample_identity_ = request.sample_info_.sample_identity; - // Use response subscriber guid (on related_sample_identity) when present. - const eprosima::fastrtps::rtps::GUID_t & reader_guid = - request.sample_info_.related_sample_identity.writer_guid(); - if (reader_guid != eprosima::fastrtps::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 = - request.sample_info_.sample_identity.writer_guid(); - info_->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); - - std::lock_guard lock(internalMutex_); - const eprosima::fastrtps::HistoryQosPolicy & history = reader->get_qos().history(); - if (eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == history.kind) { - while (list.size() >= static_cast(history.depth)) { - list.pop_front(); - } - } + explicit ServiceListener( + CustomServiceInfo* info) + : info_(info) + { + } - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); - list.push_back(request); - // the change to list_has_data_ needs to be mutually exclusive with - // rmw_wait() which checks hasData() and decides if wait() needs to - // be called - list_has_data_.store(true); - clock.unlock(); - conditionVariable_->notify_one(); - } else { - list.push_back(request); - list_has_data_.store(true); + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader*, + const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final + { + if (info.current_count_change == -1) + { + info_->pub_listener_->endpoint_erase_if_exists( + eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } + } + void + on_data_available( + eprosima::fastdds::dds::DataReader*) final + { std::unique_lock lock_mutex(on_new_request_m_); - if (on_new_request_cb_) { - on_new_request_cb_(user_data_, 1); - } else { - unread_count_++; + if (on_new_request_cb_) + { + on_new_request_cb_(user_data_, 1); } - } - } - } - - CustomServiceRequest - getRequest() - { - std::lock_guard lock(internalMutex_); - CustomServiceRequest request; - - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); - if (!list.empty()) { - request = list.front(); - list.pop_front(); - list_has_data_.store(!list.empty()); - } - } else { - if (!list.empty()) { - request = list.front(); - list.pop_front(); - list_has_data_.store(!list.empty()); - } } - return request; - } - - void - attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasData() - { - return list_has_data_.load(); - } - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_request_callback( - const void * user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_request_m_); - - if (callback) { - // Push events arrived before setting the the executor callback - if (unread_count_) { - callback(user_data, unread_count_); - unread_count_ = 0; - } - user_data_ = user_data; - on_new_request_cb_ = callback; - } else { - user_data_ = nullptr; - on_new_request_cb_ = nullptr; + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_request_callback( + const void* user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_request_m_); + + if (callback) + { + auto unread_count = info_->request_reader_->get_unread_count(); + // Push events arrived before setting the the executor callback + if (unread_count) + { + callback(user_data, unread_count); + } + + user_data_ = user_data; + on_new_request_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener(this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } + else + { + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener(this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_request_cb_ = nullptr; + } } - } private: - CustomServiceInfo * info_; - std::mutex internalMutex_; - std::list list RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::atomic_bool list_has_data_; - std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); - - rmw_event_callback_t on_new_request_cb_{nullptr}; - const void * user_data_{nullptr}; - std::mutex on_new_request_m_; - uint64_t unread_count_ = 0; + + CustomServiceInfo* info_; + + rmw_event_callback_t on_new_request_cb_{nullptr}; + + const void* user_data_{nullptr}; + + std::mutex on_new_request_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ 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 00364e533..de35ab472 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 @@ -15,11 +15,8 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ -#include #include -#include #include -#include #include #include #include @@ -86,29 +83,23 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds public: explicit SubListener( - CustomSubscriberInfo* info, - size_t qos_depth) + CustomSubscriberInfo* info) : subscriber_info_(info) - , data_(false) , deadline_changes_(false) , liveliness_changes_(false) , sample_lost_changes_(false) , incompatible_qos_changes_(false) - , conditionMutex_(nullptr) - , conditionVariable_(nullptr) { - //TODO(richiware) Review - qos_depth_ = (qos_depth > 0) ? qos_depth : std::numeric_limits::max(); } // DataReaderListener implementation void on_subscription_matched( - eprosima::fastdds::dds::DataReader* reader, + eprosima::fastdds::dds::DataReader*, const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final { { - std::lock_guard lock(internalMutex_); + std::lock_guard lock(discovery_m_); if (info.current_count_change == 1) { publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); @@ -118,26 +109,11 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } } - update_has_data(reader); } void on_data_available( - eprosima::fastdds::dds::DataReader* reader) final - { - update_has_data(reader); - - std::unique_lock lock_mutex(on_new_message_m_); - - if (on_new_message_cb_) - { - on_new_message_cb_(user_data_, 1); - } - else - { - new_data_unread_count_++; - } - } + eprosima::fastdds::dds::DataReader* reader) final; RMW_FASTRTPS_SHARED_CPP_PUBLIC void @@ -163,48 +139,9 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds eprosima::fastdds::dds::DataReader*, const eprosima::fastdds::dds::RequestedIncompatibleQosStatus&) final; - // SubListener API - void - attachCondition( - std::mutex* conditionMutex, - std::condition_variable* conditionVariable) - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; - } - - void - detachCondition() - { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; - } - - bool - hasData() const - { - return data_.load(std::memory_order_relaxed); - } - - void - update_has_data( - eprosima::fastdds::dds::DataReader* reader) - { - // Make sure to call into Fast DDS before taking the lock to avoid an - // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. - auto unread_count = reader->get_unread_count(); - bool has_data = unread_count > 0; - - std::lock_guard lock(internalMutex_); - ConditionalScopedLock clock(conditionMutex_, conditionVariable_); - data_.store(has_data, std::memory_order_relaxed); - } - size_t publisherCount() { - std::lock_guard lock(internalMutex_); + std::lock_guard lock(discovery_m_); return publishers_.size(); } @@ -213,29 +150,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds void set_on_new_message_callback( const void* user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_message_m_); - - if (callback) - { - // Push events arrived before setting the executor's callback - if (new_data_unread_count_) - { - auto unread_count = std::min(new_data_unread_count_, qos_depth_); - callback(user_data, unread_count); - new_data_unread_count_ = 0; - } - //TODO(richiware) - //user_data_ = user_data; - on_new_message_cb_ = callback; - } - else - { - //user_data_ = nullptr; - on_new_message_cb_ = nullptr; - } - } + rmw_event_callback_t callback); RMW_FASTRTPS_SHARED_CPP_PUBLIC eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; @@ -255,42 +170,36 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds CustomSubscriberInfo* subscriber_info_ = nullptr; - mutable std::mutex internalMutex_; - - std::atomic_bool data_; - bool deadline_changes_; eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); bool liveliness_changes_; eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); bool sample_lost_changes_; eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + on_new_event_m_); bool incompatible_qos_changes_; eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); - - std::mutex* conditionMutex_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); - std::condition_variable* conditionVariable_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + discovery_m_); std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( internalMutex_); rmw_event_callback_t on_new_message_cb_{nullptr}; + + const void* new_message_user_data_{nullptr}; + std::mutex on_new_message_m_; - size_t qos_depth_; - size_t new_data_unread_count_ = 0; + + std::mutex discovery_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index e3298ccc6..fb7c7c8de 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -201,6 +201,52 @@ void SubListener::set_on_new_event_callback( } } +void +SubListener::set_on_new_message_callback( + const void* user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_message_m_); + + if (callback) + { + auto unread_count = subscriber_info_->data_reader_->get_unread_count(); + // Push events arrived before setting the executor's callback + if (unread_count) + { + callback(user_data, unread_count); + } + + new_message_user_data_ = user_data; + on_new_message_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener(this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } + else + { + eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener(this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + new_message_user_data_ = nullptr; + on_new_message_cb_ = nullptr; + } +} + +void +SubListener::on_data_available( + eprosima::fastdds::dds::DataReader*) +{ + std::unique_lock lock_mutex(on_new_message_m_); + + if (on_new_message_cb_) + { + on_new_message_cb_(new_message_user_data_, 1); + } +} + void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader*, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index fcb6b19e2..3269f4acb 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -30,132 +30,135 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_destroy_client( - const char * identifier, - rmw_node_t * node, - rmw_client_t * client) + const char* identifier, + rmw_node_t* node, + rmw_client_t* client) { - rmw_ret_t final_ret = RMW_RET_OK; - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(client->data); - - { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_writer_->guid()); - common_context->graph_cache.dissociate_writer( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_reader_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); - } - - auto show_previous_error = - [&final_ret]() { - if (RMW_RET_OK != final_ret) { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_reader_->get_topicdescription(); - auto request_topic = info->request_writer_->get_topic(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); - if (ret != ReturnCode_t::RETCODE_OK) { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); - final_ret = RMW_RET_ERROR; - info->response_reader_->set_listener(nullptr); - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->request_writer_); - if (ret != ReturnCode_t::RETCODE_OK) { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); - final_ret = RMW_RET_ERROR; - info->request_writer_->set_listener(nullptr); + rmw_ret_t final_ret = RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(client->data); + + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_writer_->guid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_reader_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); } - // Delete DataWriter listener - if (nullptr != info->pub_listener_) { - delete info->pub_listener_; + auto show_previous_error = + [&final_ret]() + { + if (RMW_RET_OK != final_ret) + { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_reader_->get_topicdescription(); + auto request_topic = info->request_writer_->get_topic(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); + if (ret != ReturnCode_t::RETCODE_OK) + { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = RMW_RET_ERROR; + info->response_reader_->set_listener(nullptr); + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); + if (ret != ReturnCode_t::RETCODE_OK) + { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = RMW_RET_ERROR; + info->request_writer_->set_listener(nullptr); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) + { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomClientInfo structure + delete info; } - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + rmw_free(const_cast(client->service_name)); + rmw_client_free(client); - // Delete CustomClientInfo structure - delete info; - } - - rmw_free(const_cast(client->service_name)); - rmw_client_free(client); - - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_client_request_publisher_get_actual_qos( - const rmw_client_t * client, - rmw_qos_profile_t * qos) + const rmw_client_t* client, + rmw_qos_profile_t* qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataWriter * fastdds_rw = cli->request_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataWriter* fastdds_rw = cli->request_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_response_subscription_get_actual_qos( - const rmw_client_t * client, - rmw_qos_profile_t * qos) + const rmw_client_t* client, + rmw_qos_profile_t* qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataReader * fastdds_dr = cli->response_reader_; - dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataReader* fastdds_dr = cli->response_reader_; + dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_set_on_new_response_callback( - rmw_client_t * rmw_client, - rmw_event_callback_t callback, - const void * user_data) + rmw_client_t* rmw_client, + rmw_event_callback_t callback, + const void* user_data) { - auto custom_client_info = static_cast(rmw_client->data); - /* TODO - custom_client_info->listener_->set_on_new_response_callback( - user_data, - callback); - */ - return RMW_RET_OK; + auto custom_client_info = static_cast(rmw_client->data); + custom_client_info->listener_->set_on_new_response_callback( + user_data, + callback); + return RMW_RET_OK; } + } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 21fb355ed..94b9a7dc2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -28,135 +28,142 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_take_response( - const char * identifier, - const rmw_client_t * client, - rmw_service_info_t * request_header, - void * ros_response, - bool * taken) + const char* identifier, + const rmw_client_t* client, + rmw_service_info_t* request_header, + void* ros_response, + bool* taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - auto info = static_cast(client->data); - assert(info); + auto info = static_cast(client->data); + assert(info); - CustomClientResponse response; + CustomClientResponse response; - if (info->listener_->getResponse(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.is_cdr_buffer = true; data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->response_reader_->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (response.sample_info_.valid_data) { - response.sample_identity_ = response.sample_info_.related_sample_identity; - - if (response.sample_identity_.writer_guid() == info->reader_guid_ || - response.sample_identity_.writer_guid() == info->writer_guid_) - { - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser( - *response.buffer_, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_response, info->response_type_support_impl_)) + if (info->response_reader_->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { - request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); - request_header->request_id.sequence_number = - ((int64_t)response.sample_identity_.sequence_number().high) << - 32 | response.sample_identity_.sequence_number().low; - - *taken = true; - } + if (response.sample_info_.valid_data) + { + response.sample_identity_ = response.sample_info_.related_sample_identity; + + if (response.sample_identity_.writer_guid() == info->reader_guid_ || + response.sample_identity_.writer_guid() == info->writer_guid_) + { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser( + *response.buffer_, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_response, info->response_type_support_impl_)) + { + request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); + request_header->request_id.sequence_number = + ((int64_t)response.sample_identity_.sequence_number().high) << + 32 | response.sample_identity_.sequence_number().low; + + *taken = true; + } + } } - } } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_send_response( - const char * identifier, - const rmw_service_t * service, - rmw_request_id_t * request_header, - void * ros_response) + const char* identifier, + const rmw_service_t* service, + rmw_request_id_t* request_header, + void* ros_response) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(service->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( - request_header->writer_guid, - &wparams.related_sample_identity().writer_guid()); - wparams.related_sample_identity().sequence_number().high = - (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); - wparams.related_sample_identity().sequence_number().low = - (int32_t)(request_header->sequence_number & 0xFFFFFFFF); - - // TODO(MiguelCompany) The following block is a workaround for the race on the - // discovery of services. It is (ab)using a related_sample_identity on the request - // with the GUID of the response reader, so we can wait here for it to be matched to - // the server response writer. In the future, this should be done with the mechanism - // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) - - // According to the list of possible entity kinds in section 9.3.1.2 of RTPS - // 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 = - 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. - // Wait for the response writer to be matched with it. - auto listener = info->pub_listener_; - client_present_t ret = listener->check_for_subscription(related_guid); - if (ret == client_present_t::GONE) { - return RMW_RET_OK; - } else if (ret == client_present_t::MAYBE) { - RMW_SET_ERROR_MSG("client will not receive response"); - return RMW_RET_TIMEOUT; + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(service->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + request_header->writer_guid, + &wparams.related_sample_identity().writer_guid()); + wparams.related_sample_identity().sequence_number().high = + (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); + wparams.related_sample_identity().sequence_number().low = + (int32_t)(request_header->sequence_number & 0xFFFFFFFF); + + // TODO(MiguelCompany) The following block is a workaround for the race on the + // discovery of services. It is (ab)using a related_sample_identity on the request + // with the GUID of the response reader, so we can wait here for it to be matched to + // the server response writer. In the future, this should be done with the mechanism + // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) + + // According to the list of possible entity kinds in section 9.3.1.2 of RTPS + // 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 = + 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. + // Wait for the response writer to be matched with it. + auto listener = info->pub_listener_; + client_present_t ret = listener->check_for_subscription(related_guid); + if (ret == client_present_t::GONE) + { + return RMW_RET_OK; + } + else if (ret == client_present_t::MAYBE) + { + RMW_SET_ERROR_MSG("client will not receive response"); + return RMW_RET_TIMEOUT; + } + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_response); + data.impl = info->response_type_support_impl_; + if (info->response_writer_->write(&data, wparams)) + { + returnedValue = RMW_RET_OK; } - } - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_response); - data.impl = info->response_type_support_impl_; - if (info->response_writer_->write(&data, wparams)) { - returnedValue = RMW_RET_OK; - } else { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + else + { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } + } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index b363e660a..5da0eeac1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -42,130 +42,133 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_destroy_service( - const char * identifier, - rmw_node_t * node, - rmw_service_t * service) + const char* identifier, + rmw_node_t* node, + rmw_service_t* service) { - rmw_ret_t final_ret = RMW_RET_OK; - - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(service->data); - { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_reader_->guid()); - common_context->graph_cache.dissociate_reader( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_writer_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_writer( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); - } - - auto show_previous_error = - [&final_ret]() { - if (RMW_RET_OK != final_ret) { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_writer_->get_topic(); - auto request_topic = info->request_reader_->get_topicdescription(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); - if (ret != ReturnCode_t::RETCODE_OK) { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datareader"); - final_ret = RMW_RET_ERROR; - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->response_writer_); - if (ret != ReturnCode_t::RETCODE_OK) { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datawriter"); - final_ret = RMW_RET_ERROR; + rmw_ret_t final_ret = RMW_RET_OK; + + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(service->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_reader_->guid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_writer_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); } - // Delete DataWriter listener - if (nullptr != info->pub_listener_) { - delete info->pub_listener_; + auto show_previous_error = + [&final_ret]() + { + if (RMW_RET_OK != final_ret) + { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_writer_->get_topic(); + auto request_topic = info->request_reader_->get_topicdescription(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); + if (ret != ReturnCode_t::RETCODE_OK) + { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datareader"); + final_ret = RMW_RET_ERROR; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->response_writer_); + if (ret != ReturnCode_t::RETCODE_OK) + { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); + final_ret = RMW_RET_ERROR; + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) + { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomServiceInfo structure + delete info; } - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + rmw_free(const_cast(service->service_name)); + rmw_service_free(service); - // Delete CustomServiceInfo structure - delete info; - } - - rmw_free(const_cast(service->service_name)); - rmw_service_free(service); - - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_service_response_publisher_get_actual_qos( - const rmw_service_t * service, - rmw_qos_profile_t * qos) + const rmw_service_t* service, + rmw_qos_profile_t* qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataWriter * fastdds_rw = srv->response_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataWriter* fastdds_rw = srv->response_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_request_subscription_get_actual_qos( - const rmw_service_t * service, - rmw_qos_profile_t * qos) + const rmw_service_t* service, + rmw_qos_profile_t* qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataReader * fastdds_rr = srv->request_reader_; - dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataReader* fastdds_rr = srv->request_reader_; + dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_set_on_new_request_callback( - rmw_service_t * rmw_service, - rmw_event_callback_t callback, - const void * user_data) + rmw_service_t* rmw_service, + rmw_event_callback_t callback, + const void* user_data) { - auto custom_service_info = static_cast(rmw_service->data); - /* - custom_service_info->listener_->set_on_new_request_callback( - user_data, - callback); - */ - return RMW_RET_OK; + auto custom_service_info = static_cast(rmw_service->data); + custom_service_info->listener_->set_on_new_request_callback( + user_data, + callback); + return RMW_RET_OK; } + } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 238d5d6eb..32765a26d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -35,243 +35,260 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_destroy_subscription( - const char * identifier, - const rmw_node_t * node, - rmw_subscription_t * subscription, - bool reset_cft) + const char* identifier, + const rmw_node_t* node, + rmw_subscription_t* subscription, + bool reset_cft) { - assert(node->implementation_identifier == identifier); - assert(subscription->implementation_identifier == identifier); + assert(node->implementation_identifier == identifier); + assert(subscription->implementation_identifier == identifier); - rmw_ret_t ret = RMW_RET_OK; - rmw_error_state_t error_state; - rmw_error_string_t error_string; - auto common_context = static_cast(node->context->impl->common); - auto info = static_cast(subscription->data); - { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != ret) { - error_state = *rmw_get_error_state(); - error_string = rmw_get_error_string(); - rmw_reset_error(); + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + rmw_error_string_t error_string; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != ret) + { + error_state = *rmw_get_error_state(); + error_string = rmw_get_error_string(); + rmw_reset_error(); + } } - } - auto participant_info = - static_cast(node->context->impl->participant_info); - rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); - if (RMW_RET_OK != local_ret) { - if (RMW_RET_OK != ret) { - RMW_SAFE_FWRITE_TO_STDERR(error_string.str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); + if (RMW_RET_OK != local_ret) + { + if (RMW_RET_OK != ret) + { + RMW_SAFE_FWRITE_TO_STDERR(error_string.str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + } + ret = local_ret; } - ret = local_ret; - } else if (RMW_RET_OK != ret) { - rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); - } - return ret; + else if (RMW_RET_OK != ret) + { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + return ret; } rmw_ret_t __rmw_subscription_count_matched_publishers( - const rmw_subscription_t * subscription, - size_t * publisher_count) + const rmw_subscription_t* subscription, + size_t* publisher_count) { - auto info = static_cast(subscription->data); + auto info = static_cast(subscription->data); - *publisher_count = info->listener_->publisherCount(); + *publisher_count = info->listener_->publisherCount(); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_actual_qos( - const rmw_subscription_t * subscription, - rmw_qos_profile_t * qos) + const rmw_subscription_t* subscription, + rmw_qos_profile_t* qos) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader * fastdds_dr = info->data_reader_; - const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->get_qos(); + auto info = static_cast(subscription->data); + 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); + dds_qos_to_rmw_qos(dds_qos, qos); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_set_content_filter( - rmw_subscription_t * subscription, - const rmw_subscription_content_filter_options_t * options -) + rmw_subscription_t* subscription, + const rmw_subscription_content_filter_options_t* options + ) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; - const bool filter_expression_empty = (*options->filter_expression == '\0'); + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; + const bool filter_expression_empty = (*options->filter_expression == '\0'); - if (!filtered_topic && filter_expression_empty) { - // can't reset current subscriber - RMW_SET_ERROR_MSG( - "current subscriber has no content filter topic"); - return RMW_RET_ERROR; - } else if (filtered_topic && !filter_expression_empty) { - std::vector expression_parameters; - for (size_t i = 0; i < options->expression_parameters.size; ++i) { - expression_parameters.push_back(options->expression_parameters.data[i]); + if (!filtered_topic && filter_expression_empty) + { + // can't reset current subscriber + RMW_SET_ERROR_MSG( + "current subscriber has no content filter topic"); + return RMW_RET_ERROR; } + else if (filtered_topic && !filter_expression_empty) + { + std::vector expression_parameters; + for (size_t i = 0; i < options->expression_parameters.size; ++i) + { + expression_parameters.push_back(options->expression_parameters.data[i]); + } - ReturnCode_t ret = - filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG( - "failed to set_filter_expression"); - return RMW_RET_ERROR; + ReturnCode_t ret = + filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) + { + RMW_SET_ERROR_MSG( + "failed to set_filter_expression"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; } - return RMW_RET_OK; - } - eprosima::fastdds::dds::DomainParticipant * dds_participant = - info->dds_participant_; - eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; - const char * eprosima_fastrtps_identifier = subscription->implementation_identifier; + eprosima::fastdds::dds::DomainParticipant* dds_participant = + info->dds_participant_; + eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; + const char* eprosima_fastrtps_identifier = subscription->implementation_identifier; - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( - eprosima_fastrtps_identifier, - info->node_, - subscription, - true /* reset_cft */); - if (ret != RMW_RET_OK) { - RMW_SET_ERROR_MSG("delete subscription with reset cft"); - return RMW_RET_ERROR; - } + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, + info->node_, + subscription, + true /* reset_cft */); + if (ret != RMW_RET_OK) + { + RMW_SET_ERROR_MSG("delete subscription with reset cft"); + return RMW_RET_ERROR; + } - if (!filtered_topic) { - // create content filtered topic - eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, info->topic_, - info->topic_name_mangled_, options, &filtered_topic)) + if (!filtered_topic) + { + // create content filtered topic + eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, info->topic_, + info->topic_name_mangled_, options, &filtered_topic)) + { + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + return RMW_RET_ERROR; + } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; + } + else { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); - return RMW_RET_ERROR; + // use the existing parent topic + des_topic = info->topic_; } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; - } else { - // use the existing parent topic - des_topic = info->topic_; - } - // create data reader - eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; - const rmw_subscription_options_t * subscription_options = - &subscription->options; - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) - { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); - return RMW_RET_ERROR; - } - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() { - subscriber->delete_datareader(info->data_reader_); - }); + // create data reader + eprosima::fastdds::dds::Subscriber* subscriber = info->subscriber_; + const rmw_subscription_options_t* subscription_options = + &subscription->options; + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return RMW_RET_ERROR; + } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() + { + subscriber->delete_datareader(info->data_reader_); + }); - ///// - // Update RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); + ///// + // Update RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); - { - rmw_dds_common::Context * common_context = info->common_context_; - const rmw_node_t * node = info->node_; + { + rmw_dds_common::Context* common_context = info->common_context_; + const rmw_node_t* node = info->node_; - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - eprosima_fastrtps_identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != rmw_ret) { - static_cast(common_context->graph_cache.dissociate_writer( - info->subscription_gid_, common_context->gid, node->name, node->namespace_)); - return RMW_RET_ERROR; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) + { + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + return RMW_RET_ERROR; + } } - } - cleanup_datareader.cancel(); - return RMW_RET_OK; + cleanup_datareader.cancel(); + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_content_filter( - const rmw_subscription_t * subscription, - rcutils_allocator_t * allocator, - rmw_subscription_content_filter_options_t * options -) + const rmw_subscription_t* subscription, + rcutils_allocator_t* allocator, + rmw_subscription_content_filter_options_t* options + ) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; - if (nullptr == filtered_topic) { - RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); - return RMW_RET_ERROR; - } - std::vector expression_parameters; - ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) { - RMW_SET_ERROR_MSG( - "failed to get_expression_parameters"); - return RMW_RET_ERROR; - } + if (nullptr == filtered_topic) + { + RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); + return RMW_RET_ERROR; + } + std::vector expression_parameters; + ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) + { + RMW_SET_ERROR_MSG( + "failed to get_expression_parameters"); + return RMW_RET_ERROR; + } - std::vector string_array; - for (size_t i = 0; i < expression_parameters.size(); ++i) { - string_array.push_back(expression_parameters[i].c_str()); - } + std::vector string_array; + for (size_t i = 0; i < expression_parameters.size(); ++i) + { + string_array.push_back(expression_parameters[i].c_str()); + } - return rmw_subscription_content_filter_options_init( - filtered_topic->get_filter_expression().c_str(), - string_array.size(), - string_array.data(), - allocator, - options - ); + return rmw_subscription_content_filter_options_init( + filtered_topic->get_filter_expression().c_str(), + string_array.size(), + string_array.data(), + allocator, + options + ); } rmw_ret_t __rmw_subscription_set_on_new_message_callback( - rmw_subscription_t * rmw_subscription, - rmw_event_callback_t callback, - const void * user_data) + rmw_subscription_t* rmw_subscription, + rmw_event_callback_t callback, + const void* user_data) { - auto custom_subscriber_info = static_cast(rmw_subscription->data); - /* TODO - custom_subscriber_info->listener_->set_on_new_message_callback( - user_data, - callback); - */ - return RMW_RET_OK; + auto custom_subscriber_info = static_cast(rmw_subscription->data); + custom_subscriber_info->listener_->set_on_new_message_callback( + user_data, + callback); + return RMW_RET_OK; } + } // namespace rmw_fastrtps_shared_cpp From 2ee24cf057ba53812e99349e90bcc865e9e57c6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 1 Jul 2022 07:26:29 +0200 Subject: [PATCH 07/34] Fixing tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../src/init_rmw_context_impl.cpp | 314 ++++++++++-------- .../src/init_rmw_context_impl.cpp | 314 ++++++++++-------- .../custom_client_info.hpp | 39 ++- .../custom_service_info.hpp | 37 ++- .../custom_subscriber_info.hpp | 2 + .../src/custom_subscriber_info.cpp | 38 ++- .../src/init_rmw_context_impl.cpp | 152 +++++---- .../src/listener_thread.cpp | 264 ++++++++------- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 196 +++++------ rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 45 ++- 10 files changed, 802 insertions(+), 599 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index ec5e7db01..b5310cc6c 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -42,156 +42,178 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t -init_context_impl(rmw_context_t * context) +init_context_impl( + rmw_context_t* context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo * participant_info) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t * pub) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t * sub) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t * p) { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) + { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo* participant_info) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) + { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t* pub) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) + { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t* sub) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) + { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t* p) + { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) + { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) + { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() + { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t -rmw_fastrtps_cpp::increment_context_impl_ref_count(rmw_context_t * context) +rmw_fastrtps_cpp::increment_context_impl_ref_count( + rmw_context_t* context) { - assert(context); - assert(context->impl); - - std::lock_guard guard(context->impl->mutex); - - if (!context->impl->count) { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) { - return ret; + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + + if (!context->impl->count) + { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) + { + return ret; + } } - } - context->impl->count++; - return RMW_RET_OK; + context->impl->count++; + return RMW_RET_OK; } diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index e7daf769d..1e37ff6b8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -42,156 +42,178 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t -init_context_impl(rmw_context_t * context) +init_context_impl( + rmw_context_t* context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo * participant_info) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_dynamic_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t * pub) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_dynamic_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t * sub) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t * p) { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) + { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo* participant_info) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) + { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_dynamic_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t* pub) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) + { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t* sub) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) + { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t* p) + { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) + { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) + { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() + { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t -rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(rmw_context_t * context) +rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count( + rmw_context_t* context) { - assert(context); - assert(context->impl); - - std::lock_guard guard(context->impl->mutex); - - if (!context->impl->count) { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) { - return ret; + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + + if (!context->impl->count) + { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) + { + return ret; + } } - } - context->impl->count++; - return RMW_RET_OK; + context->impl->count++; + return RMW_RET_OK; } 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 1478424e3..9a75b9149 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 @@ -93,7 +93,12 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener if (on_new_response_cb_) { - on_new_response_cb_(user_data_, 1); + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) + { + on_new_response_cb_(user_data_, unread_responses); + } } } @@ -120,6 +125,29 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener info_->response_subscriber_matched_count_.store(publishers_.size()); } + size_t get_unread_responses() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read(data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + { + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) + { + ++ret_value; + } + } + + info_->response_reader_->return_loan(data_seq, info_seq); + } + + return ret_value; + } + // Provide handlers to perform an action when a // new event from this listener has ocurred void @@ -131,12 +159,13 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener if (callback) { - auto unread_count = info_->response_reader_->get_unread_count(); - // Push events arrived before setting the the executor callback - if (unread_count) + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) { - callback(user_data, unread_count); + callback(user_data_, unread_responses); } + user_data_ = user_data; on_new_response_cb_ = callback; 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 9805efbda..c269e4214 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 @@ -218,15 +218,40 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener } } + size_t get_unread_resquests() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read(data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + { + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) + { + ++ret_value; + } + } + + info_->request_reader_->return_loan(data_seq, info_seq); + } + + return ret_value; + } + void on_data_available( eprosima::fastdds::dds::DataReader*) final { std::unique_lock lock_mutex(on_new_request_m_); - if (on_new_request_cb_) + auto unread_requests = get_unread_resquests(); + + if (0 < unread_requests) { - on_new_request_cb_(user_data_, 1); + on_new_request_cb_(user_data_, unread_requests); } } @@ -241,11 +266,11 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener if (callback) { - auto unread_count = info_->request_reader_->get_unread_count(); - // Push events arrived before setting the the executor callback - if (unread_count) + auto unread_requests = get_unread_resquests(); + + if (0 < unread_requests) { - callback(user_data, unread_count); + callback(user_data_, unread_requests); } user_data_ = user_data; 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 de35ab472..e372294fb 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 @@ -152,6 +152,8 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds const void* user_data, rmw_event_callback_t callback); + size_t get_unread_messages(); + RMW_FASTRTPS_SHARED_CPP_PUBLIC eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index fb7c7c8de..b67982abe 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -210,11 +210,11 @@ SubListener::set_on_new_message_callback( if (callback) { - auto unread_count = subscriber_info_->data_reader_->get_unread_count(); - // Push events arrived before setting the executor's callback - if (unread_count) + auto unread_messages = get_unread_messages(); + + if (0 < unread_messages) { - callback(user_data, unread_count); + callback(new_message_user_data_, unread_messages); } new_message_user_data_ = user_data; @@ -235,6 +235,29 @@ SubListener::set_on_new_message_callback( } } +size_t SubListener::get_unread_messages() +{ + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read(data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + { + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) + { + ++ret_value; + } + } + + subscriber_info_->data_reader_->return_loan(data_seq, info_seq); + } + + return ret_value; +} + void SubListener::on_data_available( eprosima::fastdds::dds::DataReader*) @@ -243,7 +266,12 @@ SubListener::on_data_available( if (on_new_message_cb_) { - on_new_message_cb_(new_message_user_data_, 1); + auto unread_messages = get_unread_messages(); + + if (0 < unread_messages) + { + on_new_message_cb_(new_message_user_data_, unread_messages); + } } } diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 9d3dc21c4..93a2c4957 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -31,74 +31,88 @@ #include "rmw_fastrtps_shared_cpp/listener_thread.hpp" rmw_ret_t -rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * context) +rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count( + rmw_context_t* context) { - std::lock_guard guard(context->impl->mutex); - - assert(context); - assert(context->impl); - assert(0u < context->impl->count); - - if (--context->impl->count > 0) { - return RMW_RET_OK; - } - - rmw_ret_t err = RMW_RET_OK; - rmw_ret_t ret = RMW_RET_OK; - rmw_error_string_t error_string; - - auto common_context = static_cast(context->impl->common); - auto participant_info = static_cast(context->impl->participant_info); - - if (!common_context->graph_cache.remove_participant(common_context->gid)) { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't remove Participant gid from graph_cache when destroying Participant"); - } - - ret = rmw_fastrtps_shared_cpp::destroy_subscription( - context->implementation_identifier, - participant_info, - common_context->sub); - // Try to clean the other objects if the above failed. - if (RMW_RET_OK != ret) { - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_publisher( - context->implementation_identifier, - participant_info, - common_context->pub); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - // We can just return one error, log about the previous one. - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_subscription' failed\n"); - ret = err; - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_publisher' failed\n"); - ret = err; - } else if (RMW_RET_OK != ret) { - RMW_SET_ERROR_MSG(error_string.str); - } - - common_context->graph_cache.clear_on_change_callback(); - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->graph_guard_condition)) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't destroy graph_guard_condtion"); - } - - delete common_context; - context->impl->common = nullptr; - context->impl->participant_info = nullptr; - return ret; + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (--context->impl->count > 0) + { + return RMW_RET_OK; + } + + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) + { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + if (!common_context->graph_cache.remove_participant(common_context->gid)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't remove Participant gid from graph_cache when destroying Participant"); + } + + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) + { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) + { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } + else if (RMW_RET_OK != ret) + { + RMW_SET_ERROR_MSG(error_string.str); + } + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + return ret; } diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index b659dd4b3..01da1b159 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -35,146 +35,174 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -using rmw_dds_common::operator<<; +using rmw_dds_common::operator <<; static void -node_listener(rmw_context_t * context); +node_listener( + rmw_context_t* context); rmw_ret_t -rmw_fastrtps_shared_cpp::run_listener_thread(rmw_context_t * context) +rmw_fastrtps_shared_cpp::run_listener_thread( + rmw_context_t* context) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.store(true); - common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( - context->implementation_identifier); - if (common_context->listener_thread_gc) { - try { - common_context->listener_thread = std::thread(node_listener, context); - return RMW_RET_OK; - } catch (const std::exception & exc) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); - } catch (...) { - RMW_SET_ERROR_MSG("Failed to create std::thread"); + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); + if (common_context->listener_thread_gc) + { + try + { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } + catch (const std::exception& exc) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } + catch (...) + { + RMW_SET_ERROR_MSG("Failed to create std::thread"); + } + } + else + { + RMW_SET_ERROR_MSG("Failed to create guard condition"); } - } else { - RMW_SET_ERROR_MSG("Failed to create guard condition"); - } - common_context->thread_is_running.store(false); - if (common_context->listener_thread_gc) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc)) + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" - RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + } } - } - return RMW_RET_ERROR; + return RMW_RET_ERROR; } rmw_ret_t -rmw_fastrtps_shared_cpp::join_listener_thread(rmw_context_t * context) +rmw_fastrtps_shared_cpp::join_listener_thread( + rmw_context_t* context) { - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.exchange(false); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - context->implementation_identifier, common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; - } - try { - common_context->listener_thread.join(); - } catch (const std::exception & exc) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); - return RMW_RET_ERROR; - } catch (...) { - RMW_SET_ERROR_MSG("Failed to join std::thread"); - return RMW_RET_ERROR; - } - rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; - } - return RMW_RET_OK; + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) + { + return rmw_ret; + } + try + { + common_context->listener_thread.join(); + } + catch (const std::exception& exc) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } + catch (...) + { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) + { + return rmw_ret; + } + return RMW_RET_OK; } #define TERMINATE_THREAD(msg) \ - { \ - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ - ": ros discovery info listener thread will shutdown ...\n"); \ - break; \ - } + { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } void -node_listener(rmw_context_t * context) +node_listener( + rmw_context_t* context) { - assert(nullptr != context); - assert(nullptr != context->impl); - assert(nullptr != context->impl->common); - auto common_context = static_cast(context->impl->common); - while (common_context->thread_is_running.load()) { - assert(nullptr != common_context->sub); - assert(nullptr != common_context->sub->data); - void * subscriptions_buffer[] = {common_context->sub->data}; - void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; - rmw_subscriptions_t subscriptions; - rmw_guard_conditions_t guard_conditions; - subscriptions.subscriber_count = 1; - subscriptions.subscribers = subscriptions_buffer; - guard_conditions.guard_condition_count = 1; - guard_conditions.guard_conditions = guard_conditions_buffer; - // number of conditions of a subscription is 2 - rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( - context->implementation_identifier, context, 2); - if (nullptr == wait_set) { - TERMINATE_THREAD("failed to create wait set"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( - context->implementation_identifier, - &subscriptions, - &guard_conditions, - nullptr, - nullptr, - nullptr, - wait_set, - nullptr)) - { - TERMINATE_THREAD("rmw_wait failed"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( - context->implementation_identifier, wait_set)) + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { - TERMINATE_THREAD("failed to destroy wait set"); - } - if (subscriptions_buffer[0]) { - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - bool taken; - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( - context->implementation_identifier, - common_context->sub, - static_cast(&msg), - &taken, - nullptr)) - { - TERMINATE_THREAD("__rmw_take failed"); - } - if (taken) { - if (std::memcmp( - reinterpret_cast(common_context->gid.data), - reinterpret_cast(&msg.gid.data), - RMW_GID_STORAGE_SIZE) == 0) + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void* subscriptions_buffer[] = {common_context->sub->data}; + void* guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t* wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { - // ignore local messages - continue; + TERMINATE_THREAD("failed to create wait set"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( + context->implementation_identifier, + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) + { + TERMINATE_THREAD("rmw_wait failed"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + TERMINATE_THREAD("failed to destroy wait set"); + } + if (subscriptions_buffer[0]) + { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken = true; + + while (taken) + { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) + { + TERMINATE_THREAD("__rmw_take failed"); + } + if (taken) + { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); + } + } } - common_context->graph_cache.update_participant_entities(msg); - } } - } } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index d3706fe9e..b6625f1e6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -30,118 +30,124 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp -{ +namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_send_request( - const char * identifier, - const rmw_client_t * client, - const void * ros_request, - int64_t * sequence_id) + const char* identifier, + const rmw_client_t* client, + const void* ros_request, + int64_t* sequence_id) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(client->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - 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)) { - returnedValue = RMW_RET_OK; - *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | - wparams.sample_identity().sequence_number().low; - } else { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(client->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + 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)) + { + returnedValue = RMW_RET_OK; + *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | + wparams.sample_identity().sequence_number().low; + } + else + { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } rmw_ret_t __rmw_take_request( - const char * identifier, - const rmw_service_t * service, - rmw_service_info_t * request_header, - void * ros_request, - bool * taken) + const char* identifier, + const rmw_service_t* service, + rmw_service_info_t* request_header, + void* ros_request, + bool* taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - - *taken = false; - - auto info = static_cast(service->data); - assert(info); + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - CustomServiceRequest request; + *taken = false; + auto info = static_cast(service->data); + assert(info); - request.buffer_ = new eprosima::fastcdr::FastBuffer(); + CustomServiceRequest request; - if (request.buffer_ != nullptr) { - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = request.buffer_; - data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->request_reader_->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { - if (request.sample_info_.valid_data) { - request.sample_identity_ = request.sample_info_.sample_identity; - // Use response subscriber guid (on related_sample_identity) when present. - const eprosima::fastrtps::rtps::GUID_t & reader_guid = - request.sample_info_.related_sample_identity.writer_guid(); - if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown() ) { - request.sample_identity_.writer_guid() = reader_guid; - } + request.buffer_ = new eprosima::fastcdr::FastBuffer(); - // Save both guids in the clients_endpoints map - const eprosima::fastrtps::rtps::GUID_t & writer_guid = - request.sample_info_.sample_identity.writer_guid(); - info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); - - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_request, info->request_type_support_impl_)) + if (request.buffer_ != nullptr) { - // Get header - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - request.sample_identity_.writer_guid(), - request_header->request_id.writer_guid); - request_header->request_id.sequence_number = - ((int64_t)request.sample_identity_.sequence_number().high) << - 32 | request.sample_identity_.sequence_number().low; - request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); - *taken = true; - } + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->request_reader_->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) + { + if (request.sample_info_.valid_data) + { + request.sample_identity_ = request.sample_info_.sample_identity; + // Use response subscriber guid (on related_sample_identity) when present. + const eprosima::fastrtps::rtps::GUID_t& reader_guid = + request.sample_info_.related_sample_identity.writer_guid(); + if (reader_guid != eprosima::fastrtps::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 = + request.sample_info_.sample_identity.writer_guid(); + info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); + + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_request, info->request_type_support_impl_)) + { + // Get header + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + request.sample_identity_.writer_guid(), + request_header->request_id.writer_guid); + request_header->request_id.sequence_number = + ((int64_t)request.sample_identity_.sequence_number().high) << + 32 | request.sample_identity_.sequence_number().low; + request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); + *taken = true; + } + + } + } - } + delete request.buffer_; } - delete request.buffer_; - } - - return RMW_RET_OK; + return RMW_RET_OK; } + } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 23b7178cc..db7b33c79 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -55,6 +55,7 @@ __rmw_wait( // - Heap is corrupt. // In all three cases, it's better if this crashes soon enough. auto fastdds_wait_set = static_cast(wait_set->data); + bool no_has_wait = false; if (subscriptions) { @@ -62,6 +63,7 @@ __rmw_wait( { void* data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); + no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); } } @@ -72,6 +74,7 @@ __rmw_wait( { void* data = clients->clients[i]; auto custom_client_info = static_cast(data); + no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); } } @@ -82,6 +85,7 @@ __rmw_wait( { void* data = services->services[i]; auto custom_service_info = static_cast(data); + no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); } } @@ -108,9 +112,16 @@ __rmw_wait( } eprosima::fastdds::dds::ConditionSeq triggered_coditions; + Duration_t timeout{0, 0}; + if (!no_has_wait) + { + timeout = (wait_timeout) ? + Duration_t{static_cast(wait_timeout->sec), + static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; + } + ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, - (wait_timeout && (wait_timeout->sec > 0 || wait_timeout->nsec > 0)) ? - Duration_t{static_cast(wait_timeout->sec), static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite + timeout ); if (subscriptions) @@ -137,7 +148,7 @@ __rmw_wait( subscriptions->subscribers[i] = 0; } } - else + else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) { subscriptions->subscribers[i] = 0; } @@ -168,7 +179,7 @@ __rmw_wait( clients->clients[i] = 0; } } - else + else if (0 == custom_client_info->response_reader_->get_unread_count()) { clients->clients[i] = 0; } @@ -199,7 +210,7 @@ __rmw_wait( services->services[i] = 0; } } - else + else if (0 == custom_service_info->request_reader_->get_unread_count()) { services->services[i] = 0; } @@ -262,13 +273,29 @@ __rmw_wait( for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { void* data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->detach_condition(*guard_condition); - guard_condition->set_trigger_value(false); + auto condition = static_cast(data); + fastdds_wait_set->detach_condition(*condition); + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition* c) + { + return c == condition; + })) + { + if (!condition->get_trigger_value()) + { + guard_conditions->guard_conditions[i] = 0; + } + } + else + { + guard_conditions->guard_conditions[i] = 0; + } + condition->set_trigger_value(false); } } - return ReturnCode_t::RETCODE_OK == ret_code ? RMW_RET_OK : RMW_RET_TIMEOUT; + return ((no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT); } } // namespace rmw_fastrtps_shared_cpp From 9a22e9e18343754cc7e7e9d444fc22fe182bb548 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 1 Jul 2022 07:53:52 +0200 Subject: [PATCH 08/34] Fixing uncrustify MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../src/init_rmw_context_impl.cpp | 308 ++++--- rmw_fastrtps_cpp/src/publisher.cpp | 3 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 6 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 6 +- rmw_fastrtps_cpp/src/subscription.cpp | 580 +++++++------ .../src/init_rmw_context_impl.cpp | 308 ++++--- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 3 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 6 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 6 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 608 +++++++------- .../custom_client_info.hpp | 287 +++---- .../custom_event_info.hpp | 46 +- .../custom_publisher_info.hpp | 185 ++--- .../custom_service_info.hpp | 391 +++++---- .../custom_subscriber_info.hpp | 261 +++--- .../src/custom_publisher_info.cpp | 318 ++++---- .../src/custom_subscriber_info.cpp | 512 ++++++------ .../src/init_rmw_context_impl.cpp | 154 ++-- .../src/listener_thread.cpp | 261 +++--- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 207 +++-- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 197 ++--- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 194 +++-- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 241 +++--- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 203 +++-- .../src/rmw_subscription.cpp | 383 +++++---- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 771 +++++++++--------- .../src/rmw_trigger_guard_condition.cpp | 3 +- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 448 +++++----- rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp | 3 +- .../src/types/event_types.hpp | 2 +- rmw_fastrtps_shared_cpp/src/utils.cpp | 2 +- 31 files changed, 3344 insertions(+), 3559 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index b5310cc6c..7a942a196 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -43,177 +43,167 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t init_context_impl( - rmw_context_t* context) + rmw_context_t * context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo* participant_info) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t* pub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) - { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t* sub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t * sub) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t* p) - { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { - return ret; - } - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() - { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t rmw_fastrtps_cpp::increment_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - assert(context); - assert(context->impl); + assert(context); + assert(context->impl); - std::lock_guard guard(context->impl->mutex); + std::lock_guard guard(context->impl->mutex); - if (!context->impl->count) - { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) - { - return ret; - } + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; } - context->impl->count++; - return RMW_RET_OK; + } + context->impl->count++; + return RMW_RET_OK; } diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 1880117c7..57ab05787 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -281,7 +281,8 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->data_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 1973ff126..e770c9f05 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -337,7 +337,8 @@ rmw_create_client( return nullptr; } - info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->response_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -390,7 +391,8 @@ rmw_create_client( return nullptr; } - info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->request_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 263d68bff..4fe87afa8 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -336,7 +336,8 @@ rmw_create_service( return nullptr; } - info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->request_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -393,7 +394,8 @@ rmw_create_service( return nullptr; } - info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->response_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index ff3d7814b..9ac46c332 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -55,319 +55,299 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_cpp { +namespace rmw_fastrtps_cpp +{ -rmw_subscription_t* +rmw_subscription_t * create_subscription( - const CustomParticipantInfo* participant_info, - const rosidl_message_type_support_t* type_supports, - const char* topic_name, - const rmw_qos_profile_t* qos_policies, - const rmw_subscription_options_t* subscription_options, - bool keyed) + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed) { - ///// - // Check input parameters - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) - { - RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); - return nullptr; - } - RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); - if (!qos_policies->avoid_ros_namespace_conventions) - { - int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); - if (RMW_RET_OK != ret) - { - return nullptr; - } - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; - } - } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) - { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) - { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) - { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; - } - } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called for existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; - } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() - { - delete info->listener_; - if (info->type_support_) - { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = callbacks; - - ///// - // Create the Type Support struct - if (!fastdds_type) - { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!tsupport) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); - return nullptr; - } - - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) - { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(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; + ///// + // Check input parameters + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; } - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) - { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; } - - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - info->dds_participant_ = dds_participant; - info->subscriber_ = subscriber; - info->topic_name_mangled_ = topic_name_mangled; - info->topic_ = topic.desc; - des_topic = topic.desc; - - // Create ContentFilteredTopic - if (subscription_options->content_filter_options) + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called for existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { - rmw_subscription_content_filter_options_t* options = - subscription_options->content_filter_options; - if (nullptr != options->filter_expression) - { - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, des_topic, - topic_name_mangled, options, &filtered_topic)) - { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); - return nullptr; - } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; - } + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = callbacks; + + ///// + // Create the Type Support struct + if (!fastdds_type) { + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; } - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) - { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) - { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->datareader_qos_ = reader_qos; - - // create_datareader - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) - { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(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; + } + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + info->dds_participant_ = dds_participant; + info->subscriber_ = subscriber; + info->topic_name_mangled_ = topic_name_mangled; + info->topic_ = topic.desc; + des_topic = topic.desc; + + // Create ContentFilteredTopic + if (subscription_options->content_filter_options) { + rmw_subscription_content_filter_options_t * options = + subscription_options->content_filter_options; + if (nullptr != options->filter_expression) { + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, des_topic, + topic_name_mangled, options, &filtered_topic)) + { + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); return nullptr; + } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - ///// - // Allocate subscription - rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) + } + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->datareader_qos_ = reader_qos; + + // create_datareader + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return nullptr; + } + + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_subscription = rcpputils::make_scope_exit( - [rmw_subscription]() - { - rmw_free(const_cast(rmw_subscription->topic_name)); - rmw_subscription_free(rmw_subscription); - }); - - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - - rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); - if (!rmw_subscription->topic_name) + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + ///// + // Allocate subscription + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { - RMW_SET_ERROR_MSG( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - cleanup_info.cancel(); - - TRACEPOINT( - rmw_subscription_init, - static_cast(rmw_subscription), - info->subscription_gid_.data); - return rmw_subscription; + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + + rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + cleanup_info.cancel(); + + TRACEPOINT( + rmw_subscription_init, + static_cast(rmw_subscription), + info->subscription_gid_.data); + return rmw_subscription; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 1e37ff6b8..5866e05de 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -43,177 +43,167 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t init_context_impl( - rmw_context_t* context) + rmw_context_t * context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo* participant_info) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_dynamic_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_dynamic_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t* pub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) - { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_dynamic_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t* sub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t * sub) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t* p) - { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { - return ret; - } - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() - { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - assert(context); - assert(context->impl); + assert(context); + assert(context->impl); - std::lock_guard guard(context->impl->mutex); + std::lock_guard guard(context->impl->mutex); - if (!context->impl->count) - { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) - { - return ret; - } + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; } - context->impl->count++; - return RMW_RET_OK; + } + context->impl->count++; + return RMW_RET_OK; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 9fa07547f..83c656404 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -286,7 +286,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->data_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 956450843..2ad73fe71 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -368,7 +368,8 @@ rmw_create_client( return nullptr; } - info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->response_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -421,7 +422,8 @@ rmw_create_client( return nullptr; } - info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->request_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 49985c55b..91fdeab6e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -367,7 +367,8 @@ rmw_create_service( return nullptr; } - info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->request_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -424,7 +425,8 @@ rmw_create_service( return nullptr; } - info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->response_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 8d54cdf61..447d3f51c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -55,335 +55,315 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_dynamic_cpp { +namespace rmw_fastrtps_dynamic_cpp +{ -rmw_subscription_t* +rmw_subscription_t * create_subscription( - const CustomParticipantInfo* participant_info, - const rosidl_message_type_support_t* type_supports, - const char* topic_name, - const rmw_qos_profile_t* qos_policies, - const rmw_subscription_options_t* subscription_options, - bool keyed) + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - ///// - // Check input parameters - RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) - { - RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); - return nullptr; - } - RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); - if (!qos_policies->avoid_ros_namespace_conventions) - { - int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); - if (RMW_RET_OK != ret) - { - return nullptr; - } - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; - } + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + ///// + // Check input parameters + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) - { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) - { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) - { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; - } + 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( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - std::string type_name = _create_type_name( - type_support->data, type_support->typesupport_identifier); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription* des_topic; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called with existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() - { - delete info->listener_; - if (info->type_support_) - { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - ///// - // Create the Type Support struct - TypeSupportRegistry& type_registry = TypeSupportRegistry::get_instance(); - auto type_support_impl = type_registry.get_message_type_support(type_support); - if (!type_support_impl) + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + ///// + // Create the Type Support struct + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_support_impl = type_registry.get_message_type_support(type_support); + if (!type_support_impl) { + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); + return nullptr; + } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { - RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); - return nullptr; - } - auto return_type_support = rcpputils::make_scope_exit( - [&type_registry, type_support]() - { - type_registry.return_message_type_support(type_support); - }); + type_registry.return_message_type_support(type_support); + }); - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support_impl; + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support_impl; - if (!fastdds_type) - { - auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); - if (!tsupport) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); - return nullptr; - } - - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) - { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to register type"); - return nullptr; - } - - info->type_support_ = fastdds_type; - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info); - - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) - { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); + return nullptr; } - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - des_topic = topic.desc; - - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) - { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) - { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - return nullptr; - } - - eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; - switch (subscription_options->require_unique_network_flow_endpoints){ - default: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: - // Unique network flow endpoints not required. We leave the decission to the XML profile. - break; - - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: - // Ensure we request unique network flow endpoints - if (nullptr == - PropertyPolicyHelper::find_property( - reader_qos.properties(), - "fastdds.unique_network_flows")) - { - reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); - } - break; - } - - // Creates DataReader (with subscriber name to not change name policy) + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + des_topic = topic.desc; + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + return nullptr; + } + + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints) { + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->data_reader_ = subscriber->create_datareader( + des_topic, + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { info->data_reader_ = subscriber->create_datareader( - des_topic, - reader_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - if (!info->data_reader_ && - (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == - subscription_options->require_unique_network_flow_endpoints)) + des_topic, + original_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + } + + if (!info->data_reader_) { + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); + return nullptr; + } + + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - info->data_reader_ = subscriber->create_datareader( - des_topic, - original_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - } - - if (!info->data_reader_) + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { - RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); - return nullptr; - } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_subscription = rcpputils::make_scope_exit( - [rmw_subscription]() - { - rmw_free(const_cast(rmw_subscription->topic_name)); - rmw_subscription_free(rmw_subscription); - }); - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - if (!rmw_subscription->topic_name) - { - RMW_SET_ERROR_MSG( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed - rmw_subscription->is_cft_enabled = false; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - return_type_support.cancel(); - cleanup_info.cancel(); - return rmw_subscription; + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed + rmw_subscription->is_cft_enabled = false; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + return_type_support.cancel(); + cleanup_info.cancel(); + return rmw_subscription; } } // namespace rmw_fastrtps_dynamic_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 9a75b9149..d5a38b470 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 @@ -48,192 +48,173 @@ class ClientPubListener; typedef struct CustomClientInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void* request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void* response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader* response_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter* request_writer_{nullptr}; - - std::string request_topic_; - std::string response_topic_; - - ClientListener* listener_{nullptr}; - eprosima::fastrtps::rtps::GUID_t writer_guid_; - eprosima::fastrtps::rtps::GUID_t reader_guid_; - - const char* typesupport_identifier_{nullptr}; - ClientPubListener* pub_listener_{nullptr}; - std::atomic_size_t response_subscriber_matched_count_; - std::atomic_size_t request_publisher_matched_count_; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * response_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * request_writer_{nullptr}; + + std::string request_topic_; + std::string response_topic_; + + ClientListener * listener_{nullptr}; + eprosima::fastrtps::rtps::GUID_t writer_guid_; + eprosima::fastrtps::rtps::GUID_t reader_guid_; + + const char * typesupport_identifier_{nullptr}; + ClientPubListener * pub_listener_{nullptr}; + std::atomic_size_t response_subscriber_matched_count_; + std::atomic_size_t request_publisher_matched_count_; } CustomClientInfo; typedef struct CustomClientResponse { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - std::unique_ptr buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + std::unique_ptr buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: - - explicit ClientListener( - CustomClientInfo* info) - : info_(info) - { + explicit ClientListener( + CustomClientInfo * info) + : info_(info) + { + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader *) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (on_new_response_cb_) { + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) { + on_new_response_cb_(user_data_, unread_responses); + } } - - void - on_data_available( - eprosima::fastdds::dds::DataReader*) - { - std::unique_lock lock_mutex(on_new_response_m_); - - if (on_new_response_cb_) - { - auto unread_responses = get_unread_responses(); - - if (0 < unread_responses) - { - on_new_response_cb_(user_data_, unread_responses); - } - } + } + + void on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info_ == nullptr) { + return; } - - void on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - if (info_ == nullptr) - { - return; - } - if (info.current_count_change == 1) - { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else if (info.current_count_change == -1) - { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else - { - return; - } - info_->response_subscriber_matched_count_.store(publishers_.size()); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else { + return; } - - size_t get_unread_responses() + info_->response_subscriber_matched_count_.store(publishers_.size()); + } + + size_t get_unread_responses() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - info_->response_reader_->return_loan(data_seq, info_seq); + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; + count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) { + ++ret_value; } + } - return ret_value; + info_->response_reader_->return_loan(data_seq, info_seq); } - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_response_callback( - const void* user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_response_m_); - - if (callback) - { - auto unread_responses = get_unread_responses(); - - if (0 < unread_responses) - { - callback(user_data_, unread_responses); - } - - user_data_ = user_data; - on_new_response_cb_ = callback; - - eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - - user_data_ = nullptr; - on_new_response_cb_ = nullptr; - } + return ret_value; + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_response_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (callback) { + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) { + callback(user_data_, unread_responses); + } + + user_data_ = user_data; + on_new_response_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_response_cb_ = nullptr; } + } private: + CustomClientInfo * info_; - CustomClientInfo* info_; - - std::set publishers_; + std::set publishers_; - rmw_event_callback_t on_new_response_cb_{nullptr}; + rmw_event_callback_t on_new_response_cb_{nullptr}; - const void* user_data_{nullptr}; + const void * user_data_{nullptr}; - std::mutex on_new_response_m_; + std::mutex on_new_response_m_; }; class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { public: - - explicit ClientPubListener( - CustomClientInfo* info) - : info_(info) - { + explicit ClientPubListener( + CustomClientInfo * info) + : info_(info) + { + } + + void on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + if (info_ == nullptr) { + return; } - - void on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - if (info_ == nullptr) - { - return; - } - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else - { - return; - } - info_->request_publisher_matched_count_.store(subscriptions_.size()); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else { + return; } + info_->request_publisher_matched_count_.store(subscriptions_.size()); + } private: - - CustomClientInfo* info_; - std::set subscriptions_; + CustomClientInfo * info_; + 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_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index 45d631a69..e6622bbd6 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -31,42 +31,40 @@ class EventListenerInterface { public: + virtual eprosima::fastdds::dds::StatusCondition & get_statuscondition() const = 0; - virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; + /// Take ready data for an event type. + /** + * \param event_type The event type to get data for. + * \param event_info A preallocated event information (from rmw/types.h) to fill with data + * \return `true` if data was successfully taken. + * \return `false` if data was not available, in this case nothing was written to event_info. + */ + virtual bool take_event( + rmw_event_type_t event_type, + void * event_info) = 0; - /// Take ready data for an event type. - /** - * \param event_type The event type to get data for. - * \param event_info A preallocated event information (from rmw/types.h) to fill with data - * \return `true` if data was successfully taken. - * \return `false` if data was not available, in this case nothing was written to event_info. - */ - virtual bool take_event( - rmw_event_type_t event_type, - void* event_info) = 0; + // Provide handlers to perform an action when a + // new event from this listener has ocurred + virtual void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) = 0; - // Provide handlers to perform an action when a - // new event from this listener has ocurred - virtual void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) = 0; - - eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; + eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; protected: + rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; - rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; - - const void* user_data_[RMW_EVENT_INVALID] = {nullptr}; + const void * user_data_[RMW_EVENT_INVALID] = {nullptr}; - std::mutex on_new_event_m_; + std::mutex on_new_event_m_; }; struct CustomEventInfo { - virtual EventListenerInterface* get_listener() const = 0; + virtual EventListenerInterface * get_listener() const = 0; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ 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 bb344d733..3e7282fa1 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 @@ -40,114 +40,109 @@ class PubListener; typedef struct CustomPublisherInfo : public CustomEventInfo { - virtual ~CustomPublisherInfo() = default; - - eprosima::fastdds::dds::DataWriter* data_writer_{nullptr}; - PubListener* listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void* type_support_impl_{nullptr}; - rmw_gid_t publisher_gid{}; - const char* typesupport_identifier_{nullptr}; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface* - get_listener() const final; + virtual ~CustomPublisherInfo() = default; + + eprosima::fastdds::dds::DataWriter * data_writer_{nullptr}; + PubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t publisher_gid{}; + const char * typesupport_identifier_{nullptr}; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + get_listener() const final; } CustomPublisherInfo; class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener { public: - - explicit PubListener( - CustomPublisherInfo* info) - : publisher_info_(info) - , deadline_changes_(false) - , liveliness_changes_(false) - , incompatible_qos_changes_(false) - { - } - - // DataWriterListener implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - std::lock_guard lock(discovery_m_); - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter* writer, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_lost( - eprosima::fastdds::dds::DataWriter* writer, - const eprosima::fastdds::dds::LivelinessLostStatus& status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter*, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus&) final; - - // PubListener API - size_t subscriptionCount() - { - std::lock_guard lock(discovery_m_); - return subscriptions_.size(); + explicit PubListener( + CustomPublisherInfo * info) + : publisher_info_(info) + , deadline_changes_(false) + , liveliness_changes_(false) + , incompatible_qos_changes_(false) + { + } + + // DataWriterListener implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(discovery_m_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event( - rmw_event_type_t event_type, - void* event_info) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) final; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_deadline_missed( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_lost( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::LivelinessLostStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter *, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus &) final; + + // PubListener API + size_t subscriptionCount() + { + std::lock_guard lock(discovery_m_); + return subscriptions_.size(); + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition & get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void * event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) final; private: + CustomPublisherInfo * publisher_info_ = nullptr; - CustomPublisherInfo* publisher_info_ = nullptr; - - std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + std::set subscriptions_ + RCPPUTILS_TSA_GUARDED_BY( + discovery_m_); - std::mutex discovery_m_; + std::mutex discovery_m_; - bool deadline_changes_; - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool deadline_changes_; + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool incompatible_qos_changes_; - eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool incompatible_qos_changes_; + eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index c269e4214..9093f7ec1 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 @@ -48,258 +48,241 @@ class ServicePubListener; enum class client_present_t { - FAILURE, // an error occurred when checking - MAYBE, // reader not matched, writer still present - YES, // reader matched - GONE // neither reader nor writer + FAILURE, // an error occurred when checking + MAYBE, // reader not matched, writer still present + YES, // reader matched + GONE // neither reader nor writer }; typedef struct CustomServiceInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void* request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void* response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader* request_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter* response_writer_{nullptr}; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; - ServiceListener* listener_{nullptr}; - ServicePubListener* pub_listener_{nullptr}; + ServiceListener * listener_{nullptr}; + ServicePubListener * pub_listener_{nullptr}; - const char* typesupport_identifier_{nullptr}; + const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; typedef struct CustomServiceRequest { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - eprosima::fastcdr::FastBuffer* buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; - - CustomServiceRequest() - : buffer_(nullptr) - { - } - + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastcdr::FastBuffer * buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; + + CustomServiceRequest() + : buffer_(nullptr) + { + } } CustomServiceRequest; class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { - using subscriptions_set_t = - std::unordered_set; - using clients_endpoints_map_t = - std::unordered_map; + using subscriptions_set_t = + std::unordered_set; + using clients_endpoints_map_t = + std::unordered_map; public: - - explicit ServicePubListener( - CustomServiceInfo* info) - { - (void) info; - } - - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - std::lock_guard lock(mutex_); - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); - subscriptions_.erase(erase_endpoint_guid); - auto endpoint = clients_endpoints_.find(erase_endpoint_guid); - if (endpoint != clients_endpoints_.end()) - { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(erase_endpoint_guid); - } - } - else - { - return; - } - cv_.notify_all(); + explicit ServicePubListener( + CustomServiceInfo * info) + { + (void) info; + } + + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(mutex_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + subscriptions_.erase(erase_endpoint_guid); + auto endpoint = clients_endpoints_.find(erase_endpoint_guid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(erase_endpoint_guid); + } + } else { + return; } - - template - bool - wait_for_subscription( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::chrono::duration& rel_time) + cv_.notify_all(); + } + + template + bool + wait_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid, + const std::chrono::duration & rel_time) + { + auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool { - auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool - { - return subscriptions_.find(guid) != subscriptions_.end(); - }; + return subscriptions_.find(guid) != subscriptions_.end(); + }; - std::unique_lock lock(mutex_); - return cv_.wait_for(lock, rel_time, guid_is_present); - } + std::unique_lock lock(mutex_); + return cv_.wait_for(lock, rel_time, guid_is_present); + } - client_present_t - check_for_subscription( - const eprosima::fastrtps::rtps::GUID_t& guid) + client_present_t + check_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid) + { { - { - std::lock_guard lock(mutex_); - // Check if the guid is still in the map - if (clients_endpoints_.find(guid) == clients_endpoints_.end()) - { - // Client is gone - return client_present_t::GONE; - } - } - // Wait for subscription - if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) - { - return client_present_t::MAYBE; - } - return client_present_t::YES; + std::lock_guard lock(mutex_); + // Check if the guid is still in the map + if (clients_endpoints_.find(guid) == clients_endpoints_.end()) { + // Client is gone + return client_present_t::GONE; + } } - - void endpoint_erase_if_exists( - const eprosima::fastrtps::rtps::GUID_t& endpointGuid) - { - std::lock_guard lock(mutex_); - auto endpoint = clients_endpoints_.find(endpointGuid); - if (endpoint != clients_endpoints_.end()) - { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(endpointGuid); - } + // Wait for subscription + if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) { + return client_present_t::MAYBE; } - - void endpoint_add_reader_and_writer( - const eprosima::fastrtps::rtps::GUID_t& readerGuid, - const eprosima::fastrtps::rtps::GUID_t& writerGuid) - { - std::lock_guard lock(mutex_); - clients_endpoints_.emplace(readerGuid, writerGuid); - clients_endpoints_.emplace(writerGuid, readerGuid); + return client_present_t::YES; + } + + void endpoint_erase_if_exists( + const eprosima::fastrtps::rtps::GUID_t & endpointGuid) + { + std::lock_guard lock(mutex_); + auto endpoint = clients_endpoints_.find(endpointGuid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(endpointGuid); } + } -private: + void endpoint_add_reader_and_writer( + const eprosima::fastrtps::rtps::GUID_t & readerGuid, + const eprosima::fastrtps::rtps::GUID_t & writerGuid) + { + std::lock_guard lock(mutex_); + clients_endpoints_.emplace(readerGuid, writerGuid); + clients_endpoints_.emplace(writerGuid, readerGuid); + } - std::mutex mutex_; - subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); - clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); - std::condition_variable cv_; +private: + std::mutex mutex_; + subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + std::condition_variable cv_; }; class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: - - explicit ServiceListener( - CustomServiceInfo* info) - : info_(info) - { + explicit ServiceListener( + CustomServiceInfo * info) + : info_(info) + { + } + + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info.current_count_change == -1) { + info_->pub_listener_->endpoint_erase_if_exists( + eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } + } - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - if (info.current_count_change == -1) - { - info_->pub_listener_->endpoint_erase_if_exists( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - } + size_t get_unread_resquests() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; - size_t get_unread_resquests() + if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - info_->request_reader_->return_loan(data_seq, info_seq); + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; + count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) { + ++ret_value; } + } - return ret_value; - } - - void - on_data_available( - eprosima::fastdds::dds::DataReader*) final - { - std::unique_lock lock_mutex(on_new_request_m_); - - auto unread_requests = get_unread_resquests(); - - if (0 < unread_requests) - { - on_new_request_cb_(user_data_, unread_requests); - } + info_->request_reader_->return_loan(data_seq, info_seq); } - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_request_callback( - const void* user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_request_m_); - - if (callback) - { - auto unread_requests = get_unread_resquests(); + return ret_value; + } - if (0 < unread_requests) - { - callback(user_data_, unread_requests); - } + void + on_data_available( + eprosima::fastdds::dds::DataReader *) final + { + std::unique_lock lock_mutex(on_new_request_m_); - user_data_ = user_data; - on_new_request_cb_ = callback; + auto unread_requests = get_unread_resquests(); - eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - - user_data_ = nullptr; - on_new_request_cb_ = nullptr; - } + if (0 < unread_requests) { + on_new_request_cb_(user_data_, unread_requests); } + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_request_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_request_m_); + + if (callback) { + auto unread_requests = get_unread_resquests(); + + if (0 < unread_requests) { + callback(user_data_, unread_requests); + } + + user_data_ = user_data; + on_new_request_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_request_cb_ = nullptr; + } + } private: + CustomServiceInfo * info_; - CustomServiceInfo* info_; - - rmw_event_callback_t on_new_request_cb_{nullptr}; + rmw_event_callback_t on_new_request_cb_{nullptr}; - const void* user_data_{nullptr}; + const void * user_data_{nullptr}; - std::mutex on_new_request_m_; + std::mutex on_new_request_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ 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 e372294fb..3945c3fe2 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 @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -46,162 +47,158 @@ class SubListener; -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ struct LoanManager; } // namespace rmw_fastrtps_shared_cpp struct CustomSubscriberInfo : public CustomEventInfo { - virtual ~CustomSubscriberInfo() = default; - - eprosima::fastdds::dds::DataReader* data_reader_ {nullptr}; - SubListener* listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void* type_support_impl_{nullptr}; - rmw_gid_t subscription_gid_{}; - const char* typesupport_identifier_{nullptr}; - std::shared_ptr loan_manager_; - - // for re-create or delete content filtered topic - const rmw_node_t* node_ {nullptr}; - rmw_dds_common::Context* common_context_ {nullptr}; - eprosima::fastdds::dds::DomainParticipant* dds_participant_ {nullptr}; - eprosima::fastdds::dds::Subscriber* subscriber_ {nullptr}; - std::string topic_name_mangled_; - eprosima::fastdds::dds::TopicDescription* topic_ {nullptr}; - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic_ {nullptr}; - eprosima::fastdds::dds::DataReaderQos datareader_qos_; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface* - get_listener() const final; + virtual ~CustomSubscriberInfo() = default; + + eprosima::fastdds::dds::DataReader * data_reader_ {nullptr}; + SubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t subscription_gid_{}; + const char * typesupport_identifier_{nullptr}; + std::shared_ptr loan_manager_; + + // for re-create or delete content filtered topic + const rmw_node_t * node_ {nullptr}; + rmw_dds_common::Context * common_context_ {nullptr}; + eprosima::fastdds::dds::DomainParticipant * dds_participant_ {nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_ {nullptr}; + std::string topic_name_mangled_; + eprosima::fastdds::dds::TopicDescription * topic_ {nullptr}; + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic_ {nullptr}; + eprosima::fastdds::dds::DataReaderQos datareader_qos_; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + get_listener() const final; }; class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: - - explicit SubListener( - CustomSubscriberInfo* info) - : subscriber_info_(info) - , deadline_changes_(false) - , liveliness_changes_(false) - , sample_lost_changes_(false) - , incompatible_qos_changes_(false) - { - } - - // DataReaderListener implementation - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - { - std::lock_guard lock(discovery_m_); - if (info.current_count_change == 1) - { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else if (info.current_count_change == -1) - { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - } - } - - void - on_data_available( - eprosima::fastdds::dds::DataReader* reader) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastrtps::RequestedDeadlineMissedStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_changed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastrtps::LivelinessChangedStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_sample_lost( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SampleLostStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus&) final; - - size_t publisherCount() + explicit SubListener( + CustomSubscriberInfo * info) + : subscriber_info_(info) + , deadline_changes_(false) + , liveliness_changes_(false) + , sample_lost_changes_(false) + , incompatible_qos_changes_(false) + { + } + + // DataReaderListener implementation + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { { - std::lock_guard lock(discovery_m_); - return publishers_.size(); + std::lock_guard lock(discovery_m_); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } } - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_message_callback( - const void* user_data, - rmw_event_callback_t callback); - - size_t get_unread_messages(); - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event( - rmw_event_type_t event_type, - void* event_info) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) final; + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader * reader) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::RequestedDeadlineMissedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_changed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::LivelinessChangedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_sample_lost( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SampleLostStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus &) final; + + size_t publisherCount() + { + std::lock_guard lock(discovery_m_); + return publishers_.size(); + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_message_callback( + const void * user_data, + rmw_event_callback_t callback); + + size_t get_unread_messages(); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition & get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void * event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) final; private: + CustomSubscriberInfo * subscriber_info_ = nullptr; - CustomSubscriberInfo* subscriber_info_ = nullptr; - - bool deadline_changes_; - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool deadline_changes_; + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool sample_lost_changes_; - eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool sample_lost_changes_; + eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool incompatible_qos_changes_; - eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + bool incompatible_qos_changes_; + eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + discovery_m_); - std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - rmw_event_callback_t on_new_message_cb_{nullptr}; + rmw_event_callback_t on_new_message_cb_{nullptr}; - const void* new_message_user_data_{nullptr}; + const void * new_message_user_data_{nullptr}; - std::mutex on_new_message_m_; + std::mutex on_new_message_m_; - std::mutex discovery_m_; + std::mutex discovery_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 997314de7..3801804b9 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -20,207 +20,203 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -EventListenerInterface* +EventListenerInterface * CustomPublisherInfo::get_listener() const { - return listener_; + return listener_; } -eprosima::fastdds::dds::StatusCondition& PubListener::get_statuscondition() const +eprosima::fastdds::dds::StatusCondition & PubListener::get_statuscondition() const { - return publisher_info_->data_writer_->get_statuscondition(); + return publisher_info_->data_writer_->get_statuscondition(); } bool PubListener::take_event( - rmw_event_type_t event_type, - void* event_info) + rmw_event_type_t event_type, + void * event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - - std::unique_lock lock_mutex(on_new_event_m_); - - switch (event_type){ - case RMW_EVENT_LIVELINESS_LOST: - { - auto rmw_data = static_cast(event_info); - if (liveliness_changes_) - { - rmw_data->total_count = liveliness_lost_status_.total_count; - rmw_data->total_count_change = liveliness_lost_status_.total_count_change; - liveliness_changes_ = false; - } - else - { - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; - publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status); - rmw_data->total_count = liveliness_lost_status.total_count; - rmw_data->total_count_change = liveliness_lost_status.total_count_change; - } - liveliness_lost_status_.total_count_change = 0; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_) { + rmw_data->total_count = liveliness_lost_status_.total_count; + rmw_data->total_count_change = liveliness_lost_status_.total_count_change; + liveliness_changes_ = false; + } else { + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status); + rmw_data->total_count = liveliness_lost_status.total_count; + rmw_data->total_count_change = liveliness_lost_status.total_count_change; } - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - if (deadline_changes_) - { - rmw_data->total_count = offered_deadline_missed_status_.total_count; - rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; - deadline_changes_ = false; - } - else - { - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; - publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status); - rmw_data->total_count = offered_deadline_missed_status.total_count; - rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; - } - offered_deadline_missed_status_.total_count_change = 0; + liveliness_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_) { + rmw_data->total_count = offered_deadline_missed_status_.total_count; + rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; + deadline_changes_ = false; + } else { + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; + publisher_info_->data_writer_->get_offered_deadline_missed_status( + offered_deadline_missed_status); + rmw_data->total_count = offered_deadline_missed_status.total_count; + rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; } - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_) - { - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_ = false; - } - else - { - eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; - publisher_info_->data_writer_->get_offered_incompatible_qos_status(offered_incompatible_qos_status); - rmw_data->total_count = offered_incompatible_qos_status.total_count; - rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - offered_incompatible_qos_status.last_policy_id); - } - incompatible_qos_status_.total_count_change = 0; + offered_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_) { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_ = false; + } else { + eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; + publisher_info_->data_writer_->get_offered_incompatible_qos_status( + offered_incompatible_qos_status); + rmw_data->total_count = offered_incompatible_qos_status.total_count; + rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + offered_incompatible_qos_status.last_policy_id); } - break; - default: - return false; - } - event_guard[event_type].set_trigger_value(false); - return true; + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + event_guard[event_type].set_trigger_value(false); + return true; } void PubListener::set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) { - std::unique_lock lock_mutex(on_new_event_m_); - - if (callback) - { - switch (event_type) - { - case RMW_EVENT_LIVELINESS_LOST: - publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status_); - callback(user_data, liveliness_lost_status_.total_count_change); - liveliness_lost_status_.total_count_change = 0; - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status_); - callback(user_data, - offered_deadline_missed_status_.total_count_change); - offered_deadline_missed_status_.total_count_change = 0; - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - publisher_info_->data_writer_->get_offered_incompatible_qos_status(incompatible_qos_status_); - callback(user_data, - incompatible_qos_status_.total_count_change); - incompatible_qos_status_.total_count_change = 0; - break; - default: - break; - } - - user_data_[event_type] = user_data; - on_new_event_cb_[event_type] = callback; - - eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - - user_data_[event_type] = nullptr; - on_new_event_cb_[event_type] = nullptr; + std::unique_lock lock_mutex(on_new_event_m_); + + if (callback) { + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status_); + callback(user_data, liveliness_lost_status_.total_count_change); + liveliness_lost_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + publisher_info_->data_writer_->get_offered_deadline_missed_status( + offered_deadline_missed_status_); + callback( + user_data, + offered_deadline_missed_status_.total_count_change); + offered_deadline_missed_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + publisher_info_->data_writer_->get_offered_incompatible_qos_status( + incompatible_qos_status_); + callback( + user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; + break; + default: + break; } + + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener( + this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener( + this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } void PubListener::on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - offered_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - offered_deadline_missed_status_.total_count_change += status.total_count_change; + // Assign absolute values + offered_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + offered_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_ = true; + deadline_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) - { - on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED], 1); - } + if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) { + on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED + ], 1); + } - event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); + event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); } void PubListener::on_liveliness_lost( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::LivelinessLostStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::LivelinessLostStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - liveliness_lost_status_.total_count = status.total_count; - // Accumulate deltas - liveliness_lost_status_.total_count_change += status.total_count_change; + // Assign absolute values + liveliness_lost_status_.total_count = status.total_count; + // Accumulate deltas + liveliness_lost_status_.total_count_change += status.total_count_change; - liveliness_changes_ = true; + liveliness_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) - { - on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); - } + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) { + on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); + } - event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); + event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); } void PubListener::on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_ = true; + incompatible_qos_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) - { - on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); - } + if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) { + on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[ + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); + } - event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); + event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index b67982abe..fa1b15acc 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -20,345 +20,339 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -EventListenerInterface* +EventListenerInterface * CustomSubscriberInfo::get_listener() const { - return listener_; + return listener_; } -eprosima::fastdds::dds::StatusCondition& SubListener::get_statuscondition() const +eprosima::fastdds::dds::StatusCondition & SubListener::get_statuscondition() const { - return subscriber_info_->data_reader_->get_statuscondition(); + return subscriber_info_->data_reader_->get_statuscondition(); } bool SubListener::take_event( - rmw_event_type_t event_type, - void* event_info) + rmw_event_type_t event_type, + void * event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_) { + rmw_data->alive_count = liveliness_changed_status_.alive_count; + rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; + liveliness_changes_ = false; + } else { + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status); + rmw_data->alive_count = liveliness_changed_status.alive_count; + rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; + } + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_) { + rmw_data->total_count = requested_deadline_missed_status_.total_count; + rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; + deadline_changes_ = false; + } else { + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; + subscriber_info_->data_reader_->get_requested_deadline_missed_status( + requested_deadline_missed_status); + rmw_data->total_count = requested_deadline_missed_status.total_count; + rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; + } + requested_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + auto rmw_data = static_cast(event_info); + if (sample_lost_changes_) { + rmw_data->total_count = sample_lost_status_.total_count; + rmw_data->total_count_change = sample_lost_status_.total_count_change; + sample_lost_changes_ = false; + } else { + eprosima::fastdds::dds::SampleLostStatus sample_lost_status; + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status); + rmw_data->total_count = sample_lost_status.total_count; + rmw_data->total_count_change = sample_lost_status.total_count_change; + } + sample_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_) { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_ = false; + } else { + eprosima::fastdds::dds::RequestedIncompatibleQosStatus + requested_qos_incompatible_qos_status; + subscriber_info_->data_reader_->get_requested_incompatible_qos_status( + requested_qos_incompatible_qos_status); + rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; + rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + requested_qos_incompatible_qos_status.last_policy_id); + } + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + + event_guard[event_type].set_trigger_value(false); + return true; +} - std::unique_lock lock_mutex(on_new_event_m_); +void SubListener::set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_event_m_); - switch (event_type){ - case RMW_EVENT_LIVELINESS_CHANGED: + if (callback) { + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: { - auto rmw_data = static_cast(event_info); - if (liveliness_changes_) - { - rmw_data->alive_count = liveliness_changed_status_.alive_count; - rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; - liveliness_changes_ = false; - } - else - { - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; - subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status); - rmw_data->alive_count = liveliness_changed_status.alive_count; - rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; - } - liveliness_changed_status_.alive_count_change = 0; - liveliness_changed_status_.not_alive_count_change = 0; + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status_); + callback( + user_data, liveliness_changed_status_.alive_count_change + + liveliness_changed_status_.not_alive_count_change); + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; } break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: { - auto rmw_data = static_cast(event_info); - if (deadline_changes_) - { - rmw_data->total_count = requested_deadline_missed_status_.total_count; - rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; - deadline_changes_ = false; - } - else - { - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; - subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status); - rmw_data->total_count = requested_deadline_missed_status.total_count; - rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; - } - requested_deadline_missed_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_requested_deadline_missed_status( + requested_deadline_missed_status_); + callback( + user_data, + requested_deadline_missed_status_.total_count_change); + requested_deadline_missed_status_.total_count_change = 0; } break; - case RMW_EVENT_MESSAGE_LOST: + case RMW_EVENT_MESSAGE_LOST: { - auto rmw_data = static_cast(event_info); - if (sample_lost_changes_) - { - rmw_data->total_count = sample_lost_status_.total_count; - rmw_data->total_count_change = sample_lost_status_.total_count_change; - sample_lost_changes_ = false; - } - else - { - eprosima::fastdds::dds::SampleLostStatus sample_lost_status; - subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status); - rmw_data->total_count = sample_lost_status.total_count; - rmw_data->total_count_change = sample_lost_status.total_count_change; - } - sample_lost_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status_); + callback( + user_data, + sample_lost_status_.total_count_change); + sample_lost_status_.total_count_change = 0; } break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: { - auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_) - { - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_ = false; - } - else - { - eprosima::fastdds::dds::RequestedIncompatibleQosStatus requested_qos_incompatible_qos_status; - subscriber_info_->data_reader_->get_requested_incompatible_qos_status( - requested_qos_incompatible_qos_status); - rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; - rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - requested_qos_incompatible_qos_status.last_policy_id); - } - incompatible_qos_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_requested_incompatible_qos_status( + incompatible_qos_status_); + callback( + user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; } break; - default: - return false; - } - - event_guard[event_type].set_trigger_value(false); - return true; -} - -void SubListener::set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) -{ - std::unique_lock lock_mutex(on_new_event_m_); - - if (callback) - { - switch (event_type){ - case RMW_EVENT_LIVELINESS_CHANGED: - { - subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status_); - callback(user_data, liveliness_changed_status_.alive_count_change + - liveliness_changed_status_.not_alive_count_change); - liveliness_changed_status_.alive_count_change = 0; - liveliness_changed_status_.not_alive_count_change = 0; - } - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - { - subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status_); - callback(user_data, - requested_deadline_missed_status_.total_count_change); - requested_deadline_missed_status_.total_count_change = 0; - } - break; - case RMW_EVENT_MESSAGE_LOST: - { - subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status_); - callback(user_data, - sample_lost_status_.total_count_change); - sample_lost_status_.total_count_change = 0; - } - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - subscriber_info_->data_reader_->get_requested_incompatible_qos_status(incompatible_qos_status_); - callback(user_data, - incompatible_qos_status_.total_count_change); - incompatible_qos_status_.total_count_change = 0; - } - break; - default: - break; - } - - user_data_[event_type] = user_data; - on_new_event_cb_[event_type] = callback; - - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + default: + break; } - else - { - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - user_data_[event_type] = nullptr; - on_new_event_cb_[event_type] = nullptr; - } + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } void SubListener::set_on_new_message_callback( - const void* user_data, - rmw_event_callback_t callback) + const void * user_data, + rmw_event_callback_t callback) { - std::unique_lock lock_mutex(on_new_message_m_); - - if (callback) - { - auto unread_messages = get_unread_messages(); - - if (0 < unread_messages) - { - callback(new_message_user_data_, unread_messages); - } + std::unique_lock lock_mutex(on_new_message_m_); - new_message_user_data_ = user_data; - on_new_message_cb_ = callback; + if (callback) { + auto unread_messages = get_unread_messages(); - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + if (0 < unread_messages) { + callback(new_message_user_data_, unread_messages); } - else - { - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - new_message_user_data_ = nullptr; - on_new_message_cb_ = nullptr; - } + new_message_user_data_ = user_data; + on_new_message_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + new_message_user_data_ = nullptr; + on_new_message_cb_ = nullptr; + } } size_t SubListener::get_unread_messages() { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + { + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); + ++count) { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - subscriber_info_->data_reader_->return_loan(data_seq, info_seq); + if (info_seq[count].valid_data) { + ++ret_value; + } } - return ret_value; + subscriber_info_->data_reader_->return_loan(data_seq, info_seq); + } + + return ret_value; } void SubListener::on_data_available( - eprosima::fastdds::dds::DataReader*) + eprosima::fastdds::dds::DataReader *) { - std::unique_lock lock_mutex(on_new_message_m_); + std::unique_lock lock_mutex(on_new_message_m_); - if (on_new_message_cb_) - { - auto unread_messages = get_unread_messages(); + if (on_new_message_cb_) { + auto unread_messages = get_unread_messages(); - if (0 < unread_messages) - { - on_new_message_cb_(new_message_user_data_, unread_messages); - } + if (0 < unread_messages) { + on_new_message_cb_(new_message_user_data_, unread_messages); } + } } void SubListener::on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedDeadlineMissedStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - requested_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - requested_deadline_missed_status_.total_count_change += status.total_count_change; + // Assign absolute values + requested_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + requested_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_ = true; + deadline_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) - { - on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); - } + if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) { + on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[ + RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); + } - event_guard[RMW_EVENT_REQUESTED_DEADLINE_MISSED].set_trigger_value(true); + event_guard[RMW_EVENT_REQUESTED_DEADLINE_MISSED].set_trigger_value(true); } void SubListener::on_liveliness_changed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::LivelinessChangedStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::LivelinessChangedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - liveliness_changed_status_.alive_count = status.alive_count; - liveliness_changed_status_.not_alive_count = status.not_alive_count; - // Accumulate deltas - liveliness_changed_status_.alive_count_change += status.alive_count_change; - liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; + // Assign absolute values + liveliness_changed_status_.alive_count = status.alive_count; + liveliness_changed_status_.not_alive_count = status.not_alive_count; + // Accumulate deltas + liveliness_changed_status_.alive_count_change += status.alive_count_change; + liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; - liveliness_changes_ = true; + liveliness_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) - { - on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); - } + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) { + on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); + } - event_guard[RMW_EVENT_LIVELINESS_CHANGED].set_trigger_value(true); + event_guard[RMW_EVENT_LIVELINESS_CHANGED].set_trigger_value(true); } void SubListener::on_sample_lost( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SampleLostStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SampleLostStatus & status) { - std::lock_guard lock_mutex(on_new_event_m_); + std::lock_guard lock_mutex(on_new_event_m_); - // Assign absolute values - sample_lost_status_.total_count = status.total_count; - // Accumulate deltas - sample_lost_status_.total_count_change += status.total_count_change; + // Assign absolute values + sample_lost_status_.total_count = status.total_count; + // Accumulate deltas + sample_lost_status_.total_count_change += status.total_count_change; - sample_lost_changes_ = true; + sample_lost_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) - { - on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); - } + if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) { + on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); + } - event_guard[RMW_EVENT_MESSAGE_LOST].set_trigger_value(true); + event_guard[RMW_EVENT_MESSAGE_LOST].set_trigger_value(true); } void SubListener::on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) { - std::lock_guard lock_mutex(on_new_event_m_); + std::lock_guard lock_mutex(on_new_event_m_); - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_ = true; + incompatible_qos_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) - { - on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); - } + if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) { + on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[ + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); + } - event_guard[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE].set_trigger_value(true); + event_guard[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 93a2c4957..917a100a4 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -32,87 +32,79 @@ rmw_ret_t rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - std::lock_guard guard(context->impl->mutex); - - assert(context); - assert(context->impl); - assert(0u < context->impl->count); - - if (--context->impl->count > 0) - { - return RMW_RET_OK; - } - - rmw_ret_t err = RMW_RET_OK; - rmw_ret_t ret = RMW_RET_OK; - rmw_error_string_t error_string; - - ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); - if (RMW_RET_OK != ret) - { - return ret; - } - - auto common_context = static_cast(context->impl->common); - auto participant_info = static_cast(context->impl->participant_info); - - if (!common_context->graph_cache.remove_participant(common_context->gid)) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't remove Participant gid from graph_cache when destroying Participant"); - } - - ret = rmw_fastrtps_shared_cpp::destroy_subscription( - context->implementation_identifier, - participant_info, - common_context->sub); - // Try to clean the other objects if the above failed. - if (RMW_RET_OK != ret) - { - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_publisher( - context->implementation_identifier, - participant_info, - common_context->pub); - if (RMW_RET_OK != ret && RMW_RET_OK != err) - { - // We can just return one error, log about the previous one. - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_subscription' failed\n"); - ret = err; - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret && RMW_RET_OK != err) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_publisher' failed\n"); - ret = err; - } - else if (RMW_RET_OK != ret) - { - RMW_SET_ERROR_MSG(error_string.str); - } - - common_context->graph_cache.clear_on_change_callback(); - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->graph_guard_condition)) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't destroy graph_guard_condtion"); - } - - delete common_context; - context->impl->common = nullptr; - context->impl->participant_info = nullptr; + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (--context->impl->count > 0) { + return RMW_RET_OK; + } + + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + if (!common_context->graph_cache.remove_participant(common_context->gid)) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't remove Participant gid from graph_cache when destroying Participant"); + } + + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); + } + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + return ret; } diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index 01da1b159..2b6b6b002 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -35,174 +35,153 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -using rmw_dds_common::operator <<; +using rmw_dds_common::operator<<; static void node_listener( - rmw_context_t* context); + rmw_context_t * context); rmw_ret_t rmw_fastrtps_shared_cpp::run_listener_thread( - rmw_context_t* context) + rmw_context_t * context) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.store(true); - common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( - context->implementation_identifier); - if (common_context->listener_thread_gc) - { - try - { - common_context->listener_thread = std::thread(node_listener, context); - return RMW_RET_OK; - } - catch (const std::exception& exc) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); - } - catch (...) - { - RMW_SET_ERROR_MSG("Failed to create std::thread"); - } - } - else - { - RMW_SET_ERROR_MSG("Failed to create guard condition"); + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } catch (...) { + RMW_SET_ERROR_MSG("Failed to create std::thread"); } - common_context->thread_is_running.store(false); - if (common_context->listener_thread_gc) + } else { + RMW_SET_ERROR_MSG("Failed to create guard condition"); + } + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" - RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); - } + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); } - return RMW_RET_ERROR; + } + return RMW_RET_ERROR; } rmw_ret_t rmw_fastrtps_shared_cpp::join_listener_thread( - rmw_context_t* context) + rmw_context_t * context) { - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.exchange(false); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - context->implementation_identifier, common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) - { - return rmw_ret; - } - try - { - common_context->listener_thread.join(); - } - catch (const std::exception& exc) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); - return RMW_RET_ERROR; - } - catch (...) - { - RMW_SET_ERROR_MSG("Failed to join std::thread"); - return RMW_RET_ERROR; - } - rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) - { - return rmw_ret; - } - return RMW_RET_OK; + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + try { + common_context->listener_thread.join(); + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } catch (...) { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; } #define TERMINATE_THREAD(msg) \ - { \ - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ - ": ros discovery info listener thread will shutdown ...\n"); \ - break; \ - } + { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } void node_listener( - rmw_context_t* context) + rmw_context_t * context) { - assert(nullptr != context); - assert(nullptr != context->impl); - assert(nullptr != context->impl->common); - auto common_context = static_cast(context->impl->common); - while (common_context->thread_is_running.load()) + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void * subscriptions_buffer[] = {common_context->sub->data}; + void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { + TERMINATE_THREAD("failed to create wait set"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( + context->implementation_identifier, + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) { - assert(nullptr != common_context->sub); - assert(nullptr != common_context->sub->data); - void* subscriptions_buffer[] = {common_context->sub->data}; - void* guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; - rmw_subscriptions_t subscriptions; - rmw_guard_conditions_t guard_conditions; - subscriptions.subscriber_count = 1; - subscriptions.subscribers = subscriptions_buffer; - guard_conditions.guard_condition_count = 1; - guard_conditions.guard_conditions = guard_conditions_buffer; - // number of conditions of a subscription is 2 - rmw_wait_set_t* wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( - context->implementation_identifier, context, 2); - if (nullptr == wait_set) - { - TERMINATE_THREAD("failed to create wait set"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( - context->implementation_identifier, - &subscriptions, - &guard_conditions, - nullptr, - nullptr, - nullptr, - wait_set, - nullptr)) - { - TERMINATE_THREAD("rmw_wait failed"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( - context->implementation_identifier, wait_set)) + TERMINATE_THREAD("rmw_wait failed"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + TERMINATE_THREAD("failed to destroy wait set"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken = true; + + while (taken) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) { - TERMINATE_THREAD("failed to destroy wait set"); + TERMINATE_THREAD("__rmw_take failed"); } - if (subscriptions_buffer[0]) - { - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - bool taken = true; - - while (taken) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( - context->implementation_identifier, - common_context->sub, - static_cast(&msg), - &taken, - nullptr)) - { - TERMINATE_THREAD("__rmw_take failed"); - } - if (taken) - { - if (std::memcmp( - reinterpret_cast(common_context->gid.data), - reinterpret_cast(&msg.gid.data), - RMW_GID_STORAGE_SIZE) == 0) - { - // ignore local messages - continue; - } - common_context->graph_cache.update_participant_entities(msg); - } - } + if (taken) { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); } + } } + } } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 3269f4acb..efd071929 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -30,135 +30,132 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_client( - const char* identifier, - rmw_node_t* node, - rmw_client_t* client) + const char * identifier, + rmw_node_t * node, + rmw_client_t * client) { - rmw_ret_t final_ret = RMW_RET_OK; - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(client->data); - + rmw_ret_t final_ret = RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(client->data); + + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_writer_->guid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_reader_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_writer_->guid()); - common_context->graph_cache.dissociate_writer( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_reader_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_reader_->get_topicdescription(); + auto request_topic = info->request_writer_->get_topic(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = RMW_RET_ERROR; + info->response_reader_->set_listener(nullptr); } - auto show_previous_error = - [&final_ret]() - { - if (RMW_RET_OK != final_ret) - { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_reader_->get_topicdescription(); - auto request_topic = info->request_writer_->get_topic(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); - final_ret = RMW_RET_ERROR; - info->response_reader_->set_listener(nullptr); - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->request_writer_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); - final_ret = RMW_RET_ERROR; - info->request_writer_->set_listener(nullptr); - } - - // Delete DataWriter listener - if (nullptr != info->pub_listener_) - { - delete info->pub_listener_; - } - - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - - // Delete CustomClientInfo structure - delete info; + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = RMW_RET_ERROR; + info->request_writer_->set_listener(nullptr); } - rmw_free(const_cast(client->service_name)); - rmw_client_free(client); + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomClientInfo structure + delete info; + } + + rmw_free(const_cast(client->service_name)); + rmw_client_free(client); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_client_request_publisher_get_actual_qos( - const rmw_client_t* client, - rmw_qos_profile_t* qos) + const rmw_client_t * client, + rmw_qos_profile_t * qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataWriter* fastdds_rw = cli->request_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = cli->request_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_response_subscription_get_actual_qos( - const rmw_client_t* client, - rmw_qos_profile_t* qos) + const rmw_client_t * client, + rmw_qos_profile_t * qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataReader* fastdds_dr = cli->response_reader_; - dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataReader * fastdds_dr = cli->response_reader_; + dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_set_on_new_response_callback( - rmw_client_t* rmw_client, - rmw_event_callback_t callback, - const void* user_data) + rmw_client_t * rmw_client, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_client_info = static_cast(rmw_client->data); - custom_client_info->listener_->set_on_new_response_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_client_info = static_cast(rmw_client->data); + custom_client_info->listener_->set_on_new_response_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index b5d45b5e6..413791b6c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -22,129 +22,130 @@ #include "types/event_types.hpp" static const std::unordered_set g_rmw_event_type_set{ - RMW_EVENT_LIVELINESS_CHANGED, - RMW_EVENT_REQUESTED_DEADLINE_MISSED, - RMW_EVENT_LIVELINESS_LOST, - RMW_EVENT_OFFERED_DEADLINE_MISSED, - RMW_EVENT_MESSAGE_LOST, - RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, - RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE + RMW_EVENT_LIVELINESS_CHANGED, + RMW_EVENT_REQUESTED_DEADLINE_MISSED, + RMW_EVENT_LIVELINESS_LOST, + RMW_EVENT_OFFERED_DEADLINE_MISSED, + RMW_EVENT_MESSAGE_LOST, + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE }; -namespace rmw_fastrtps_shared_cpp { -namespace internal { +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( - const rmw_event_type_t event_type) + const rmw_event_type_t event_type) { - eprosima::fastdds::dds::StatusMask ret_statusmask = eprosima::fastdds::dds::StatusMask::none(); - switch (event_type) - { - case RMW_EVENT_LIVELINESS_CHANGED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_changed(); - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_deadline_missed(); - break; - case RMW_EVENT_LIVELINESS_LOST: - ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_lost(); - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_deadline_missed(); - break; - case RMW_EVENT_MESSAGE_LOST: - ret_statusmask = eprosima::fastdds::dds::StatusMask::sample_lost(); - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_incompatible_qos(); - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_incompatible_qos(); - break; - default: - break; - } - - return ret_statusmask; + eprosima::fastdds::dds::StatusMask ret_statusmask = eprosima::fastdds::dds::StatusMask::none(); + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_changed(); + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_deadline_missed(); + break; + case RMW_EVENT_LIVELINESS_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_lost(); + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_deadline_missed(); + break; + case RMW_EVENT_MESSAGE_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::sample_lost(); + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_incompatible_qos(); + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_incompatible_qos(); + break; + default: + break; + } + + return ret_statusmask; } bool is_event_supported( - rmw_event_type_t event_type) + rmw_event_type_t event_type) { - return g_rmw_event_type_set.count(event_type) == 1; + return g_rmw_event_type_set.count(event_type) == 1; } rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( - eprosima::fastdds::dds::QosPolicyId_t policy_id) + eprosima::fastdds::dds::QosPolicyId_t policy_id) { - using eprosima::fastdds::dds::QosPolicyId_t; - - switch (policy_id){ - case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_DURABILITY; - case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: - return RMW_QOS_POLICY_DEADLINE; - case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIVELINESS; - case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_RELIABILITY; - case QosPolicyId_t::HISTORY_QOS_POLICY_ID: - return RMW_QOS_POLICY_HISTORY; - case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIFESPAN; - default: - return RMW_QOS_POLICY_INVALID; - } + using eprosima::fastdds::dds::QosPolicyId_t; + + switch (policy_id) { + case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_DURABILITY; + case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: + return RMW_QOS_POLICY_DEADLINE; + case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIVELINESS; + case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_RELIABILITY; + case QosPolicyId_t::HISTORY_QOS_POLICY_ID: + return RMW_QOS_POLICY_HISTORY; + case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIFESPAN; + default: + return RMW_QOS_POLICY_INVALID; + } } } // namespace internal rmw_ret_t __rmw_init_event( - const char* identifier, - rmw_event_t* rmw_event, - const char* topic_endpoint_impl_identifier, - void* data, - rmw_event_type_t event_type) + const char * identifier, + rmw_event_t * rmw_event, + const char * topic_endpoint_impl_identifier, + void * data, + rmw_event_type_t event_type) { - RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - topic endpoint, - topic_endpoint_impl_identifier, - identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!internal::is_event_supported(event_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); - return RMW_RET_UNSUPPORTED; - } - - rmw_event->implementation_identifier = topic_endpoint_impl_identifier; - rmw_event->data = data; - rmw_event->event_type = event_type; - CustomEventInfo* event = static_cast(rmw_event->data); - eprosima::fastdds::dds::StatusMask statusmask = event->get_listener()->get_statuscondition().get_enabled_statuses(); - statusmask << internal::rmw_event_to_dds_statusmask(event_type); - event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); - - return RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + topic endpoint, + topic_endpoint_impl_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!internal::is_event_supported(event_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = topic_endpoint_impl_identifier; + rmw_event->data = data; + rmw_event->event_type = event_type; + CustomEventInfo * event = static_cast(rmw_event->data); + eprosima::fastdds::dds::StatusMask statusmask = + event->get_listener()->get_statuscondition().get_enabled_statuses(); + statusmask << internal::rmw_event_to_dds_statusmask(event_type); + event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); + + return RMW_RET_OK; } rmw_ret_t __rmw_event_set_callback( - rmw_event_t* rmw_event, - rmw_event_callback_t callback, - const void* user_data) + rmw_event_t * rmw_event, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_event_info = static_cast(rmw_event->data); - custom_event_info->get_listener()->set_on_new_event_callback( - rmw_event->event_type, - user_data, - callback); - return RMW_RET_OK; + auto custom_event_info = static_cast(rmw_event->data); + custom_event_info->get_listener()->set_on_new_event_callback( + rmw_event->event_type, + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index b6625f1e6..cb8cb02c6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -30,124 +30,120 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_send_request( - const char* identifier, - const rmw_client_t* client, - const void* ros_request, - int64_t* sequence_id) + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(client->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - 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)) - { - returnedValue = RMW_RET_OK; - *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | - wparams.sample_identity().sequence_number().low; - } - else - { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(client->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + 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)) { + returnedValue = RMW_RET_OK; + *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | + wparams.sample_identity().sequence_number().low; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } rmw_ret_t __rmw_take_request( - const char* identifier, - const rmw_service_t* service, - rmw_service_info_t* request_header, - void* ros_request, - bool* taken) + const char * identifier, + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - auto info = static_cast(service->data); - assert(info); + auto info = static_cast(service->data); + assert(info); - CustomServiceRequest request; + CustomServiceRequest request; - request.buffer_ = new eprosima::fastcdr::FastBuffer(); + request.buffer_ = new eprosima::fastcdr::FastBuffer(); - if (request.buffer_ != nullptr) + if (request.buffer_ != nullptr) { + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->request_reader_->take_next_sample( + &data, + &request.sample_info_) == ReturnCode_t::RETCODE_OK) { - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = request.buffer_; - data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->request_reader_->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (request.sample_info_.valid_data) - { - request.sample_identity_ = request.sample_info_.sample_identity; - // Use response subscriber guid (on related_sample_identity) when present. - const eprosima::fastrtps::rtps::GUID_t& reader_guid = - request.sample_info_.related_sample_identity.writer_guid(); - if (reader_guid != eprosima::fastrtps::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 = - request.sample_info_.sample_identity.writer_guid(); - info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); - - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_request, info->request_type_support_impl_)) - { - // Get header - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - request.sample_identity_.writer_guid(), - request_header->request_id.writer_guid); - request_header->request_id.sequence_number = - ((int64_t)request.sample_identity_.sequence_number().high) << - 32 | request.sample_identity_.sequence_number().low; - request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); - *taken = true; - } - - } + if (request.sample_info_.valid_data) { + request.sample_identity_ = request.sample_info_.sample_identity; + // Use response subscriber guid (on related_sample_identity) when present. + const eprosima::fastrtps::rtps::GUID_t & reader_guid = + request.sample_info_.related_sample_identity.writer_guid(); + if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown()) { + request.sample_identity_.writer_guid() = reader_guid; } - delete request.buffer_; + // Save both guids in the clients_endpoints map + const eprosima::fastrtps::rtps::GUID_t & writer_guid = + request.sample_info_.sample_identity.writer_guid(); + info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); + + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_request, info->request_type_support_impl_)) + { + // Get header + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + request.sample_identity_.writer_guid(), + request_header->request_id.writer_guid); + request_header->request_id.sequence_number = + ((int64_t)request.sample_identity_.sequence_number().high) << + 32 | request.sample_identity_.sequence_number().low; + request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); + *taken = true; + } + } } + delete request.buffer_; + } + - return RMW_RET_OK; + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 94b9a7dc2..371f5b640 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -28,142 +28,137 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_take_response( - const char* identifier, - const rmw_client_t* client, - rmw_service_info_t* request_header, - void* ros_response, - bool* taken) + const char * identifier, + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - - *taken = false; - - auto info = static_cast(client->data); - assert(info); - - CustomClientResponse 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.is_cdr_buffer = true; - data.data = response.buffer_.get(); - data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->response_reader_->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (response.sample_info_.valid_data) + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + *taken = false; + + auto info = static_cast(client->data); + assert(info); + + CustomClientResponse 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.is_cdr_buffer = true; + data.data = response.buffer_.get(); + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->response_reader_->take_next_sample( + &data, + &response.sample_info_) == ReturnCode_t::RETCODE_OK) + { + if (response.sample_info_.valid_data) { + response.sample_identity_ = response.sample_info_.related_sample_identity; + + if (response.sample_identity_.writer_guid() == info->reader_guid_ || + response.sample_identity_.writer_guid() == info->writer_guid_) + { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser( + *response.buffer_, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_response, info->response_type_support_impl_)) { - response.sample_identity_ = response.sample_info_.related_sample_identity; - - if (response.sample_identity_.writer_guid() == info->reader_guid_ || - response.sample_identity_.writer_guid() == info->writer_guid_) - { - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser( - *response.buffer_, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_response, info->response_type_support_impl_)) - { - request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); - request_header->request_id.sequence_number = - ((int64_t)response.sample_identity_.sequence_number().high) << - 32 | response.sample_identity_.sequence_number().low; - - *taken = true; - } - } + request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); + request_header->request_id.sequence_number = + ((int64_t)response.sample_identity_.sequence_number().high) << + 32 | response.sample_identity_.sequence_number().low; + + *taken = true; } + } } + } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_send_response( - const char* identifier, - const rmw_service_t* service, - rmw_request_id_t* request_header, - void* ros_response) + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(service->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( - request_header->writer_guid, - &wparams.related_sample_identity().writer_guid()); - wparams.related_sample_identity().sequence_number().high = - (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); - wparams.related_sample_identity().sequence_number().low = - (int32_t)(request_header->sequence_number & 0xFFFFFFFF); - - // TODO(MiguelCompany) The following block is a workaround for the race on the - // discovery of services. It is (ab)using a related_sample_identity on the request - // with the GUID of the response reader, so we can wait here for it to be matched to - // the server response writer. In the future, this should be done with the mechanism - // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) - - // According to the list of possible entity kinds in section 9.3.1.2 of RTPS - // 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 = - 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. - // Wait for the response writer to be matched with it. - auto listener = info->pub_listener_; - client_present_t ret = listener->check_for_subscription(related_guid); - if (ret == client_present_t::GONE) - { - return RMW_RET_OK; - } - else if (ret == client_present_t::MAYBE) - { - RMW_SET_ERROR_MSG("client will not receive response"); - return RMW_RET_TIMEOUT; - } - } - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_response); - data.impl = info->response_type_support_impl_; - if (info->response_writer_->write(&data, wparams)) - { - returnedValue = RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(service->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + request_header->writer_guid, + &wparams.related_sample_identity().writer_guid()); + wparams.related_sample_identity().sequence_number().high = + (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); + wparams.related_sample_identity().sequence_number().low = + (int32_t)(request_header->sequence_number & 0xFFFFFFFF); + + // TODO(MiguelCompany) The following block is a workaround for the race on the + // discovery of services. It is (ab)using a related_sample_identity on the request + // with the GUID of the response reader, so we can wait here for it to be matched to + // the server response writer. In the future, this should be done with the mechanism + // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) + + // According to the list of possible entity kinds in section 9.3.1.2 of RTPS + // 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 = + 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. + // Wait for the response writer to be matched with it. + auto listener = info->pub_listener_; + client_present_t ret = listener->check_for_subscription(related_guid); + if (ret == client_present_t::GONE) { + return RMW_RET_OK; + } else if (ret == client_present_t::MAYBE) { + RMW_SET_ERROR_MSG("client will not receive response"); + return RMW_RET_TIMEOUT; } - else - { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_response); + data.impl = info->response_type_support_impl_; + if (info->response_writer_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 5da0eeac1..7d27da250 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -42,133 +42,130 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_service( - const char* identifier, - rmw_node_t* node, - rmw_service_t* service) + const char * identifier, + rmw_node_t * node, + rmw_service_t * service) { - rmw_ret_t final_ret = RMW_RET_OK; - - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(service->data); + rmw_ret_t final_ret = RMW_RET_OK; + + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(service->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_reader_->guid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_writer_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_reader_->guid()); - common_context->graph_cache.dissociate_reader( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_writer_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_writer( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_writer_->get_topic(); + auto request_topic = info->request_reader_->get_topicdescription(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datareader"); + final_ret = RMW_RET_ERROR; } - auto show_previous_error = - [&final_ret]() - { - if (RMW_RET_OK != final_ret) - { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_writer_->get_topic(); - auto request_topic = info->request_reader_->get_topicdescription(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datareader"); - final_ret = RMW_RET_ERROR; - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->response_writer_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datawriter"); - final_ret = RMW_RET_ERROR; - } - - // Delete DataWriter listener - if (nullptr != info->pub_listener_) - { - delete info->pub_listener_; - } - - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - - // Delete CustomServiceInfo structure - delete info; + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->response_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); + final_ret = RMW_RET_ERROR; } - rmw_free(const_cast(service->service_name)); - rmw_service_free(service); + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomServiceInfo structure + delete info; + } + + rmw_free(const_cast(service->service_name)); + rmw_service_free(service); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_service_response_publisher_get_actual_qos( - const rmw_service_t* service, - rmw_qos_profile_t* qos) + const rmw_service_t * service, + rmw_qos_profile_t * qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataWriter* fastdds_rw = srv->response_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = srv->response_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_request_subscription_get_actual_qos( - const rmw_service_t* service, - rmw_qos_profile_t* qos) + const rmw_service_t * service, + rmw_qos_profile_t * qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataReader* fastdds_rr = srv->request_reader_; - dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataReader * fastdds_rr = srv->request_reader_; + dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_set_on_new_request_callback( - rmw_service_t* rmw_service, - rmw_event_callback_t callback, - const void* user_data) + rmw_service_t * rmw_service, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_service_info = static_cast(rmw_service->data); - custom_service_info->listener_->set_on_new_request_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_service_info = static_cast(rmw_service->data); + custom_service_info->listener_->set_on_new_request_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 32765a26d..983a21ac2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -35,260 +35,243 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_subscription( - const char* identifier, - const rmw_node_t* node, - rmw_subscription_t* subscription, - bool reset_cft) + const char * identifier, + const rmw_node_t * node, + rmw_subscription_t * subscription, + bool reset_cft) { - assert(node->implementation_identifier == identifier); - assert(subscription->implementation_identifier == identifier); + assert(node->implementation_identifier == identifier); + assert(subscription->implementation_identifier == identifier); - rmw_ret_t ret = RMW_RET_OK; - rmw_error_state_t error_state; - rmw_error_string_t error_string; - auto common_context = static_cast(node->context->impl->common); - auto info = static_cast(subscription->data); - { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != ret) - { - error_state = *rmw_get_error_state(); - error_string = rmw_get_error_string(); - rmw_reset_error(); - } + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + rmw_error_string_t error_string; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != ret) { + error_state = *rmw_get_error_state(); + error_string = rmw_get_error_string(); + rmw_reset_error(); } + } - auto participant_info = - static_cast(node->context->impl->participant_info); - rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); - if (RMW_RET_OK != local_ret) - { - if (RMW_RET_OK != ret) - { - RMW_SAFE_FWRITE_TO_STDERR(error_string.str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - } - ret = local_ret; + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); + if (RMW_RET_OK != local_ret) { + if (RMW_RET_OK != ret) { + RMW_SAFE_FWRITE_TO_STDERR(error_string.str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); } - else if (RMW_RET_OK != ret) - { - rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); - } - return ret; + ret = local_ret; + } else if (RMW_RET_OK != ret) { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + return ret; } rmw_ret_t __rmw_subscription_count_matched_publishers( - const rmw_subscription_t* subscription, - size_t* publisher_count) + const rmw_subscription_t * subscription, + size_t * publisher_count) { - auto info = static_cast(subscription->data); + auto info = static_cast(subscription->data); - *publisher_count = info->listener_->publisherCount(); + *publisher_count = info->listener_->publisherCount(); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_actual_qos( - const rmw_subscription_t* subscription, - rmw_qos_profile_t* qos) + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader* fastdds_dr = info->data_reader_; - const eprosima::fastdds::dds::DataReaderQos& dds_qos = fastdds_dr->get_qos(); + auto info = static_cast(subscription->data); + 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); + dds_qos_to_rmw_qos(dds_qos, qos); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_set_content_filter( - rmw_subscription_t* subscription, - const rmw_subscription_content_filter_options_t* options - ) + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options +) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; - const bool filter_expression_empty = (*options->filter_expression == '\0'); + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; + const bool filter_expression_empty = (*options->filter_expression == '\0'); - if (!filtered_topic && filter_expression_empty) - { - // can't reset current subscriber - RMW_SET_ERROR_MSG( - "current subscriber has no content filter topic"); - return RMW_RET_ERROR; + if (!filtered_topic && filter_expression_empty) { + // can't reset current subscriber + RMW_SET_ERROR_MSG( + "current subscriber has no content filter topic"); + return RMW_RET_ERROR; + } else if (filtered_topic && !filter_expression_empty) { + std::vector expression_parameters; + for (size_t i = 0; i < options->expression_parameters.size; ++i) { + expression_parameters.push_back(options->expression_parameters.data[i]); } - else if (filtered_topic && !filter_expression_empty) - { - std::vector expression_parameters; - for (size_t i = 0; i < options->expression_parameters.size; ++i) - { - expression_parameters.push_back(options->expression_parameters.data[i]); - } - ReturnCode_t ret = - filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) - { - RMW_SET_ERROR_MSG( - "failed to set_filter_expression"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; + ReturnCode_t ret = + filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to set_filter_expression"); + return RMW_RET_ERROR; } + return RMW_RET_OK; + } - eprosima::fastdds::dds::DomainParticipant* dds_participant = - info->dds_participant_; - eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; - const char* eprosima_fastrtps_identifier = subscription->implementation_identifier; + eprosima::fastdds::dds::DomainParticipant * dds_participant = + info->dds_participant_; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + const char * eprosima_fastrtps_identifier = subscription->implementation_identifier; - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( - eprosima_fastrtps_identifier, - info->node_, - subscription, - true /* reset_cft */); - if (ret != RMW_RET_OK) - { - RMW_SET_ERROR_MSG("delete subscription with reset cft"); - return RMW_RET_ERROR; - } + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, + info->node_, + subscription, + true /* reset_cft */); + if (ret != RMW_RET_OK) { + RMW_SET_ERROR_MSG("delete subscription with reset cft"); + return RMW_RET_ERROR; + } - if (!filtered_topic) + if (!filtered_topic) { + // create content filtered topic + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, info->topic_, + info->topic_name_mangled_, options, &filtered_topic)) { - // create content filtered topic - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, info->topic_, - info->topic_name_mangled_, options, &filtered_topic)) - { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); - return RMW_RET_ERROR; - } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; - } - else - { - // use the existing parent topic - des_topic = info->topic_; + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + return RMW_RET_ERROR; } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; + } else { + // use the existing parent topic + des_topic = info->topic_; + } - // create data reader - eprosima::fastdds::dds::Subscriber* subscriber = info->subscriber_; - const rmw_subscription_options_t* subscription_options = - &subscription->options; - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) + // create data reader + eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; + const rmw_subscription_options_t * subscription_options = + &subscription->options; + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return RMW_RET_ERROR; + } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); - return RMW_RET_ERROR; - } - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); + subscriber->delete_datareader(info->data_reader_); + }); - ///// - // Update RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); + ///// + // Update RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); - { - rmw_dds_common::Context* common_context = info->common_context_; - const rmw_node_t* node = info->node_; + { + rmw_dds_common::Context * common_context = info->common_context_; + const rmw_node_t * node = info->node_; - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - eprosima_fastrtps_identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != rmw_ret) - { - static_cast(common_context->graph_cache.dissociate_writer( - info->subscription_gid_, common_context->gid, node->name, node->namespace_)); - return RMW_RET_ERROR; - } + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + return RMW_RET_ERROR; } - cleanup_datareader.cancel(); - return RMW_RET_OK; + } + cleanup_datareader.cancel(); + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_content_filter( - const rmw_subscription_t* subscription, - rcutils_allocator_t* allocator, - rmw_subscription_content_filter_options_t* options - ) + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options +) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; - if (nullptr == filtered_topic) - { - RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); - return RMW_RET_ERROR; - } - std::vector expression_parameters; - ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) - { - RMW_SET_ERROR_MSG( - "failed to get_expression_parameters"); - return RMW_RET_ERROR; - } + if (nullptr == filtered_topic) { + RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); + return RMW_RET_ERROR; + } + std::vector expression_parameters; + ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to get_expression_parameters"); + return RMW_RET_ERROR; + } - std::vector string_array; - for (size_t i = 0; i < expression_parameters.size(); ++i) - { - string_array.push_back(expression_parameters[i].c_str()); - } + std::vector string_array; + for (size_t i = 0; i < expression_parameters.size(); ++i) { + string_array.push_back(expression_parameters[i].c_str()); + } - return rmw_subscription_content_filter_options_init( - filtered_topic->get_filter_expression().c_str(), - string_array.size(), - string_array.data(), - allocator, - options - ); + return rmw_subscription_content_filter_options_init( + filtered_topic->get_filter_expression().c_str(), + string_array.size(), + string_array.data(), + allocator, + options + ); } rmw_ret_t __rmw_subscription_set_on_new_message_callback( - rmw_subscription_t* rmw_subscription, - rmw_event_callback_t callback, - const void* user_data) + rmw_subscription_t * rmw_subscription, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_subscriber_info = static_cast(rmw_subscription->data); - custom_subscriber_info->listener_->set_on_new_message_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_subscriber_info = static_cast(rmw_subscription->data); + custom_subscriber_info->listener_->set_on_new_message_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 8405c1b6d..89ba122ce 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -36,533 +36,502 @@ #include "tracetools/tracetools.h" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ using DataSharingKind = eprosima::fastdds::dds::DataSharingKind; void _assign_message_info( - const char* identifier, - rmw_message_info_t* message_info, - const eprosima::fastdds::dds::SampleInfo* sinfo) + const char * identifier, + rmw_message_info_t * message_info, + const eprosima::fastdds::dds::SampleInfo * sinfo) { - message_info->source_timestamp = sinfo->source_timestamp.to_ns(); - message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); - auto fastdds_sn = sinfo->sample_identity.sequence_number(); - message_info->publication_sequence_number = - (static_cast(fastdds_sn.high) << 32) | - static_cast(fastdds_sn.low); - message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; - rmw_gid_t* sender_gid = &message_info->publisher_gid; - 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( - sinfo->sample_identity.writer_guid(), - sender_gid->data); + message_info->source_timestamp = sinfo->source_timestamp.to_ns(); + message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); + auto fastdds_sn = sinfo->sample_identity.sequence_number(); + message_info->publication_sequence_number = + (static_cast(fastdds_sn.high) << 32) | + static_cast(fastdds_sn.low); + message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; + rmw_gid_t * sender_gid = &message_info->publisher_gid; + 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( + sinfo->sample_identity.writer_guid(), + sender_gid->data); } rmw_ret_t _take( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - - data.is_cdr_buffer = false; - data.data = ros_message; - data.impl = info->type_support_impl_; - - while (0 < info->data_reader_->get_unread_count()) - { - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) - { - - if (subscription->options.ignore_local_publications) - { - auto sample_writer_guid = - eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); - - if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) - { - // This is a local publication. Ignore it - continue; - } - } - - if (sinfo.valid_data) - { - if (message_info) - { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; - break; - } + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + + data.is_cdr_buffer = false; + data.data = ros_message; + data.impl = info->type_support_impl_; + + while (0 < info->data_reader_->get_unread_count()) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (subscription->options.ignore_local_publications) { + auto sample_writer_guid = + eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); + + if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { + // This is a local publication. Ignore it + continue; } - } + } - TRACEPOINT( - rmw_take, - static_cast(subscription), - static_cast(ros_message), - (message_info ? message_info->source_timestamp : 0LL), - *taken); - return RMW_RET_OK; + if (sinfo.valid_data) { + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + break; + } + } + } + + TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + (message_info ? message_info->source_timestamp : 0LL), + *taken); + return RMW_RET_OK; } rmw_ret_t _take_sequence( - const char* identifier, - const rmw_subscription_t* subscription, - size_t count, - rmw_message_sequence_t* message_sequence, - rmw_message_info_sequence_t* message_info_sequence, - size_t* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) { - *taken = 0; - bool taken_flag = false; - rmw_ret_t ret = RMW_RET_OK; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - // Limit the upper bound of reads to the number unread at the beginning. - // This prevents any samples that are added after the beginning of the - // _take_sequence call from being read. - auto unread_count = info->data_reader_->get_unread_count(); - if (unread_count < count) - { - count = unread_count; + *taken = 0; + bool taken_flag = false; + rmw_ret_t ret = RMW_RET_OK; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + // Limit the upper bound of reads to the number unread at the beginning. + // This prevents any samples that are added after the beginning of the + // _take_sequence call from being read. + auto unread_count = info->data_reader_->get_unread_count(); + if (unread_count < count) { + count = unread_count; + } + + for (size_t ii = 0; ii < count; ++ii) { + taken_flag = false; + ret = _take( + identifier, subscription, message_sequence->data[*taken], + &taken_flag, &message_info_sequence->data[*taken], allocation); + + if (ret != RMW_RET_OK) { + break; } - for (size_t ii = 0; ii < count; ++ii) - { - taken_flag = false; - ret = _take( - identifier, subscription, message_sequence->data[*taken], - &taken_flag, &message_info_sequence->data[*taken], allocation); - - if (ret != RMW_RET_OK) - { - break; - } - - if (taken_flag) - { - (*taken)++; - } + if (taken_flag) { + (*taken)++; } + } - message_sequence->size = *taken; - message_info_sequence->size = *taken; + message_sequence->size = *taken; + message_info_sequence->size = *taken; - return ret; + return ret; } rmw_ret_t __rmw_take_event( - const char* identifier, - const rmw_event_t* event_handle, - void* event_info, - bool* taken) + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - event handle, - event_handle->implementation_identifier, - identifier, - return RMW_RET_ERROR); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + event handle, + event_handle->implementation_identifier, + identifier, + return RMW_RET_ERROR); - auto event = static_cast(event_handle->data); - if (event->get_listener()->take_event(event_handle->event_type, event_info)) - { - *taken = true; - return RMW_RET_OK; - } + auto event = static_cast(event_handle->data); + if (event->get_listener()->take_event(event_handle->event_type, event_info)) { + *taken = true; + return RMW_RET_OK; + } - return RMW_RET_ERROR; + return RMW_RET_ERROR; } rmw_ret_t __rmw_take( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, nullptr, allocation); + return _take(identifier, subscription, ros_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_sequence( - const char* identifier, - const rmw_subscription_t* subscription, - size_t count, - rmw_message_sequence_t* message_sequence, - rmw_message_info_sequence_t* message_info_sequence, - size_t* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - if (0u == count) - { - RMW_SET_ERROR_MSG("count cannot be 0"); - return RMW_RET_INVALID_ARGUMENT; - } + if (0u == count) { + RMW_SET_ERROR_MSG("count cannot be 0"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_sequence->capacity) - { - RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_info_sequence->capacity) - { - RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_info_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - return _take_sequence( - identifier, subscription, count, message_sequence, message_info_sequence, - taken, allocation); + return _take_sequence( + identifier, subscription, count, message_sequence, message_info_sequence, + taken, allocation); } rmw_ret_t __rmw_take_with_info( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, message_info, allocation); + return _take(identifier, subscription, ros_message, taken, message_info, allocation); } rmw_ret_t _take_serialized_message( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastcdr::FastBuffer buffer; - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = &buffer; - data.impl = nullptr; // not used when is_cdr_buffer is true - - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) - { - if (sinfo.valid_data) - { - auto buffer_size = static_cast(buffer.getBufferSize()); - if (serialized_message->buffer_capacity < buffer_size) - { - auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); - if (ret != RMW_RET_OK) - { - return ret; // Error message already set - } - } - serialized_message->buffer_length = buffer_size; - memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); - - if (message_info) - { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer; + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &buffer; + data.impl = nullptr; // not used when is_cdr_buffer is true + + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (sinfo.valid_data) { + auto buffer_size = static_cast(buffer.getBufferSize()); + if (serialized_message->buffer_capacity < buffer_size) { + auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); + if (ret != RMW_RET_OK) { + return ret; // Error message already set } + } + serialized_message->buffer_length = buffer_size; + memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); + + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; } + } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_take_serialized_message( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, nullptr, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_serialized_message_with_info( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, message_info, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, message_info, allocation); } // ----------------- Loans related code ------------------------- // struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection { - GenericSequence() = default; - - void resize( - size_type /*new_length*/) override - { - // This kind of collection should only be used with loans - throw std::bad_alloc(); - } - + GenericSequence() = default; + + void resize( + size_type /*new_length*/) override + { + // This kind of collection should only be used with loans + throw std::bad_alloc(); + } }; struct LoanManager { - struct Item - { - GenericSequence data_seq{}; - eprosima::fastdds::dds::SampleInfoSeq info_seq{}; - }; - - explicit LoanManager( - const eprosima::fastrtps::ResourceLimitedContainerConfig& items_cfg) - : items(items_cfg) - { + struct Item + { + GenericSequence data_seq{}; + eprosima::fastdds::dds::SampleInfoSeq info_seq{}; + }; + + explicit LoanManager( + const eprosima::fastrtps::ResourceLimitedContainerConfig & items_cfg) + : items(items_cfg) + { + } + + void add_item( + std::unique_ptr item) + { + std::lock_guard guard(mtx); + items.push_back(std::move(item)); + } + + std::unique_ptr erase_item( + void * loaned_message) + { + std::unique_ptr ret{nullptr}; + + std::lock_guard guard(mtx); + for (auto it = items.begin(); it != items.end(); ++it) { + if (loaned_message == (*it)->data_seq.buffer()[0]) { + ret = std::move(*it); + items.erase(it); + break; + } } - void add_item( - std::unique_ptr item) - { - std::lock_guard guard(mtx); - items.push_back(std::move(item)); - } - - std::unique_ptr erase_item( - void* loaned_message) - { - std::unique_ptr ret{nullptr}; - - std::lock_guard guard(mtx); - for (auto it = items.begin(); it != items.end(); ++it) - { - if (loaned_message == (*it)->data_seq.buffer()[0]) - { - ret = std::move(*it); - items.erase(it); - break; - } - } - - return ret; - } + return ret; + } private: - - std::mutex mtx; - using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY( - mtx); + std::mutex mtx; + using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; + ItemVector items RCPPUTILS_TSA_GUARDED_BY( + mtx); }; void __init_subscription_for_loans( - rmw_subscription_t* subscription) + rmw_subscription_t * subscription) { - auto info = static_cast(subscription->data); - const auto& qos = info->data_reader_->get_qos(); - bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); - subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); - if (subscription->can_loan_messages) - { - const auto& allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; - info->loan_manager_ = std::make_shared(allocation_qos); - } + auto info = static_cast(subscription->data); + const auto & qos = info->data_reader_->get_qos(); + bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); + subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); + if (subscription->can_loan_messages) { + const auto & allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; + info->loan_manager_ = std::make_shared(allocation_qos); + } } rmw_ret_t __rmw_take_loaned_message_internal( - const char* identifier, - const rmw_subscription_t* subscription, - void** loaned_message, - bool* taken, - rmw_message_info_t* message_info) + const char * identifier, + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) - { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; - } - - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } - auto info = static_cast(subscription->data); + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - auto item = std::make_unique(); + auto info = static_cast(subscription->data); - while (ReturnCode_t::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]); - } - *loaned_message = item->data_seq.buffer()[0]; - *taken = true; + auto item = std::make_unique(); - info->loan_manager_->add_item(std::move(item)); + while (ReturnCode_t::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]); + } + *loaned_message = item->data_seq.buffer()[0]; + *taken = true; - return RMW_RET_OK; - } + info->loan_manager_->add_item(std::move(item)); - // Should return loan before taking again - info->data_reader_->return_loan(item->data_seq, item->info_seq); + return RMW_RET_OK; } - // No data available, return loan information. - *taken = false; - return RMW_RET_OK; + // Should return loan before taking again + info->data_reader_->return_loan(item->data_seq, item->info_seq); + } + + // No data available, return loan information. + *taken = false; + return RMW_RET_OK; } rmw_ret_t __rmw_return_loaned_message_from_subscription( - const char* identifier, - const rmw_subscription_t* subscription, - void* loaned_message) + const char * identifier, + const rmw_subscription_t * subscription, + void * loaned_message) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) - { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + 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)) { + RMW_SET_ERROR_MSG("Error returning loan"); + return RMW_RET_ERROR; } - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - - auto info = static_cast(subscription->data); - 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)) - { - RMW_SET_ERROR_MSG("Error returning loan"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; - } + return RMW_RET_OK; + } - RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); + return RMW_RET_ERROR; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp index 45b5287bc..0f3494ff8 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp @@ -35,7 +35,8 @@ __rmw_trigger_guard_condition( return RMW_RET_ERROR; } - auto guard_condition = static_cast(guard_condition_handle->data); + auto guard_condition = + static_cast(guard_condition_handle->data); guard_condition->set_trigger_value(true); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index db7b33c79..a0095c930 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -27,275 +27,255 @@ #include "fastdds/dds/core/condition/GuardCondition.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_wait( - const char* identifier, - rmw_subscriptions_t* subscriptions, - rmw_guard_conditions_t* guard_conditions, - rmw_services_t* services, - rmw_clients_t* clients, - rmw_events_t* events, - rmw_wait_set_t* wait_set, - const rmw_time_t* wait_timeout) + const char * identifier, + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_events_t * events, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - wait set handle, - wait_set->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - // If wait_set_info is ever nullptr, it can only mean one of three things: - // - Wait set is invalid. Caller did not respect preconditions. - // - Implementation is logically broken. Definitely not something we want to treat as a normal - // error. - // - Heap is corrupt. - // In all three cases, it's better if this crashes soon enough. - auto fastdds_wait_set = static_cast(wait_set->data); - bool no_has_wait = false; + // If wait_set_info is ever nullptr, it can only mean one of three things: + // - Wait set is invalid. Caller did not respect preconditions. + // - Implementation is logically broken. Definitely not something we want to treat as a normal + // error. + // - Heap is corrupt. + // In all three cases, it's better if this crashes soon enough. + auto fastdds_wait_set = static_cast(wait_set->data); + bool no_has_wait = false; - if (subscriptions) - { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) - { - void* data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); - } + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_subscriber_info->data_reader_->get_statuscondition()); } + } - if (clients) - { - for (size_t i = 0; i < clients->client_count; ++i) - { - void* data = clients->clients[i]; - auto custom_client_info = static_cast(data); - no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); - } + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_client_info->response_reader_->get_statuscondition()); } + } - if (services) - { - for (size_t i = 0; i < services->service_count; ++i) - { - void* data = services->services[i]; - auto custom_service_info = static_cast(data); - no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); - } + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_service_info->request_reader_->get_statuscondition()); } + } - if (events) - { - for (size_t i = 0; i < events->event_count; ++i) - { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->attach_condition(custom_event_info->get_listener()->get_statuscondition()); - fastdds_wait_set->attach_condition(custom_event_info->get_listener()->event_guard[event->event_type]); - } + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->attach_condition( + custom_event_info->get_listener()->get_statuscondition()); + fastdds_wait_set->attach_condition( + custom_event_info->get_listener()->event_guard[event-> + event_type]); } + } - if (guard_conditions) - { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) - { - void* data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->attach_condition(*guard_condition); - } + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + fastdds_wait_set->attach_condition(*guard_condition); } + } - eprosima::fastdds::dds::ConditionSeq triggered_coditions; - Duration_t timeout{0, 0}; - if (!no_has_wait) - { - timeout = (wait_timeout) ? - Duration_t{static_cast(wait_timeout->sec), - static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; - } + eprosima::fastdds::dds::ConditionSeq triggered_coditions; + Duration_t timeout{0, 0}; + if (!no_has_wait) { + timeout = (wait_timeout) ? + Duration_t{static_cast(wait_timeout->sec), + static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; + } - ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, - timeout - ); + ReturnCode_t ret_code = fastdds_wait_set->wait( + triggered_coditions, + timeout + ); - if (subscriptions) - { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) - { - void* data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_subscriber_info->data_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - subscriptions->subscribers[i] = 0; - } - } - else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) - { - subscriptions->subscribers[i] = 0; - } + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_subscriber_info->data_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + subscriptions->subscribers[i] = 0; } + } else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) { + subscriptions->subscribers[i] = 0; + } } + } - if (clients) - { - for (size_t i = 0; i < clients->client_count; ++i) - { - void* data = clients->clients[i]; - auto custom_client_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_client_info->response_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - clients->clients[i] = 0; - } - } - else if (0 == custom_client_info->response_reader_->get_unread_count()) - { - clients->clients[i] = 0; - } + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_client_info->response_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + clients->clients[i] = 0; } + } else if (0 == custom_client_info->response_reader_->get_unread_count()) { + clients->clients[i] = 0; + } } + } - if (services) - { - for (size_t i = 0; i < services->service_count; ++i) - { - void* data = services->services[i]; - auto custom_service_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_service_info->request_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - services->services[i] = 0; - } - } - else if (0 == custom_service_info->request_reader_->get_unread_count()) - { - services->services[i] = 0; - } + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_service_info->request_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + services->services[i] = 0; } + } else if (0 == custom_service_info->request_reader_->get_unread_count()) { + services->services[i] = 0; + } } + } - if (events) - { - for (size_t i = 0; i < events->event_count; ++i) + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->detach_condition(custom_event_info->get_listener()->get_statuscondition()); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_event_info->get_listener()->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + eprosima::fastdds::dds::GuardCondition * guard_condition = + &custom_event_info->get_listener()->event_guard[event->event_type]; + bool active = false; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (changed_statuses.is_active( + rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event->event_type))) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->detach_condition(custom_event_info->get_listener()->get_statuscondition()); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_event_info->get_listener()->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - eprosima::fastdds::dds::GuardCondition* guard_condition = - &custom_event_info->get_listener()->event_guard[event->event_type]; - bool active = false; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event->event_type))) - { - active = true; - } - } - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [guard_condition](const eprosima::fastdds::dds::Condition* c) - { - return c == guard_condition; - })) - { - if (guard_condition->get_trigger_value()) - { - active = true; - guard_condition->set_trigger_value(false); - } - } + active = true; + } + } + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [guard_condition](const eprosima::fastdds::dds::Condition * c) + { + return c == guard_condition; + })) + { + if (guard_condition->get_trigger_value()) { + active = true; + guard_condition->set_trigger_value(false); + } + } - if (!active) - { - events->events[i] = 0; - } - } + if (!active) { + events->events[i] = 0; + } } + } - if (guard_conditions) - { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) - { - void* data = guard_conditions->guard_conditions[i]; - auto condition = static_cast(data); - fastdds_wait_set->detach_condition(*condition); - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - if (!condition->get_trigger_value()) - { - guard_conditions->guard_conditions[i] = 0; - } - } - else - { - guard_conditions->guard_conditions[i] = 0; - } - condition->set_trigger_value(false); + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto condition = static_cast(data); + fastdds_wait_set->detach_condition(*condition); + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + if (!condition->get_trigger_value()) { + guard_conditions->guard_conditions[i] = 0; } + } else { + guard_conditions->guard_conditions[i] = 0; + } + condition->set_trigger_value(false); } + } - return ((no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT); + return (no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp index 8c127418a..b706a5006 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp @@ -94,7 +94,8 @@ __rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set) if (wait_set->data) { if (fastdds_wait_set) { RMW_TRY_DESTRUCTOR( - fastdds_wait_set->eprosima::fastdds::dds::WaitSet::~WaitSet(), fastdds_wait_set, result = RMW_RET_ERROR) + fastdds_wait_set->eprosima::fastdds::dds::WaitSet::~WaitSet(), fastdds_wait_set, + result = RMW_RET_ERROR) } rmw_free(wait_set->data); } diff --git a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp index 0cc3225d6..47e10f905 100644 --- a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp +++ b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp @@ -27,7 +27,7 @@ namespace internal bool is_event_supported(rmw_event_type_t event_type); eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( - const rmw_event_type_t event_type); + const rmw_event_type_t event_type); } // namespace internal } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index f9111588e..37662cf01 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -205,7 +205,7 @@ create_datareader( des_topic, datareader_qos, listener, - eprosima::fastdds::dds::StatusMask::subscription_matched()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); } return true; } From ac33f66cf30d54b1c0021c211a5c787d15bf3f25 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Wed, 6 Jul 2022 16:12:00 +0200 Subject: [PATCH 09/34] Simplify SubListener's get_unread_messages() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../src/custom_subscriber_info.cpp | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index fa1b15acc..a156b21a6 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -234,26 +234,7 @@ SubListener::set_on_new_message_callback( size_t SubListener::get_unread_messages() { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read( - data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); - ++count) - { - if (info_seq[count].valid_data) { - ++ret_value; - } - } - - subscriber_info_->data_reader_->return_loan(data_seq, info_seq); - } - - return ret_value; + return subscriber_info_->data_reader_->get_unread_count(); } void From a64fc56bb40d91bab35c2b8fcc6760fd9ea333a5 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Wed, 6 Jul 2022 17:08:29 +0200 Subject: [PATCH 10/34] Simplified get_unread_requests() and get_unread_responses() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../custom_client_info.hpp | 21 +------------------ .../custom_service_info.hpp | 21 +------------------ 2 files changed, 2 insertions(+), 40 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index d5a38b470..61fd6fdb4 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 @@ -118,26 +118,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener size_t get_unread_responses() { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read( - data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; - count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) { - ++ret_value; - } - } - - info_->response_reader_->return_loan(data_seq, info_seq); - } - - return ret_value; + return info_->response_reader_->get_unread_count(); } // Provide handlers to perform an action when a 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 9093f7ec1..4d04c06e4 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -206,26 +206,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener size_t get_unread_resquests() { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read( - data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; - count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) { - ++ret_value; - } - } - - info_->request_reader_->return_loan(data_seq, info_seq); - } - - return ret_value; + return info_->request_reader_->get_unread_count(); } void From 6de684e4e2b63ad751f070a3a8b854a1c71cd512 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 07:26:37 +0200 Subject: [PATCH 11/34] Moved Waitset creation/destruction outside loop as suggested MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../src/listener_thread.cpp | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index 2b6b6b002..f998bfaca 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -122,6 +122,15 @@ node_listener( assert(nullptr != context->impl); assert(nullptr != context->impl->common); auto common_context = static_cast(context->impl->common); + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY("failed to create waitset") \ + ": ros discovery info listener thread will shutdown ...\n"); + } while (common_context->thread_is_running.load()) { assert(nullptr != common_context->sub); assert(nullptr != common_context->sub->data); @@ -133,12 +142,6 @@ node_listener( subscriptions.subscribers = subscriptions_buffer; guard_conditions.guard_condition_count = 1; guard_conditions.guard_conditions = guard_conditions_buffer; - // number of conditions of a subscription is 2 - rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( - context->implementation_identifier, context, 2); - if (nullptr == wait_set) { - TERMINATE_THREAD("failed to create wait set"); - } if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( context->implementation_identifier, &subscriptions, @@ -151,11 +154,6 @@ node_listener( { TERMINATE_THREAD("rmw_wait failed"); } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( - context->implementation_identifier, wait_set)) - { - TERMINATE_THREAD("failed to destroy wait set"); - } if (subscriptions_buffer[0]) { rmw_dds_common::msg::ParticipantEntitiesInfo msg; bool taken = true; @@ -184,4 +182,12 @@ node_listener( } } } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY("failed to destroy waitset") \ + ": ros discovery info listener thread will shutdown ...\n"); + } } From dc5d78dbd6d7b12e81a4004904769171395f97d4 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 07:28:15 +0200 Subject: [PATCH 12/34] Renamed variable. Removed unneded checks. Replaced get_unread_count with get_first_untaken_info MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 72 +++++++----------------- 1 file changed, 21 insertions(+), 51 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index a0095c930..2e3936996 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -56,13 +56,14 @@ __rmw_wait( // - Heap is corrupt. // In all three cases, it's better if this crashes soon enough. auto fastdds_wait_set = static_cast(wait_set->data); - bool no_has_wait = false; + bool skip_wait = false; if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); - no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); + eprosima::fastdds::dds::SampleInfo sample_info; + skip_wait |= (ReturnCode_t::RETCODE_OK == custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_subscriber_info->data_reader_->get_statuscondition()); } @@ -72,7 +73,8 @@ __rmw_wait( for (size_t i = 0; i < clients->client_count; ++i) { void * data = clients->clients[i]; auto custom_client_info = static_cast(data); - no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); + eprosima::fastdds::dds::SampleInfo sample_info; + skip_wait |= (ReturnCode_t::RETCODE_OK == custom_client_info->response_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_client_info->response_reader_->get_statuscondition()); } @@ -82,7 +84,8 @@ __rmw_wait( for (size_t i = 0; i < services->service_count; ++i) { void * data = services->services[i]; auto custom_service_info = static_cast(data); - no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); + eprosima::fastdds::dds::SampleInfo sample_info; + skip_wait |= (ReturnCode_t::RETCODE_OK == custom_service_info->request_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_service_info->request_reader_->get_statuscondition()); } @@ -110,7 +113,7 @@ __rmw_wait( eprosima::fastdds::dds::ConditionSeq triggered_coditions; Duration_t timeout{0, 0}; - if (!no_has_wait) { + if (!skip_wait) { timeout = (wait_timeout) ? Duration_t{static_cast(wait_timeout->sec), static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; @@ -128,21 +131,15 @@ __rmw_wait( eprosima::fastdds::dds::StatusCondition & status_condition = custom_subscriber_info->data_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition * condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition * c) - { - return c == condition; - })) + eprosima::fastdds::dds::SampleInfo sample_info; + if (ReturnCode_t::RETCODE_OK == ret_code) { eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { subscriptions->subscribers[i] = 0; } - } else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) { + } else if (ReturnCode_t::RETCODE_OK == custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { subscriptions->subscribers[i] = 0; } } @@ -155,21 +152,15 @@ __rmw_wait( eprosima::fastdds::dds::StatusCondition & status_condition = custom_client_info->response_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition * condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition * c) - { - return c == condition; - })) + eprosima::fastdds::dds::SampleInfo sample_info; + if (ReturnCode_t::RETCODE_OK == ret_code) { eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { clients->clients[i] = 0; } - } else if (0 == custom_client_info->response_reader_->get_unread_count()) { + } else if (ReturnCode_t::RETCODE_OK == custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { clients->clients[i] = 0; } } @@ -182,21 +173,15 @@ __rmw_wait( eprosima::fastdds::dds::StatusCondition & status_condition = custom_service_info->request_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition * condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition * c) - { - return c == condition; - })) + eprosima::fastdds::dds::SampleInfo sample_info; + if (ReturnCode_t::RETCODE_OK == ret_code) { eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { services->services[i] = 0; } - } else if (0 == custom_service_info->request_reader_->get_unread_count()) { + } else if (ReturnCode_t::RETCODE_OK == custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { services->services[i] = 0; } } @@ -210,17 +195,10 @@ __rmw_wait( eprosima::fastdds::dds::StatusCondition & status_condition = custom_event_info->get_listener()->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition * condition = &status_condition; eprosima::fastdds::dds::GuardCondition * guard_condition = &custom_event_info->get_listener()->event_guard[event->event_type]; bool active = false; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition * c) - { - return c == condition; - })) + if (ReturnCode_t::RETCODE_OK == ret_code) { eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); @@ -231,13 +209,7 @@ __rmw_wait( active = true; } } - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [guard_condition](const eprosima::fastdds::dds::Condition * c) - { - return c == guard_condition; - })) + if (ReturnCode_t::RETCODE_OK == ret_code) { if (guard_condition->get_trigger_value()) { active = true; @@ -265,9 +237,7 @@ __rmw_wait( return c == condition; })) { - if (!condition->get_trigger_value()) { - guard_conditions->guard_conditions[i] = 0; - } + guard_conditions->guard_conditions[i] = 0; } else { guard_conditions->guard_conditions[i] = 0; } @@ -275,7 +245,7 @@ __rmw_wait( } } - return (no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT; + return (skip_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT; } } // namespace rmw_fastrtps_shared_cpp From ec7a5761483fe6b2429071568a25c4260d13876b Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 07:29:36 +0200 Subject: [PATCH 13/34] Modified oneliners. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../custom_publisher_info.hpp | 16 ++++++---------- .../custom_service_info.hpp | 6 ++---- .../custom_subscriber_info.hpp | 14 +++++--------- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 3 +-- 4 files changed, 14 insertions(+), 25 deletions(-) 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 3e7282fa1..38fe45be5 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 @@ -123,26 +123,22 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds private: CustomPublisherInfo * publisher_info_ = nullptr; - std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + mutable std::mutex discovery_m_; - std::mutex discovery_m_; + std::set subscriptions_ + RCPPUTILS_TSA_GUARDED_BY(discovery_m_); bool deadline_changes_; eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); bool liveliness_changes_; eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); bool incompatible_qos_changes_; eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 4d04c06e4..883a6a7ad 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 @@ -177,10 +177,8 @@ class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener private: std::mutex mutex_; - subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); - clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); + subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY(mutex_); std::condition_variable cv_; }; 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 3945c3fe2..ee9464e02 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 @@ -171,26 +171,22 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds bool deadline_changes_; eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); bool liveliness_changes_; eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); bool sample_lost_changes_; eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); bool incompatible_qos_changes_; eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + RCPPUTILS_TSA_GUARDED_BY(discovery_m_); std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + discovery_m_); rmw_event_callback_t on_new_message_cb_{nullptr}; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 89ba122ce..c03d46b36 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -438,8 +438,7 @@ struct LoanManager private: std::mutex mtx; using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY( - mtx); + ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); }; void From da3ade08f57f5406eb959004943a74f9fda1ec42 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 09:03:42 +0200 Subject: [PATCH 14/34] Cleaned more unneeded checks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 49 +++--------------------- 1 file changed, 5 insertions(+), 44 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 2e3936996..8f916a419 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -132,14 +132,7 @@ __rmw_wait( custom_subscriber_info->data_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == ret_code) - { - eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { - subscriptions->subscribers[i] = 0; - } - } else if (ReturnCode_t::RETCODE_OK == custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { subscriptions->subscribers[i] = 0; } } @@ -153,14 +146,7 @@ __rmw_wait( custom_client_info->response_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == ret_code) - { - eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { - clients->clients[i] = 0; - } - } else if (ReturnCode_t::RETCODE_OK == custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { clients->clients[i] = 0; } } @@ -174,14 +160,7 @@ __rmw_wait( custom_service_info->request_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK == ret_code) - { - eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { - services->services[i] = 0; - } - } else if (ReturnCode_t::RETCODE_OK == custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { services->services[i] = 0; } } @@ -198,17 +177,7 @@ __rmw_wait( eprosima::fastdds::dds::GuardCondition * guard_condition = &custom_event_info->get_listener()->event_guard[event->event_type]; bool active = false; - if (ReturnCode_t::RETCODE_OK == ret_code) - { - eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (changed_statuses.is_active( - rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event->event_type))) - { - active = true; - } - } + if (ReturnCode_t::RETCODE_OK == ret_code) { if (guard_condition->get_trigger_value()) { @@ -229,17 +198,9 @@ __rmw_wait( void * data = guard_conditions->guard_conditions[i]; auto condition = static_cast(data); fastdds_wait_set->detach_condition(*condition); - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if( - triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition * c) - { - return c == condition; - })) + if (!condition->get_trigger_value()) { guard_conditions->guard_conditions[i] = 0; - } else { - guard_conditions->guard_conditions[i] = 0; } condition->set_trigger_value(false); } From 850eeebef3c79478811fedaf80497217fec32620 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 09:31:39 +0200 Subject: [PATCH 15/34] Added RCPPUTILS_TSA_GUARDED_BY macros to previously atomic booleans MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../custom_publisher_info.hpp | 12 +++++++++--- .../custom_subscriber_info.hpp | 16 ++++++++++++---- 2 files changed, 21 insertions(+), 7 deletions(-) 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 38fe45be5..661279201 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 @@ -128,15 +128,21 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds std::set subscriptions_ RCPPUTILS_TSA_GUARDED_BY(discovery_m_); - bool deadline_changes_; + bool deadline_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - bool liveliness_changes_; + bool liveliness_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - bool incompatible_qos_changes_; + bool incompatible_qos_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); }; 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 ee9464e02..84dd6ff38 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 @@ -169,19 +169,27 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds private: CustomSubscriberInfo * subscriber_info_ = nullptr; - bool deadline_changes_; + bool deadline_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - bool liveliness_changes_; + bool liveliness_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - bool sample_lost_changes_; + bool sample_lost_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); - bool incompatible_qos_changes_; + bool incompatible_qos_changes_ + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); + eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ RCPPUTILS_TSA_GUARDED_BY(discovery_m_); From 5ff6c5e27bab4e6cb482f6b4429c78ce59e4e34e Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Mon, 11 Jul 2022 10:58:01 +0200 Subject: [PATCH 16/34] Fixed wrong mutex guard. Renamed and removed break; from TERMINATE_THREAD MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- .../custom_subscriber_info.hpp | 2 +- .../src/listener_thread.cpp | 24 ++++++-------- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 32 ++++++++++++------- 3 files changed, 32 insertions(+), 26 deletions(-) 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 84dd6ff38..a1260d356 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 @@ -191,7 +191,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY(discovery_m_); + RCPPUTILS_TSA_GUARDED_BY(on_new_event_m_); std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( discovery_m_); diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index f998bfaca..cde9cae51 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -105,13 +105,12 @@ rmw_fastrtps_shared_cpp::join_listener_thread( return RMW_RET_OK; } -#define TERMINATE_THREAD(msg) \ +#define LOG_THREAD_FATAL_ERROR(msg) \ { \ RCUTILS_SAFE_FWRITE_TO_STDERR( \ RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ ": ros discovery info listener thread will shutdown ...\n"); \ - break; \ } void @@ -124,12 +123,10 @@ node_listener( auto common_context = static_cast(context->impl->common); // number of conditions of a subscription is 2 rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( - context->implementation_identifier, context, 2); + context->implementation_identifier, context, 2); if (nullptr == wait_set) { - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY("failed to create waitset") \ - ": ros discovery info listener thread will shutdown ...\n"); + LOG_THREAD_FATAL_ERROR("failed to create waitset"); + return; } while (common_context->thread_is_running.load()) { assert(nullptr != common_context->sub); @@ -152,7 +149,8 @@ node_listener( wait_set, nullptr)) { - TERMINATE_THREAD("rmw_wait failed"); + LOG_THREAD_FATAL_ERROR("rmw_wait failed"); + break; } if (subscriptions_buffer[0]) { rmw_dds_common::msg::ParticipantEntitiesInfo msg; @@ -166,7 +164,8 @@ node_listener( &taken, nullptr)) { - TERMINATE_THREAD("__rmw_take failed"); + LOG_THREAD_FATAL_ERROR("__rmw_take failed"); + break; } if (taken) { if (std::memcmp( @@ -183,11 +182,8 @@ node_listener( } } if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( - context->implementation_identifier, wait_set)) + context->implementation_identifier, wait_set)) { - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY("failed to destroy waitset") \ - ": ros discovery info listener thread will shutdown ...\n"); + LOG_THREAD_FATAL_ERROR("failed to destroy waitset"); } } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 8f916a419..46dc16369 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -63,7 +63,9 @@ __rmw_wait( void * data = subscriptions->subscribers[i]; auto custom_subscriber_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - skip_wait |= (ReturnCode_t::RETCODE_OK == custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)); + skip_wait |= + (ReturnCode_t::RETCODE_OK == + custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_subscriber_info->data_reader_->get_statuscondition()); } @@ -74,7 +76,9 @@ __rmw_wait( void * data = clients->clients[i]; auto custom_client_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - skip_wait |= (ReturnCode_t::RETCODE_OK == custom_client_info->response_reader_->get_first_untaken_info(&sample_info)); + skip_wait |= + (ReturnCode_t::RETCODE_OK == + custom_client_info->response_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_client_info->response_reader_->get_statuscondition()); } @@ -85,7 +89,9 @@ __rmw_wait( void * data = services->services[i]; auto custom_service_info = static_cast(data); eprosima::fastdds::dds::SampleInfo sample_info; - skip_wait |= (ReturnCode_t::RETCODE_OK == custom_service_info->request_reader_->get_first_untaken_info(&sample_info)); + skip_wait |= + (ReturnCode_t::RETCODE_OK == + custom_service_info->request_reader_->get_first_untaken_info(&sample_info)); fastdds_wait_set->attach_condition( custom_service_info->request_reader_->get_statuscondition()); } @@ -132,7 +138,9 @@ __rmw_wait( custom_subscriber_info->data_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != + custom_subscriber_info->data_reader_->get_first_untaken_info(&sample_info)) + { subscriptions->subscribers[i] = 0; } } @@ -146,7 +154,9 @@ __rmw_wait( custom_client_info->response_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != + custom_client_info->response_reader_->get_first_untaken_info(&sample_info)) + { clients->clients[i] = 0; } } @@ -160,7 +170,9 @@ __rmw_wait( custom_service_info->request_reader_->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); eprosima::fastdds::dds::SampleInfo sample_info; - if (ReturnCode_t::RETCODE_OK != custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) { + if (ReturnCode_t::RETCODE_OK != + custom_service_info->request_reader_->get_first_untaken_info(&sample_info)) + { services->services[i] = 0; } } @@ -178,8 +190,7 @@ __rmw_wait( &custom_event_info->get_listener()->event_guard[event->event_type]; bool active = false; - if (ReturnCode_t::RETCODE_OK == ret_code) - { + if (ReturnCode_t::RETCODE_OK == ret_code) { if (guard_condition->get_trigger_value()) { active = true; guard_condition->set_trigger_value(false); @@ -198,9 +209,8 @@ __rmw_wait( void * data = guard_conditions->guard_conditions[i]; auto condition = static_cast(data); fastdds_wait_set->detach_condition(*condition); - if (!condition->get_trigger_value()) - { - guard_conditions->guard_conditions[i] = 0; + if (!condition->get_trigger_value()) { + guard_conditions->guard_conditions[i] = 0; } condition->set_trigger_value(false); } From 6735e87006ae2743e24542d0ab4ce0caaa0d04af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Wed, 13 Jul 2022 09:05:13 +0200 Subject: [PATCH 17/34] Fix waiting events MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 46dc16369..e67b6c207 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -191,6 +191,15 @@ __rmw_wait( bool active = false; if (ReturnCode_t::RETCODE_OK == ret_code) { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (changed_statuses.is_active( + rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event->event_type))) + { + active = true; + } + if (guard_condition->get_trigger_value()) { active = true; guard_condition->set_trigger_value(false); From fe61133f6760e88bd5700da7a68fe868074176da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Wed, 13 Jul 2022 10:17:46 +0200 Subject: [PATCH 18/34] Fixing crash MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../include/rmw_fastrtps_shared_cpp/custom_client_info.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/custom_service_info.hpp | 2 +- rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 61fd6fdb4..9560e80a6 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 @@ -134,7 +134,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener auto unread_responses = get_unread_responses(); if (0 < unread_responses) { - callback(user_data_, unread_responses); + callback(user_data, unread_responses); } user_data_ = user_data; 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 883a6a7ad..583c3e61f 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 @@ -233,7 +233,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener auto unread_requests = get_unread_resquests(); if (0 < unread_requests) { - callback(user_data_, unread_requests); + callback(user_data, unread_requests); } user_data_ = user_data; diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index a156b21a6..e513edfac 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -209,7 +209,7 @@ SubListener::set_on_new_message_callback( auto unread_messages = get_unread_messages(); if (0 < unread_messages) { - callback(new_message_user_data_, unread_messages); + callback(user_data, unread_messages); } new_message_user_data_ = user_data; From 67fc6503403909997e0215cd2b84488326c0d1bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Wed, 13 Jul 2022 14:03:13 +0200 Subject: [PATCH 19/34] Fix linking error on Windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_subscriber_info.hpp | 13 ++++++++++++- .../src/custom_subscriber_info.cpp | 15 --------------- 2 files changed, 12 insertions(+), 16 deletions(-) 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 a1260d356..7860081d1 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 @@ -111,7 +111,18 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds void on_data_available( - eprosima::fastdds::dds::DataReader * reader) final; + eprosima::fastdds::dds::DataReader *) final + { + std::unique_lock lock_mutex(on_new_message_m_); + + if (on_new_message_cb_) { + auto unread_messages = get_unread_messages(); + + if (0 < unread_messages) { + on_new_message_cb_(new_message_user_data_, unread_messages); + } + } + } RMW_FASTRTPS_SHARED_CPP_PUBLIC void diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index e513edfac..6fa86444e 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -237,21 +237,6 @@ size_t SubListener::get_unread_messages() return subscriber_info_->data_reader_->get_unread_count(); } -void -SubListener::on_data_available( - eprosima::fastdds::dds::DataReader *) -{ - std::unique_lock lock_mutex(on_new_message_m_); - - if (on_new_message_cb_) { - auto unread_messages = get_unread_messages(); - - if (0 < unread_messages) { - on_new_message_cb_(new_message_user_data_, unread_messages); - } - } -} - void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader *, From 50dd0aa9c7b608f2be7b3f9866e1dab1277d0369 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Thu, 14 Jul 2022 14:38:52 +0200 Subject: [PATCH 20/34] Usage of new function get_unread_count(bool) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../include/rmw_fastrtps_shared_cpp/custom_client_info.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/custom_service_info.hpp | 2 +- rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 9560e80a6..b270d7359 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 @@ -118,7 +118,7 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener size_t get_unread_responses() { - return info_->response_reader_->get_unread_count(); + return info_->response_reader_->get_unread_count(true); } // Provide handlers to perform an action when a 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 583c3e61f..e75c3f07b 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 @@ -204,7 +204,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener size_t get_unread_resquests() { - return info_->request_reader_->get_unread_count(); + return info_->request_reader_->get_unread_count(true); } void diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 6fa86444e..9588d601c 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -234,7 +234,7 @@ SubListener::set_on_new_message_callback( size_t SubListener::get_unread_messages() { - return subscriber_info_->data_reader_->get_unread_count(); + return subscriber_info_->data_reader_->get_unread_count(true); } void From 6b8946a1ebd1d436a4b73c1729ab9df3eb352740 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 15 Jul 2022 08:32:57 +0200 Subject: [PATCH 21/34] Fix windows compilation linkage error MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp | 5 ++++- rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 5 ----- 2 files changed, 4 insertions(+), 6 deletions(-) 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 7860081d1..fc77ce92b 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 @@ -161,7 +161,10 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds const void * user_data, rmw_event_callback_t callback); - size_t get_unread_messages(); + size_t get_unread_messages() + { + return subscriber_info_->data_reader_->get_unread_count(true); + } RMW_FASTRTPS_SHARED_CPP_PUBLIC eprosima::fastdds::dds::StatusCondition & get_statuscondition() const final; diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 9588d601c..4990dee12 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -232,11 +232,6 @@ SubListener::set_on_new_message_callback( } } -size_t SubListener::get_unread_messages() -{ - return subscriber_info_->data_reader_->get_unread_count(true); -} - void SubListener::on_requested_deadline_missed( eprosima::fastdds::dds::DataReader *, From cc8c3f3be5d013cecfeb7fde8c9edb7ffdae5b72 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Wed, 13 Jul 2022 09:17:41 +0200 Subject: [PATCH 22/34] Removed unneeded include. Restored some cleanup code. Added some comments. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_cpp/src/publisher.cpp | 3 ++- rmw_fastrtps_cpp/src/rmw_client.cpp | 3 ++- rmw_fastrtps_cpp/src/rmw_service.cpp | 3 ++- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 2 +- .../rmw_fastrtps_shared_cpp/custom_publisher_info.hpp | 1 - rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 5 +++++ rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 6 ++++++ 8 files changed, 19 insertions(+), 6 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 57ab05787..d6cce9c40 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -269,7 +269,7 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - // Creates DataWriter + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->data_writer_ = publisher->create_datawriter( topic.topic, writer_qos, @@ -281,6 +281,7 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } + // Set the StatusCondition to none to prevent triggering via WaitSets info->data_writer_->get_statuscondition().set_enabled_statuses( eprosima::fastdds::dds::StatusMask::none()); diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index e770c9f05..3bf12ce7b 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -379,7 +379,7 @@ rmw_create_client( return nullptr; } - // Creates DataWriter + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->request_writer_ = publisher->create_datawriter( request_topic.topic, writer_qos, @@ -391,6 +391,7 @@ rmw_create_client( return nullptr; } + // Set the StatusCondition to none to prevent triggering via WaitSets info->request_writer_->get_statuscondition().set_enabled_statuses( eprosima::fastdds::dds::StatusMask::none()); diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 4fe87afa8..eeea45859 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -382,7 +382,7 @@ rmw_create_service( return nullptr; } - // Creates DataWriter + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->response_writer_ = publisher->create_datawriter( response_topic.topic, writer_qos, @@ -394,6 +394,7 @@ rmw_create_service( return nullptr; } + // Set the StatusCondition to none to prevent triggering via WaitSets info->response_writer_->get_statuscondition().set_enabled_statuses( eprosima::fastdds::dds::StatusMask::none()); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 9ac46c332..737104fc7 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -295,7 +295,7 @@ create_subscription( return nullptr; } - // Initialize DataReader's StatusCondition to + // Initialize DataReader's StatusCondition to be notified when new data is available info->data_reader_->get_statuscondition().set_enabled_statuses( eprosima::fastdds::dds::StatusMask::data_available()); diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 447d3f51c..3a6426273 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -314,7 +314,7 @@ create_subscription( return nullptr; } - // Initialize DataReader's StatusCondition to + // Initialize DataReader's StatusCondition to be notified when new data is available info->data_reader_->get_statuscondition().set_enabled_statuses( eprosima::fastdds::dds::StatusMask::data_available()); 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 661279201..7c382e70b 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 @@ -15,7 +15,6 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ -#include #include #include diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index efd071929..dc6aec8eb 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -94,6 +94,11 @@ __rmw_destroy_client( info->response_reader_->set_listener(nullptr); } + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; + } + // Delete DataWriter ret = participant_info->publisher_->delete_datawriter(info->request_writer_); if (ret != ReturnCode_t::RETCODE_OK) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 7d27da250..056adc83c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -103,6 +103,12 @@ __rmw_destroy_service( show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datareader"); final_ret = RMW_RET_ERROR; + info->request_reader_->set_listener(nullptr); + } + + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; } // Delete DataWriter From 9e8865fe6c04979de4388b496b64019d365ae725 Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Wed, 13 Jul 2022 09:30:49 +0200 Subject: [PATCH 23/34] Set nullptr after delete MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 056adc83c..51af59baf 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -109,6 +109,7 @@ __rmw_destroy_service( // Delete DataReader listener if (nullptr != info->listener_) { delete info->listener_; + info->listener_ = nullptr; } // Delete DataWriter @@ -122,6 +123,7 @@ __rmw_destroy_service( // Delete DataWriter listener if (nullptr != info->pub_listener_) { delete info->pub_listener_; + info->pub_listener_ = nullptr; } // Delete topics and unregister types From cfea91c6bfd226ea6a24ec3eeedede1870ab919d Mon Sep 17 00:00:00 2001 From: Javier Santiago Date: Wed, 13 Jul 2022 09:38:58 +0200 Subject: [PATCH 24/34] Detach listener MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Javier Santiago Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 51af59baf..be6d2ad08 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -118,6 +118,7 @@ __rmw_destroy_service( show_previous_error(); RMW_SET_ERROR_MSG("Fail in delete datawriter"); final_ret = RMW_RET_ERROR; + info->response_writer_->set_listener(nullptr); } // Delete DataWriter listener From 07838b63415dfdd5b3dfdd328e454f6c055f99a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 15 Jul 2022 09:35:41 +0200 Subject: [PATCH 25/34] Fix creation of datareader for content filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 983a21ac2..d6df6d9dc 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -189,6 +189,11 @@ __rmw_subscription_set_content_filter( RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); return RMW_RET_ERROR; } + + // Initialize DataReader's StatusCondition to be notified when new data is available + info->data_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); + // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( [subscriber, info]() From b778478200654ad36d9bd4800962a620482b35d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Wed, 20 Jul 2022 14:15:54 +0200 Subject: [PATCH 26/34] Fix wrong usage of take_next_sample with read samples MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_client_info.hpp | 1 - .../custom_service_info.hpp | 1 - rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 27 +++--- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 23 +++-- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 90 +++++++++++++------ 5 files changed, 93 insertions(+), 49 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index b270d7359..2ae8375d4 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 @@ -72,7 +72,6 @@ typedef struct CustomClientResponse { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; std::unique_ptr buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastdds::dds::DataReaderListener 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 e75c3f07b..f0d32c3a4 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 @@ -73,7 +73,6 @@ typedef struct CustomServiceRequest { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; eprosima::fastcdr::FastBuffer * buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; CustomServiceRequest() : buffer_(nullptr) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index cb8cb02c6..46aebe6f6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -18,6 +18,7 @@ #include "fastcdr/FastBuffer.h" #include "fastdds/rtps/common/WriteParams.h" +#include "fastdds/dds/core/StackAllocatedSequence.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -100,22 +101,24 @@ __rmw_take_request( data.is_cdr_buffer = true; data.data = request.buffer_; data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->request_reader_->take_next_sample( - &data, - &request.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (request.sample_info_.valid_data) { - request.sample_identity_ = request.sample_info_.sample_identity; + + 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 (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 = - request.sample_info_.related_sample_identity.writer_guid(); + info_seq[0].related_sample_identity.writer_guid(); if (reader_guid != eprosima::fastrtps::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 = - request.sample_info_.sample_identity.writer_guid(); + info_seq[0].sample_identity.writer_guid(); info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); auto raw_type_support = dynamic_cast( @@ -132,11 +135,15 @@ __rmw_take_request( request_header->request_id.sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; - request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); + request_header->source_timestamp = info_seq[0].source_timestamp.to_ns(); + request_header->received_timestamp = info_seq[0].source_timestamp.to_ns(); *taken = true; } } + + info->request_reader_->return_loan(data_values, info_seq); + data_values.length(0); + info_seq.length(0); } delete request.buffer_; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 371f5b640..c61b143b1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -17,6 +17,7 @@ #include "fastcdr/Cdr.h" #include "fastdds/rtps/common/WriteParams.h" +#include "fastdds/dds/core/StackAllocatedSequence.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -60,12 +61,14 @@ __rmw_take_response( data.is_cdr_buffer = true; data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->response_reader_->take_next_sample( - &data, - &response.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (response.sample_info_.valid_data) { - response.sample_identity_ = response.sample_info_.related_sample_identity; + + 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 (info_seq[0].valid_data) { + response.sample_identity_ = info_seq[0].related_sample_identity; if (response.sample_identity_.writer_guid() == info->reader_guid_ || response.sample_identity_.writer_guid() == info->writer_guid_) @@ -79,8 +82,8 @@ __rmw_take_response( if (raw_type_support->deserializeROSmessage( deser, ros_response, info->response_type_support_impl_)) { - request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); + request_header->source_timestamp = info_seq[0].source_timestamp.to_ns(); + request_header->received_timestamp = info_seq[0].reception_timestamp.to_ns(); request_header->request_id.sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; @@ -89,6 +92,10 @@ __rmw_take_response( } } } + + info->response_reader_->return_loan(data_values, info_seq); + data_values.length(0); + info_seq.length(0); } return RMW_RET_OK; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index c03d46b36..48bfc1870 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -21,6 +21,7 @@ #include "rmw/rmw.h" #include "fastdds/dds/subscriber/SampleInfo.hpp" +#include "fastdds/dds/core/StackAllocatedSequence.hpp" #include "fastrtps/utils/collections/ResourceLimitedVector.hpp" @@ -83,33 +84,50 @@ _take( auto info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - eprosima::fastdds::dds::SampleInfo sinfo; - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; data.data = ros_message; data.impl = info->type_support_impl_; - while (0 < info->data_reader_->get_unread_count()) { - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - if (subscription->options.ignore_local_publications) { - auto sample_writer_guid = - eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); + eprosima::fastdds::dds::StackAllocatedSequence data_values; + const_cast(data_values.buffer())[0] = &data; + eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; - if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { - // This is a local publication. Ignore it - continue; - } + while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { + class ReturnLoan + { +public: + ReturnLoan(std::function functor) + : functor_(functor) {} + + ~ReturnLoan() {functor_();} + +private: + std::function functor_; + } return_loan( + [&]() + { + info->data_reader_->return_loan(data_values, info_seq); + data_values.length(0); + info_seq.length(0); + }); + + if (subscription->options.ignore_local_publications) { + auto sample_writer_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info_seq[0].publication_handle); + + if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { + // This is a local publication. Ignore it + continue; } + } - if (sinfo.valid_data) { - if (message_info) { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; - break; + if (info_seq[0].valid_data) { + if (message_info) { + _assign_message_info(identifier, message_info, &info_seq[0]); } + *taken = true; + break; } } @@ -144,14 +162,6 @@ _take_sequence( auto info = static_cast(subscription->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - // Limit the upper bound of reads to the number unread at the beginning. - // This prevents any samples that are added after the beginning of the - // _take_sequence call from being read. - auto unread_count = info->data_reader_->get_unread_count(); - if (unread_count < count) { - count = unread_count; - } - for (size_t ii = 0; ii < count; ++ii) { taken_flag = false; ret = _take( @@ -315,8 +325,30 @@ _take_serialized_message( data.data = &buffer; data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { - if (sinfo.valid_data) { + 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)) { + class ReturnLoan + { +public: + ReturnLoan(std::function functor) + : functor_(functor) {} + + ~ReturnLoan() {functor_();} + +private: + std::function functor_; + } return_loan( + [&]() + { + info->data_reader_->return_loan(data_values, info_seq); + data_values.length(0); + info_seq.length(0); + }); + + if (info_seq[0].valid_data) { auto buffer_size = static_cast(buffer.getBufferSize()); if (serialized_message->buffer_capacity < buffer_size) { auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); @@ -328,7 +360,7 @@ _take_serialized_message( memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); if (message_info) { - _assign_message_info(identifier, message_info, &sinfo); + _assign_message_info(identifier, message_info, &info_seq[0]); } *taken = true; } From 62336a22628ee8df8721d47f1115893f4ae65078 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Wed, 20 Jul 2022 14:54:12 +0200 Subject: [PATCH 27/34] Apply suggestions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 4 --- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 4 --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 36 ++++++++++++++------ 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 46aebe6f6..ef4e6b41b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -140,10 +140,6 @@ __rmw_take_request( *taken = true; } } - - info->request_reader_->return_loan(data_values, info_seq); - data_values.length(0); - info_seq.length(0); } delete request.buffer_; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index c61b143b1..0166b7cb6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -92,10 +92,6 @@ __rmw_take_response( } } } - - info->response_reader_->return_loan(data_values, info_seq); - data_values.length(0); - info_seq.length(0); } return RMW_RET_OK; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 48bfc1870..b894617ac 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -97,17 +97,23 @@ _take( class ReturnLoan { public: - ReturnLoan(std::function functor) - : functor_(functor) {} + ReturnLoan( + std::function functor) + : functor_(functor) + { + } - ~ReturnLoan() {functor_();} + ~ReturnLoan() + { + functor_(); + } private: std::function functor_; - } return_loan( + } + return_loan( [&]() { - info->data_reader_->return_loan(data_values, info_seq); data_values.length(0); info_seq.length(0); }); @@ -333,17 +339,23 @@ _take_serialized_message( class ReturnLoan { public: - ReturnLoan(std::function functor) - : functor_(functor) {} + ReturnLoan( + std::function functor) + : functor_(functor) + { + } - ~ReturnLoan() {functor_();} + ~ReturnLoan() + { + functor_(); + } private: std::function functor_; - } return_loan( + } + return_loan( [&]() { - info->data_reader_->return_loan(data_values, info_seq); data_values.length(0); info_seq.length(0); }); @@ -427,6 +439,7 @@ struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection // This kind of collection should only be used with loans throw std::bad_alloc(); } + }; struct LoanManager @@ -470,7 +483,8 @@ struct LoanManager private: std::mutex mtx; using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); + ItemVector items RCPPUTILS_TSA_GUARDED_BY( + mtx); }; void From 03f6b71a027f44ec01261a85ce2bb1640c4d18d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Thu, 21 Jul 2022 15:48:13 +0200 Subject: [PATCH 28/34] Apply suggestion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 40 +++--------------------- 1 file changed, 4 insertions(+), 36 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index b894617ac..df97654f3 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -37,6 +37,8 @@ #include "tracetools/tracetools.h" +#include "rcpputils/scope_exit.hpp" + namespace rmw_fastrtps_shared_cpp { @@ -94,24 +96,7 @@ _take( eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { - class ReturnLoan - { -public: - ReturnLoan( - std::function functor) - : functor_(functor) - { - } - - ~ReturnLoan() - { - functor_(); - } - -private: - std::function functor_; - } - return_loan( + auto reset = rcpputils::make_scope_exit( [&]() { data_values.length(0); @@ -336,24 +321,7 @@ _take_serialized_message( eprosima::fastdds::dds::SampleInfoSeq info_seq{1}; while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) { - class ReturnLoan - { -public: - ReturnLoan( - std::function functor) - : functor_(functor) - { - } - - ~ReturnLoan() - { - functor_(); - } - -private: - std::function functor_; - } - return_loan( + auto reset = rcpputils::make_scope_exit( [&]() { data_values.length(0); From e95e13037ffd1ee0b994f5ae0c5503a2d4cb1f93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Thu, 11 Aug 2022 13:36:34 +0200 Subject: [PATCH 29/34] Fix rosbag2 failure tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index df97654f3..0cfb7f098 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -343,6 +343,7 @@ _take_serialized_message( _assign_message_info(identifier, message_info, &info_seq[0]); } *taken = true; + break; } } From 68aacaa45f9e0426ad883460a6549bfdcb8d076b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 12 Aug 2022 09:31:45 +0200 Subject: [PATCH 30/34] Apply suggestions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_publisher_info.hpp | 2 ++ .../src/custom_publisher_info.cpp | 25 ++++++++----------- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 3 +-- 3 files changed, 13 insertions(+), 17 deletions(-) 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 7c382e70b..2c6687974 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 @@ -120,6 +120,8 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds rmw_event_callback_t callback) final; private: + void trigger_event(rmw_event_type_t event_type); + CustomPublisherInfo * publisher_info_ = nullptr; mutable std::mutex discovery_m_; diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 3801804b9..b6f9fa2fa 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -171,12 +171,7 @@ PubListener::on_offered_deadline_missed( deadline_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) { - on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED - ], 1); - } - - event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); + trigger_event(RMW_EVENT_OFFERED_DEADLINE_MISSED); } void PubListener::on_liveliness_lost( @@ -192,11 +187,7 @@ void PubListener::on_liveliness_lost( liveliness_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) { - on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); - } - - event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); + trigger_event(RMW_EVENT_LIVELINESS_LOST); } void PubListener::on_offered_incompatible_qos( @@ -213,10 +204,14 @@ void PubListener::on_offered_incompatible_qos( incompatible_qos_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) { - on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[ - RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); + trigger_event(RMW_EVENT_OFFERED_QOS_INCOMPATIBLE); +} + +void PubListener::trigger_event(rmw_event_type_t event_type) +{ + if (on_new_event_cb_[event_type]) { + on_new_event_cb_[event_type](user_data_[event_type], 1); } - event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); + event_guard[event_type].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 0cfb7f098..0626b84ca 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -452,8 +452,7 @@ struct LoanManager private: std::mutex mtx; using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY( - mtx); + ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); }; void From 09cb19b216181f45e73f1b8f008954dd88c66eb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 12 Aug 2022 11:41:20 +0200 Subject: [PATCH 31/34] Change usage of StatusMask::operator<< MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../custom_client_info.hpp | 10 ++++------ .../custom_service_info.hpp | 10 ++++------ .../src/custom_publisher_info.cpp | 10 ++++------ .../src/custom_subscriber_info.cpp | 20 ++++++++----------- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 6 +++--- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 1 - 6 files changed, 23 insertions(+), 34 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 2ae8375d4..35de2cb24 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 @@ -140,14 +140,12 @@ class ClientListener : public eprosima::fastdds::dds::DataReaderListener on_new_response_cb_ = callback; eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener( - this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + status_mask |= eprosima::fastdds::dds::StatusMask::data_available(); + info_->response_reader_->set_listener(this, status_mask); } else { eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener( - this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + status_mask &= ~eprosima::fastdds::dds::StatusMask::data_available(); + info_->response_reader_->set_listener(this, status_mask); user_data_ = nullptr; on_new_response_cb_ = nullptr; 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 f0d32c3a4..36da54c5f 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 @@ -239,14 +239,12 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener on_new_request_cb_ = callback; eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener( - this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + status_mask |= eprosima::fastdds::dds::StatusMask::data_available(); + info_->request_reader_->set_listener(this, status_mask); } else { eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener( - this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + status_mask &= ~eprosima::fastdds::dds::StatusMask::data_available(); + info_->request_reader_->set_listener(this, status_mask); user_data_ = nullptr; on_new_request_cb_ = nullptr; diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index b6f9fa2fa..2cc784513 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -142,15 +142,13 @@ void PubListener::set_on_new_event_callback( eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener( - this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + status_mask |= rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type); + publisher_info_->data_writer_->set_listener(this, status_mask); } else { eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener( - this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + status_mask &= ~rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type); + publisher_info_->data_writer_->set_listener(this, status_mask); user_data_[event_type] = nullptr; on_new_event_cb_[event_type] = nullptr; diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 4990dee12..fe8a8e8a0 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -183,15 +183,13 @@ void SubListener::set_on_new_event_callback( eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener( - this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + status_mask |= rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type); + subscriber_info_->data_reader_->set_listener(this, status_mask); } else { eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener( - this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + status_mask &= ~rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type); + subscriber_info_->data_reader_->set_listener(this, status_mask); user_data_[event_type] = nullptr; on_new_event_cb_[event_type] = nullptr; @@ -217,15 +215,13 @@ SubListener::set_on_new_message_callback( eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener( - this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + status_mask |= eprosima::fastdds::dds::StatusMask::data_available(); + subscriber_info_->data_reader_->set_listener(this, status_mask); } else { eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener( - this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + status_mask &= ~eprosima::fastdds::dds::StatusMask::data_available(); + subscriber_info_->data_reader_->set_listener(this, status_mask); new_message_user_data_ = nullptr; on_new_message_cb_ = nullptr; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 413791b6c..b9fa3bb51 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -126,10 +126,10 @@ __rmw_init_event( rmw_event->data = data; rmw_event->event_type = event_type; CustomEventInfo * event = static_cast(rmw_event->data); - eprosima::fastdds::dds::StatusMask statusmask = + eprosima::fastdds::dds::StatusMask status_mask = event->get_listener()->get_statuscondition().get_enabled_statuses(); - statusmask << internal::rmw_event_to_dds_statusmask(event_type); - event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); + status_mask |= rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type); + event->get_listener()->get_statuscondition().set_enabled_statuses(status_mask); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 0626b84ca..237ca98f3 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -408,7 +408,6 @@ struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection // This kind of collection should only be used with loans throw std::bad_alloc(); } - }; struct LoanManager From c175f3a9546a377ffab16390ee970b93ae0fb3e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 12 Aug 2022 11:53:42 +0200 Subject: [PATCH 32/34] Protecting a member MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../rmw_fastrtps_shared_cpp/custom_event_info.hpp | 6 +++++- rmw_fastrtps_shared_cpp/src/rmw_wait.cpp | 11 +++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index e6622bbd6..74da14caa 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -51,10 +51,14 @@ class EventListenerInterface const void * user_data, rmw_event_callback_t callback) = 0; + eprosima::fastdds::dds::GuardCondition & get_event_guard(rmw_event_type_t event_type) + { + return event_guard[event_type]; + } +protected: eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; -protected: rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; const void * user_data_[RMW_EVENT_INVALID] = {nullptr}; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index e67b6c207..ee541700a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -104,8 +104,7 @@ __rmw_wait( fastdds_wait_set->attach_condition( custom_event_info->get_listener()->get_statuscondition()); fastdds_wait_set->attach_condition( - custom_event_info->get_listener()->event_guard[event-> - event_type]); + custom_event_info->get_listener()->get_event_guard(event->event_type)); } } @@ -186,8 +185,8 @@ __rmw_wait( eprosima::fastdds::dds::StatusCondition & status_condition = custom_event_info->get_listener()->get_statuscondition(); fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::GuardCondition * guard_condition = - &custom_event_info->get_listener()->event_guard[event->event_type]; + eprosima::fastdds::dds::GuardCondition & guard_condition = + custom_event_info->get_listener()->get_event_guard(event->event_type); bool active = false; if (ReturnCode_t::RETCODE_OK == ret_code) { @@ -200,9 +199,9 @@ __rmw_wait( active = true; } - if (guard_condition->get_trigger_value()) { + if (guard_condition.get_trigger_value()) { active = true; - guard_condition->set_trigger_value(false); + guard_condition.set_trigger_value(false); } } From 8b1a8865f5767c233970a1604b1082a7af81a251 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 12 Aug 2022 11:56:39 +0200 Subject: [PATCH 33/34] Apply suggestions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- .../include/rmw_fastrtps_shared_cpp/custom_service_info.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 36da54c5f..4e284d0d8 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 @@ -15,8 +15,8 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ #define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ -#include #include +#include #include #include @@ -214,7 +214,7 @@ class ServiceListener : public eprosima::fastdds::dds::DataReaderListener auto unread_requests = get_unread_resquests(); - if (0 < unread_requests) { + if (0u < unread_requests) { on_new_request_cb_(user_data_, unread_requests); } } From fccdddde394c4d78c7e3a5ad65e7114e48a2f0ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= Date: Fri, 12 Aug 2022 12:01:49 +0200 Subject: [PATCH 34/34] Fix extra space MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ricardo González Moreno --- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index dc6aec8eb..fe1b74975 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -124,7 +124,7 @@ __rmw_destroy_client( rmw_free(const_cast(client->service_name)); rmw_client_free(client); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion return final_ret; }