diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 160aae1f4..108e67071 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -53,7 +53,6 @@ add_library(rmw_fastrtps_cpp src/get_service.cpp src/get_subscriber.cpp src/identifier.cpp - src/qos.cpp src/rmw_logging.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp diff --git a/rmw_fastrtps_cpp/src/qos.cpp b/rmw_fastrtps_cpp/src/qos.cpp deleted file mode 100644 index 53b2b6262..000000000 --- a/rmw_fastrtps_cpp/src/qos.cpp +++ /dev/null @@ -1,160 +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. - -#include - -#include "qos.hpp" - -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" - -#include "rmw/error_handling.h" - -extern "C" -{ -bool -get_datareader_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr) -{ - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - switch (qos_policies.reliability) { - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - sattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - sattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); - return false; - } - - switch (qos_policies.durability) { - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - sattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - sattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS durability policy"); - return false; - } - - if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { - sattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - // ensure the history depth is at least the requested queue size - assert(sattr.topic.historyQos.depth >= 0); - if ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == sattr.topic.historyQos.kind && - static_cast(sattr.topic.historyQos.depth) < qos_policies.depth) - { - if (qos_policies.depth > (std::numeric_limits::max)()) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - sattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - return true; -} - -bool -get_datawriter_qos( - const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr) -{ - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - switch (qos_policies.durability) { - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - pattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - pattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS durability policy"); - return false; - } - - switch (qos_policies.reliability) { - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - pattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - pattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); - return false; - } - - if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { - pattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - // ensure the history depth is at least the requested queue size - assert(pattr.topic.historyQos.depth >= 0); - if ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind && - static_cast(pattr.topic.historyQos.depth) < qos_policies.depth) - { - if (qos_policies.depth > (std::numeric_limits::max)()) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - pattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - return true; -} -} // extern "C" diff --git a/rmw_fastrtps_cpp/src/qos.hpp b/rmw_fastrtps_cpp/src/qos.hpp deleted file mode 100644 index c490929b6..000000000 --- a/rmw_fastrtps_cpp/src/qos.hpp +++ /dev/null @@ -1,45 +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 QOS_HPP_ -#define QOS_HPP_ - -#include "rmw/rmw.h" - -namespace eprosima -{ -namespace fastrtps -{ -class SubscriberAttributes; -class PublisherAttributes; -} // namespace fastrtps -} // namespace eprosima - -extern "C" -{ -RMW_LOCAL -bool -get_datareader_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr); - -RMW_LOCAL -bool -get_datawriter_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::PublisherAttributes & pattr); -} -// extern "C" - -#endif // QOS_HPP_ diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index b70c47ef2..74048cb90 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -22,11 +22,11 @@ #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "./qos.hpp" #include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 8e37ed129..74c6e2e07 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -57,6 +57,13 @@ rmw_destroy_node(rmw_node_t * node) eprosima_fastrtps_identifier, node); } +rmw_ret_t +rmw_node_assert_liveliness(const rmw_node_t * node) +{ + return rmw_fastrtps_shared_cpp::__rmw_node_assert_liveliness( + eprosima_fastrtps_identifier, node); +} + const rmw_guard_condition_t * rmw_node_get_graph_guard_condition(const rmw_node_t * node) { diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 873301f01..2c299f0bb 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -18,14 +18,14 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "./qos.hpp" #include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; @@ -111,6 +111,10 @@ rmw_create_publisher( eprosima::fastrtps::PublisherAttributes publisherParam; const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + // Load default XML profile. Domain::getDefaultPublisherAttributes(publisherParam); @@ -233,6 +237,13 @@ rmw_publisher_count_matched_subscriptions( publisher, subscription_count); } +rmw_ret_t +rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_assert_liveliness( + eprosima_fastrtps_identifier, publisher); +} + rmw_ret_t rmw_publisher_get_actual_qos( const rmw_publisher_t * publisher, diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 0409ff820..54b61b386 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -30,15 +30,15 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" #include "./type_support_common.hpp" -#include "./qos.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 4d92c958f..f21995001 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -19,17 +19,17 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "./qos.hpp" #include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; @@ -110,6 +110,10 @@ rmw_create_subscription( } } + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + (void)ignore_local_publications; CustomSubscriberInfo * info = nullptr; rmw_subscription_t * rmw_subscription = nullptr; diff --git a/rmw_fastrtps_cpp/src/rmw_take.cpp b/rmw_fastrtps_cpp/src/rmw_take.cpp index 228df03e8..3cf729047 100644 --- a/rmw_fastrtps_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_cpp/src/rmw_take.cpp @@ -69,4 +69,14 @@ rmw_take_serialized_message_with_info( eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info, allocation); } + +rmw_ret_t +rmw_take_event( + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_event( + eprosima_fastrtps_identifier, event_handle, event_info, taken); +} } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_wait.cpp b/rmw_fastrtps_cpp/src/rmw_wait.cpp index 5ec82be64..c58b0b9be 100644 --- a/rmw_fastrtps_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_cpp/src/rmw_wait.cpp @@ -25,10 +25,11 @@ rmw_wait( 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) { return rmw_fastrtps_shared_cpp::__rmw_wait( - subscriptions, guard_conditions, services, clients, wait_set, wait_timeout); + subscriptions, guard_conditions, services, clients, events, wait_set, wait_timeout); } } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 8551ad6f0..51f335679 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -54,7 +54,6 @@ add_library(rmw_fastrtps_dynamic_cpp src/get_service.cpp src/get_subscriber.cpp src/identifier.cpp - src/qos.cpp src/rmw_logging.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/qos.cpp b/rmw_fastrtps_dynamic_cpp/src/qos.cpp deleted file mode 100644 index 53b2b6262..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/qos.cpp +++ /dev/null @@ -1,160 +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. - -#include - -#include "qos.hpp" - -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/attributes/SubscriberAttributes.h" - -#include "rmw/error_handling.h" - -extern "C" -{ -bool -get_datareader_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr) -{ - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - switch (qos_policies.reliability) { - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - sattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - sattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); - return false; - } - - switch (qos_policies.durability) { - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - sattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - sattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS durability policy"); - return false; - } - - if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { - sattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - // ensure the history depth is at least the requested queue size - assert(sattr.topic.historyQos.depth >= 0); - if ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == sattr.topic.historyQos.kind && - static_cast(sattr.topic.historyQos.depth) < qos_policies.depth) - { - if (qos_policies.depth > (std::numeric_limits::max)()) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - sattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - return true; -} - -bool -get_datawriter_qos( - const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr) -{ - switch (qos_policies.history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; - break; - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS history policy"); - return false; - } - - switch (qos_policies.durability) { - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - pattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - pattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; - break; - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS durability policy"); - return false; - } - - switch (qos_policies.reliability) { - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - pattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - pattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - break; - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - break; - default: - RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); - return false; - } - - if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { - pattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - // ensure the history depth is at least the requested queue size - assert(pattr.topic.historyQos.depth >= 0); - if ( - eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind && - static_cast(pattr.topic.historyQos.depth) < qos_policies.depth) - { - if (qos_policies.depth > (std::numeric_limits::max)()) { - RMW_SET_ERROR_MSG( - "failed to set history depth since the requested queue size exceeds the DDS type"); - return false; - } - pattr.topic.historyQos.depth = static_cast(qos_policies.depth); - } - - return true; -} -} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/qos.hpp b/rmw_fastrtps_dynamic_cpp/src/qos.hpp deleted file mode 100644 index c490929b6..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/qos.hpp +++ /dev/null @@ -1,45 +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 QOS_HPP_ -#define QOS_HPP_ - -#include "rmw/rmw.h" - -namespace eprosima -{ -namespace fastrtps -{ -class SubscriberAttributes; -class PublisherAttributes; -} // namespace fastrtps -} // namespace eprosima - -extern "C" -{ -RMW_LOCAL -bool -get_datareader_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::SubscriberAttributes & sattr); - -RMW_LOCAL -bool -get_datawriter_qos( - const rmw_qos_profile_t & qos_policies, - eprosima::fastrtps::PublisherAttributes & pattr); -} -// extern "C" - -#endif // QOS_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 0cbf937b2..3121319eb 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -23,15 +23,17 @@ #include "rosidl_typesupport_introspection_c/identifier.h" -#include "client_service_common.hpp" -#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "qos.hpp" -#include "type_support_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +#include "client_service_common.hpp" +#include "type_support_common.hpp" + using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 9c740eb63..5bf5072ec 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -57,6 +57,13 @@ rmw_destroy_node(rmw_node_t * node) eprosima_fastrtps_identifier, node); } +rmw_ret_t +rmw_node_assert_liveliness(const rmw_node_t * node) +{ + return rmw_fastrtps_shared_cpp::__rmw_node_assert_liveliness( + eprosima_fastrtps_identifier, node); +} + const rmw_guard_condition_t * rmw_node_get_graph_guard_condition(const rmw_node_t * node) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 5eb9f967a..ea78585bc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -18,13 +18,14 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "qos.hpp" + #include "type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; @@ -105,6 +106,10 @@ rmw_create_publisher( } } + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + CustomPublisherInfo * info = nullptr; rmw_publisher_t * rmw_publisher = nullptr; eprosima::fastrtps::PublisherAttributes publisherParam; @@ -228,6 +233,13 @@ rmw_publisher_count_matched_subscriptions( publisher, subscription_count); } +rmw_ret_t +rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_assert_liveliness( + eprosima_fastrtps_identifier, publisher); +} + rmw_ret_t rmw_publisher_get_actual_qos( const rmw_publisher_t * publisher, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index f11c8b0b1..86db42797 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -35,13 +35,15 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" -#include "type_support_common.hpp" -#include "client_service_common.hpp" -#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "qos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +#include "type_support_common.hpp" +#include "client_service_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index 08e3c3e90..93e35ea03 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -19,17 +19,17 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "qos.hpp" #include "type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; @@ -110,6 +110,10 @@ rmw_create_subscription( } } + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + (void)ignore_local_publications; CustomSubscriberInfo * info = nullptr; rmw_subscription_t * rmw_subscription = nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp index 9c2ea1533..e9c936169 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp @@ -69,4 +69,14 @@ rmw_take_serialized_message_with_info( eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info, allocation); } + +rmw_ret_t +rmw_take_event( + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_event( + eprosima_fastrtps_identifier, event_handle, event_info, taken); +} } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp index 5ec82be64..c58b0b9be 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp @@ -25,10 +25,11 @@ rmw_wait( 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) { return rmw_fastrtps_shared_cpp::__rmw_wait( - subscriptions, guard_conditions, services, clients, wait_set, wait_timeout); + subscriptions, guard_conditions, services, clients, events, wait_set, wait_timeout); } } // extern "C" diff --git a/rmw_fastrtps_shared_cpp/src/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp similarity index 72% rename from rmw_fastrtps_shared_cpp/src/qos.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index c490929b6..90f77f24a 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef QOS_HPP_ -#define QOS_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #include "rmw/rmw.h" +#include "./visibility_control.h" + namespace eprosima { namespace fastrtps @@ -26,20 +28,26 @@ class PublisherAttributes; } // namespace fastrtps } // namespace eprosima -extern "C" -{ -RMW_LOCAL +RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::SubscriberAttributes & sattr); -RMW_LOCAL +RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr); -} -// extern "C" -#endif // QOS_HPP_ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +is_time_default( + const rmw_time_t & time); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +is_valid_qos( + const rmw_qos_profile_t & qos_policies); + +#endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index ee38c769b..9dfcdc689 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -20,6 +20,7 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/types.h" +#include "rmw/event.h" #include "rmw/names_and_types.h" namespace rmw_fastrtps_shared_cpp @@ -96,6 +97,12 @@ __rmw_destroy_node( const char * identifier, rmw_node_t * node); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_node_assert_liveliness( + const char * identifier, + const rmw_node_t * node); + RMW_FASTRTPS_SHARED_CPP_PUBLIC const rmw_guard_condition_t * __rmw_node_get_graph_guard_condition(const rmw_node_t * node); @@ -124,6 +131,12 @@ __rmw_publish_serialized_message( const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_assert_liveliness( + const char * identifier, + const rmw_publisher_t * publisher); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_destroy_publisher( @@ -254,6 +267,14 @@ __rmw_take( bool * taken, rmw_subscription_allocation_t * allocation); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_event( + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_take_with_info( @@ -299,6 +320,7 @@ __rmw_wait( 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); diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 53b2b6262..4228b24bb 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -14,15 +14,13 @@ #include -#include "qos.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "fastrtps/attributes/PublisherAttributes.h" #include "fastrtps/attributes/SubscriberAttributes.h" #include "rmw/error_handling.h" -extern "C" -{ bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, @@ -157,4 +155,31 @@ get_datawriter_qos( return true; } -} // extern "C" + +bool +is_time_default( + const rmw_time_t & time) +{ + return time.sec == 0 && time.nsec == 0; +} + +bool +is_valid_qos( + const rmw_qos_profile_t & qos_policies) +{ + if (!is_time_default(qos_policies.deadline)) { + RMW_SET_ERROR_MSG("Deadline unsupported for fastrtps"); + return false; + } + if (!is_time_default(qos_policies.lifespan)) { + RMW_SET_ERROR_MSG("Lifespan unsupported for fastrtps"); + return false; + } + if (qos_policies.liveliness == RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE || + qos_policies.liveliness == RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + { + RMW_SET_ERROR_MSG("Liveliness unsupported for fastrtps"); + return false; + } + return true; +} diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 89976d472..b9f77310b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -19,12 +19,12 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" -#include "qos.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index 653bf1e4c..971426432 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -22,6 +22,7 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "fastrtps/config.h" @@ -363,6 +364,34 @@ __rmw_destroy_node( return result_ret; } +rmw_ret_t +__rmw_node_assert_liveliness( + const char * identifier, + const rmw_node_t * node) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto node_info = static_cast(node->data); + if (nullptr == node_info) { + RMW_SET_ERROR_MSG("node info handle is null"); + return RMW_RET_ERROR; + } + if (nullptr == node_info->participant) { + RMW_SET_ERROR_MSG("node internal participant is invalid"); + return RMW_RET_ERROR; + } + + // node_info->participant->assert_liveliness(); + RMW_SET_ERROR_MSG("assert_liveliness() of node is currently not supported"); + + return RMW_RET_UNSUPPORTED; +} + const rmw_guard_condition_t * __rmw_node_get_graph_guard_condition(const rmw_node_t * node) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 469e8faf3..f5c15def9 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -19,13 +19,14 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" -#include "qos.hpp" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" using Domain = eprosima::fastrtps::Domain; @@ -102,6 +103,30 @@ __rmw_publisher_count_matched_subscriptions( return RMW_RET_OK; } +rmw_ret_t +__rmw_publisher_assert_liveliness( + const char * identifier, + const rmw_publisher_t * publisher) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(publisher->data); + if (nullptr == info) { + RMW_SET_ERROR_MSG("publisher internal data is invalid"); + return RMW_RET_ERROR; + } + + // info->publisher_->assert_liveliness(); + RMW_SET_ERROR_MSG("assert_liveliness() of publisher is currently not supported"); + + return RMW_RET_UNSUPPORTED; +} + rmw_ret_t __rmw_publisher_get_actual_qos( const rmw_publisher_t * publisher, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index b11fffe4d..bfed95105 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -30,10 +30,10 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" -#include "qos.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 2556b7c76..9ecb309cf 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -24,11 +24,10 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" -#include "qos.hpp" - #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index fbdfdbf38..23383f714 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -82,6 +82,29 @@ _take( return RMW_RET_OK; } +rmw_ret_t +__rmw_take_event( + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + event_handle, "event_handle pointer is null", return RMW_RET_ERROR); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + event_info, "event info output pointer is null", return RMW_RET_ERROR); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(taken, "boolean flag for taken is null", return RMW_RET_ERROR); + + *taken = false; + + if (event_handle->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("event handle not from this implementation"); + return RMW_RET_ERROR; + } + + return RMW_RET_UNSUPPORTED; +} + rmw_ret_t __rmw_take( const char * identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index 3c3c4a837..4d0f70f1b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -83,6 +83,7 @@ __rmw_wait( 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) { @@ -130,6 +131,15 @@ __rmw_wait( } } + // TODO(mm318): implement attachCondition for events when feature becomes available in fastrtps + // if (events) { + // for (size_t i = 0; i < events->event_count; ++i) { + // void * data = events->events[i]; + // auto custom_event_info = static_cast(data); + // custom_event_info->getListener()->attachCondition(conditionMutex, conditionVariable); + // } + // } + if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { void * data = guard_conditions->guard_conditions[i]; @@ -203,6 +213,18 @@ __rmw_wait( } } + // TODO(mm318): implement detachCondition for events when feature becomes available in fastrtps + // if (events) { + // for (size_t i = 0; i < events->event_count; ++i) { + // void * data = events->events[i]; + // auto custom_event_info = static_cast(data); + // custom_event_info->getListener()->detachCondition(); + // if (!custom_event_info->getListener()->hasEvent()) { + // services->services[i] = 0; + // } + // } + // } + if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { void * data = guard_conditions->guard_conditions[i];