diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index fb80b4b..e2251a4 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -359,6 +359,16 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + ament_add_gtest(test_qos_event test/test_qos_event.cpp) + if(TARGET test_qos_event) + ament_target_dependencies(test_qos_event + "rmw" + "test_msgs" + ) + target_link_libraries(test_qos_event + ${PROJECT_NAME} + ) + endif() ament_add_gtest(test_rate test/test_rate.cpp) if(TARGET test_rate) ament_target_dependencies(test_rate diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index b017080..e7f3503 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -84,7 +84,11 @@ public: options_.event_callbacks.liveliness_callback, RCL_PUBLISHER_LIVELINESS_LOST); } - + if (options_.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options_.event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } // Setup continues in the post construction method, post_init_setup(). } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 3546151..ad011f8 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -16,8 +16,10 @@ #define RCLCPP__QOS_EVENT_HPP_ #include +#include #include "rcl/error_handling.h" +#include "rmw/incompatible_qos_events_statuses.h" #include "rcutils/logging_macros.h" @@ -32,17 +34,23 @@ using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; +using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t; +using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; using QOSDeadlineRequestedCallbackType = std::function; using QOSDeadlineOfferedCallbackType = std::function; using QOSLivelinessChangedCallbackType = std::function; using QOSLivelinessLostCallbackType = std::function; +using QOSOfferedIncompatibleQoSCallbackType = std::function; +using QOSRequestedIncompatibleQoSCallbackType = + std::function; /// Contains callbacks for various types of events a Publisher can receive from the middleware. struct PublisherEventCallbacks { QOSDeadlineOfferedCallbackType deadline_callback; QOSLivelinessLostCallbackType liveliness_callback; + QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; }; /// Contains callbacks for non-message events that a Subscription can receive from the middleware. @@ -50,6 +58,22 @@ struct SubscriptionEventCallbacks { QOSDeadlineRequestedCallbackType deadline_callback; QOSLivelinessChangedCallbackType liveliness_callback; + QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; +}; + +class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + UnsupportedEventTypeException( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix); + + RCLCPP_PUBLIC + UnsupportedEventTypeException( + const exceptions::RCLErrorBase & base_exc, + const std::string & prefix); }; class QOSEventHandlerBase : public Waitable @@ -94,9 +118,11 @@ public: rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type); if (ret != RCL_RET_OK) { if (ret == RCL_RET_UNSUPPORTED) { - rclcpp::exceptions::throw_from_rcl_error(ret, "event type is not supported"); + UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event"); + rcl_reset_error(); + throw exc; } else { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event"); } } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index c352d57..2e920e2 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -118,6 +118,11 @@ public: options.event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options.event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 75072f7..8af3918 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -12,11 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/qos_event.hpp" namespace rclcpp { +UnsupportedEventTypeException::UnsupportedEventTypeException( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: UnsupportedEventTypeException(exceptions::RCLErrorBase(ret, error_state), prefix) +{} + +UnsupportedEventTypeException::UnsupportedEventTypeException( + const exceptions::RCLErrorBase & base_exc, + const std::string & prefix) +: exceptions::RCLErrorBase(base_exc), + std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message) +{} + QOSEventHandlerBase::~QOSEventHandlerBase() { if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { diff --git a/rclcpp/test/test_qos_event.cpp b/rclcpp/test/test_qos_event.cpp new file mode 100644 index 0000000..2de87d5 --- /dev/null +++ b/rclcpp/test/test_qos_event.cpp @@ -0,0 +1,152 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "rmw/rmw.h" +#include "test_msgs/msg/empty.hpp" + +class TestQosEvent : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("test_qos_event", "/ns"); + is_fastrtps = + std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") != std::string::npos; + } + + void TearDown() + { + node.reset(); + } + + static constexpr char topic_name[] = "test_topic"; + rclcpp::Node::SharedPtr node; + bool is_fastrtps; +}; + +constexpr char TestQosEvent::topic_name[]; + +/* + Testing construction of a publishers with QoS event callback functions. + */ +TEST_F(TestQosEvent, test_publisher_constructor) +{ + rclcpp::PublisherOptions options; + + // options arg with no callbacks + auto publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness lost - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with three of the callbacks + options.event_callbacks.incompatible_qos_callback = + [node = node.get()](rclcpp::QOSOfferedIncompatibleQoSInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered incompatible qos - total %d (delta %d), last_policy_kind: %d", + event.total_count, event.total_count_change, event.last_policy_kind); + }; + try { + publisher = node->create_publisher( + topic_name, 10, options); + } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { + EXPECT_TRUE(is_fastrtps); + } +} + +/* + Testing construction of a subscriptions with QoS event callback functions. + */ +TEST_F(TestQosEvent, test_subscription_constructor) +{ + rclcpp::SubscriptionOptions options; + + auto message_callback = [node = node.get()](const test_msgs::msg::Empty::SharedPtr /*msg*/) { + RCLCPP_INFO(node->get_logger(), "Message received"); + }; + + // options arg with no callbacks + auto subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", + event.alive_count, event.alive_count_change, + event.not_alive_count, event.not_alive_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with three of the callbacks + options.event_callbacks.incompatible_qos_callback = + [node = node.get()](rclcpp::QOSRequestedIncompatibleQoSInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested incompatible qos - total %d (delta %d), last_policy_kind: %d", + event.total_count, event.total_count_change, event.last_policy_kind); + }; + try { + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { + EXPECT_TRUE(is_fastrtps); + } +}