ros2 · ivanpauno · Apr 1, 2020 · Nov 15, 2019 · Mar 4, 2020 · Mar 11, 2020
@@ -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

@@ -84,7 +84,11 @@ class Publisher : public PublisherBase
         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().
   }
 

@@ -16,8 +16,10 @@
 #define RCLCPP__QOS_EVENT_HPP_
 
 #include <functional>
+#include <string>
 
 #include "rcl/error_handling.h"
+#include "rmw/incompatible_qos_events_statuses.h"
 
 #include "rcutils/logging_macros.h"
 
@@ -32,24 +34,46 @@ 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<void (QOSDeadlineRequestedInfo &)>;
 using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
 using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
 using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
+using QOSRequestedIncompatibleQoSCallbackType =
+  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
 
 /// 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.
 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 @@ class QOSEventHandler : public QOSEventHandlerBase
     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");
       }
     }
   }

@@ -118,6 +118,11 @@ class Subscription : public SubscriptionBase
         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)) {

@@ -12,11 +12,27 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <string>
+
 #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) {

@@ -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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#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<rclcpp::Node>("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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+      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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+    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<test_msgs::msg::Empty>(
+      topic_name, 10, message_callback, options);
+  } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) {
+    EXPECT_TRUE(is_fastrtps);
+  }
+}