From a565fc8c360693230124e0f190077c462cc04e5d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 9 Feb 2022 21:38:44 +0000 Subject: [PATCH 01/14] Add in fake support for the MESSAGE_LOST event. Fast-DDS (and rmw_fastrtps_cpp) don't really support this right now, but having it in the list here allows RViz2 to start up. Signed-off-by: Chris Lalancette --- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 4544d3136..0b7c37972 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -23,7 +23,10 @@ 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_OFFERED_DEADLINE_MISSED, + // TODO(clalancette): This isn't really supported at the moment, but enabling + // it here allows rviz2 to start-up when using rmw_fastrtps_cpp + RMW_EVENT_MESSAGE_LOST }; namespace rmw_fastrtps_shared_cpp From 41ee781e6f6b3adba37ae6fe7a3e67cfeb06fb45 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 09:22:29 +0100 Subject: [PATCH 02/14] Add SubListener::on_sample_lost empty stub. Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp | 6 ++++++ rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 6 ++++++ 2 files changed, 12 insertions(+) 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 85a4ac540..2af71b453 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 @@ -112,6 +112,12 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds 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; + // EventListenerInterface implementation RMW_FASTRTPS_SHARED_CPP_PUBLIC bool diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 17da1fa79..b74d86682 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -64,6 +64,12 @@ void SubListener::on_liveliness_changed( liveliness_changes_.store(true, std::memory_order_relaxed); } +void SubListener::on_sample_lost( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::SampleLostStatus & status) +{ +} + bool SubListener::hasEvent(rmw_event_type_t event_type) const { assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); From 4ea34a26b3dd9737ed7cf0fb834e7036e6b9309f Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 09:30:08 +0100 Subject: [PATCH 03/14] Process data inside listener callback. Signed-off-by: Miguel Company --- .../custom_subscriber_info.hpp | 4 ++++ .../src/custom_subscriber_info.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+) 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 2af71b453..a16435d5e 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 @@ -182,6 +182,10 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds 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::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index b74d86682..79dc7155f 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -68,6 +68,18 @@ void SubListener::on_sample_lost( eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastdds::dds::SampleLostStatus & status) { + std::lock_guard lock(internalMutex_); + + // 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 + sample_lost_status_.total_count = status.total_count; + // Accumulate deltas + sample_lost_status_.total_count_change += status.total_count_change; + + sample_lost_changes_.store(true, std::memory_order_relaxed); } bool SubListener::hasEvent(rmw_event_type_t event_type) const From 657b2a5d53196b13affbce88b50f210d726fcb18 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 09:33:57 +0100 Subject: [PATCH 04/14] Add RMW_EVENT_MESSAGE_LOST to hasEvent and takeNextEvent. Signed-off-by: Miguel Company --- .../src/custom_subscriber_info.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 79dc7155f..2dffec498 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -90,6 +90,8 @@ bool SubListener::hasEvent(rmw_event_type_t event_type) const 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); default: break; } @@ -122,6 +124,15 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) 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; default: return false; } From 492cb4635f08ffa302a1d1499188c882a27059b0 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 09:35:15 +0100 Subject: [PATCH 05/14] Remove TODO on rmw_event Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 0b7c37972..91d1fdaac 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -24,8 +24,6 @@ static const std::unordered_set g_rmw_event_type_set{ RMW_EVENT_REQUESTED_DEADLINE_MISSED, RMW_EVENT_LIVELINESS_LOST, RMW_EVENT_OFFERED_DEADLINE_MISSED, - // TODO(clalancette): This isn't really supported at the moment, but enabling - // it here allows rviz2 to start-up when using rmw_fastrtps_cpp RMW_EVENT_MESSAGE_LOST }; From c1c1c5e56539e60703f47d1f84f2ebd2a252630c Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 11:00:47 +0100 Subject: [PATCH 06/14] Add SubListener::on_requested_incompatible_qos empty stub. Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp | 6 ++++++ rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp | 6 ++++++ 2 files changed, 12 insertions(+) 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 a16435d5e..37b86043a 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 @@ -118,6 +118,12 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds 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 diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 2dffec498..5265336a5 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -82,6 +82,12 @@ void SubListener::on_sample_lost( sample_lost_changes_.store(true, std::memory_order_relaxed); } +void SubListener::on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) +{ +} + bool SubListener::hasEvent(rmw_event_type_t event_type) const { assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); From 4780796785764dd1bf310263e99454ea79086607 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 11:01:48 +0100 Subject: [PATCH 07/14] Process data inside listener callback. Signed-off-by: Miguel Company --- .../custom_subscriber_info.hpp | 4 ++++ .../src/custom_subscriber_info.cpp | 13 +++++++++++++ 2 files changed, 17 insertions(+) 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 37b86043a..bf79741f5 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 @@ -192,6 +192,10 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds 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::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 5265336a5..2bb5569b8 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -86,6 +86,19 @@ void SubListener::on_requested_incompatible_qos( eprosima::fastdds::dds::DataReader * /* reader */, const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & 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); } bool SubListener::hasEvent(rmw_event_type_t event_type) const From f2b5a64d28751a23306175d5327e503e127a9171 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Thu, 10 Feb 2022 11:02:59 +0100 Subject: [PATCH 08/14] Add RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE to hasEvent and takeNextEvent. Signed-off-by: Miguel Company --- .../src/custom_subscriber_info.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 2bb5569b8..0156f245a 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -111,6 +111,8 @@ bool SubListener::hasEvent(rmw_event_type_t event_type) const 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; } @@ -152,6 +154,15 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) 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; + incompatible_qos_status_.total_count_change = 0; + incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + break; default: return false; } From 73cb116b7fc8a6cd656ab1fbc28321acfe4bccee Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 14 Feb 2022 08:15:05 +0100 Subject: [PATCH 09/14] Translate policy id. Signed-off-by: Miguel Company --- .../src/custom_subscriber_info.cpp | 4 +++ rmw_fastrtps_shared_cpp/src/event_helpers.hpp | 33 +++++++++++++++++++ rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 24 ++++++++++++++ 3 files changed, 61 insertions(+) create mode 100644 rmw_fastrtps_shared_cpp/src/event_helpers.hpp diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index 0156f245a..1703761b1 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -17,6 +17,7 @@ #include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" #include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" +#include "event_helpers.hpp" #include "types/event_types.hpp" EventListenerInterface * @@ -159,6 +160,9 @@ bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) 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); } diff --git a/rmw_fastrtps_shared_cpp/src/event_helpers.hpp b/rmw_fastrtps_shared_cpp/src/event_helpers.hpp new file mode 100644 index 000000000..e667f96ff --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/event_helpers.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EVENT_HELPERS_HPP_ +#define EVENT_HELPERS_HPP_ + +#include "fastdds/dds/core/policy/QosPolicies.hpp" + +#include "rmw/qos_policy_kind.h" + +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ + +rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( + eprosima::fastdds::dds::QosPolicyId_t policy_id); + +} // namespace internal +} // namespace rmw_fastrtps_shared_cpp + +#endif // EVENT_HELPERS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 91d1fdaac..9aae4613b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -17,6 +17,7 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "event_helpers.hpp" #include "types/event_types.hpp" static const std::unordered_set g_rmw_event_type_set{ @@ -37,6 +38,29 @@ bool is_event_supported(rmw_event_type_t event_type) 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) +{ + 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 From ec139a3acecac1ac4a08144db2e55a5b85d4e38e Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 14 Feb 2022 08:53:10 +0100 Subject: [PATCH 10/14] Add PubListener::on_offered_incompatible_qos empty stub. Signed-off-by: Miguel Company --- .../rmw_fastrtps_shared_cpp/custom_publisher_info.hpp | 5 +++++ rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp | 6 ++++++ 2 files changed, 11 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 71ae6981c..81f6420a0 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 @@ -94,6 +94,11 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds 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 diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 840d2e830..335831026 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -60,6 +60,12 @@ void PubListener::on_liveliness_lost( liveliness_changes_.store(true, std::memory_order_relaxed); } +void PubListener::on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status) +{ +} + bool PubListener::hasEvent(rmw_event_type_t event_type) const { assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); From a1f10ff71b5457883f903979c56402d2d4abaa55 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 14 Feb 2022 08:58:47 +0100 Subject: [PATCH 11/14] Process data inside listener callback. Signed-off-by: Miguel Company --- .../custom_publisher_info.hpp | 4 ++++ .../src/custom_publisher_info.cpp | 13 +++++++++++++ 2 files changed, 17 insertions(+) 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 81f6420a0..3a8cf9598 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 @@ -146,6 +146,10 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds 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::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); std::condition_variable * conditionVariable_ 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 335831026..91b5c2bae 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -64,6 +64,19 @@ void PubListener::on_offered_incompatible_qos( 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); } bool PubListener::hasEvent(rmw_event_type_t event_type) const From e4b2cdd7fe5bca579897c60b320707ef93ef307b Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 14 Feb 2022 09:01:03 +0100 Subject: [PATCH 12/14] Add RMW_EVENT_OFFERED_QOS_INCOMPATIBLE to hasEvent and takeNextEvent. Signed-off-by: Miguel Company --- .../src/custom_publisher_info.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 91b5c2bae..1a14fb98a 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -13,8 +13,11 @@ // limitations under the License. #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" + #include "fastdds/dds/core/status/BaseStatus.hpp" #include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" + +#include "event_helpers.hpp" #include "types/event_types.hpp" EventListenerInterface * @@ -87,6 +90,8 @@ bool PubListener::hasEvent(rmw_event_type_t event_type) const 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; } @@ -116,6 +121,18 @@ bool PubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) 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; } From ed84aaa5a0302dac797c912fba8e9380ed5711a4 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Mon, 14 Feb 2022 09:02:23 +0100 Subject: [PATCH 13/14] Add incompatible qos events to supported list. Signed-off-by: Miguel Company --- rmw_fastrtps_shared_cpp/src/rmw_event.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index 9aae4613b..6705ef551 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -25,7 +25,9 @@ static const std::unordered_set g_rmw_event_type_set{ RMW_EVENT_REQUESTED_DEADLINE_MISSED, RMW_EVENT_LIVELINESS_LOST, RMW_EVENT_OFFERED_DEADLINE_MISSED, - RMW_EVENT_MESSAGE_LOST + RMW_EVENT_MESSAGE_LOST, + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE }; namespace rmw_fastrtps_shared_cpp From 1248647a3069edcdb217ad56979ad62b9a4efb19 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 18 Feb 2022 12:00:43 +0100 Subject: [PATCH 14/14] Initialize added atomic_bool fields. Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp | 1 + .../include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp | 2 ++ 2 files changed, 3 insertions(+) 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 3a8cf9598..8f5676ad0 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 @@ -61,6 +61,7 @@ class PubListener : public EventListenerInterface, public eprosima::fastdds::dds explicit PubListener(CustomPublisherInfo * info) : deadline_changes_(false), liveliness_changes_(false), + incompatible_qos_changes_(false), conditionMutex_(nullptr), conditionVariable_(nullptr) { 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 bf79741f5..eddc7bac2 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 @@ -70,6 +70,8 @@ class SubListener : public EventListenerInterface, public eprosima::fastdds::dds : data_(false), deadline_changes_(false), liveliness_changes_(false), + sample_lost_changes_(false), + incompatible_qos_changes_(false), conditionMutex_(nullptr), conditionVariable_(nullptr) {