Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (#535)

Signed-off-by: Jaison Titus <jaisontj92@gmail.com>
Signed-off-by: Miaofei <miaofei@amazon.com>
Co-authored-by: Miaofei <miaofei@amazon.com>

rcl/include/rcl/event.h

@@ -30,13 +30,15 @@ extern "C"
 typedef enum rcl_publisher_event_type_t
 {
   RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
-  RCL_PUBLISHER_LIVELINESS_LOST
+  RCL_PUBLISHER_LIVELINESS_LOST,
+  RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS,
 } rcl_publisher_event_type_t;
 
 typedef enum rcl_subscription_event_type_t
 {
   RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED,
-  RCL_SUBSCRIPTION_LIVELINESS_CHANGED
+  RCL_SUBSCRIPTION_LIVELINESS_CHANGED,
+  RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS,
 } rcl_subscription_event_type_t;
 
 /// rmw struct.

rcl/src/rcl/event.c

@@ -76,6 +76,9 @@ rcl_publisher_event_init(
     case RCL_PUBLISHER_LIVELINESS_LOST:
       rmw_event_type = RMW_EVENT_LIVELINESS_LOST;
       break;
+    case RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS:
+      rmw_event_type = RMW_EVENT_OFFERED_QOS_INCOMPATIBLE;
+      break;
     default:
       RCL_SET_ERROR_MSG("Event type for publisher not supported");
       return RCL_RET_INVALID_ARGUMENT;
@@ -116,6 +119,9 @@ rcl_subscription_event_init(
     case RCL_SUBSCRIPTION_LIVELINESS_CHANGED:
       rmw_event_type = RMW_EVENT_LIVELINESS_CHANGED;
       break;
+    case RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS:
+      rmw_event_type = RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE;
+      break;
     default:
       RCL_SET_ERROR_MSG("Event type for subscription not supported");
       return RCL_RET_INVALID_ARGUMENT;

rcl/test/rcl/test_events.cpp

@@ -14,13 +14,17 @@
 
 #include <gtest/gtest.h>
 
+#include <algorithm>
+#include <array>
 #include <chrono>
 #include <string>
 #include <thread>
+#include <tuple>
 
 #include "rcl/rcl.h"
 #include "rcl/subscription.h"
 #include "rcl/error_handling.h"
+#include "rmw/incompatible_qos_events_statuses.h"
 
 #include "test_msgs/msg/strings.h"
 #include "rosidl_generator_c/string_functions.h"
@@ -44,13 +48,22 @@ constexpr seconds MAX_WAIT_PER_TESTCASE = 10s;
 
 #define EXPECT_OK(varname) EXPECT_EQ(varname, RCL_RET_OK) << rcl_get_error_string().str
 
+struct TestIncompatibleQosEventParams
+{
+  std::string testcase_name;
+  rmw_qos_policy_kind_t qos_policy_kind;
+  rmw_qos_profile_t publisher_qos_profile;
+  rmw_qos_profile_t subscription_qos_profile;
+  std::string error_msg;
+};
 
-class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION)
+  : public ::testing::TestWithParam<TestIncompatibleQosEventParams>
 {
 public:
   void SetUp()
   {
-    is_opensplice = (std::string(rmw_get_implementation_identifier()).find("rmw_opensplice") == 0);
+    is_fastrtps = (std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0);
 
     rcl_ret_t ret;
     {
@@ -75,20 +88,12 @@ public:
     ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings);
   }
 
-  rcl_ret_t setup_publisher(
+  rcl_ret_t setup_publisher(const rmw_qos_profile_t qos_profile)
-    const rmw_time_t & deadline,
-    const rmw_time_t & lifespan,
-    const rmw_time_t & liveliness_lease_duration,
-    const rmw_qos_liveliness_policy_t liveliness_policy)
   {
     // init publisher
     publisher = rcl_get_zero_initialized_publisher();
     rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
-    publisher_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+    publisher_options.qos = qos_profile;
-    publisher_options.qos.deadline = deadline;
-    publisher_options.qos.lifespan = lifespan;
-    publisher_options.qos.liveliness = liveliness_policy;
-    publisher_options.qos.liveliness_lease_duration = liveliness_lease_duration;
     return rcl_publisher_init(
       &publisher,
       this->node_ptr,
@@ -97,21 +102,12 @@ public:
       &publisher_options);
   }
 
-  rcl_ret_t setup_subscriber(
+  rcl_ret_t setup_subscriber(const rmw_qos_profile_t qos_profile)
-    const rmw_time_t & deadline,
-    const rmw_time_t & lifespan,
-    const rmw_time_t & liveliness_lease_duration,
-    const rmw_qos_liveliness_policy_t liveliness_policy)
   {
     // init publisher
     subscription = rcl_get_zero_initialized_subscription();
     rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
-    subscription_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+    subscription_options.qos = qos_profile;
-    subscription_options.qos.deadline = deadline;
-    subscription_options.qos.lifespan = lifespan;
-    subscription_options.qos.liveliness = liveliness_policy;
-    subscription_options.qos.liveliness_lease_duration = liveliness_lease_duration;
-
     return rcl_subscription_init(
       &subscription,
       this->node_ptr,
@@ -120,35 +116,46 @@ public:
       &subscription_options);
   }
 
-  void setup_publisher_and_subscriber(
+  void setup_publisher_subscriber(
+    const rmw_qos_profile_t pub_qos_profile,
+    const rmw_qos_profile_t sub_qos_profile)
+  {
+    rcl_ret_t ret;
+
+    // init publisher
+    ret = setup_publisher(pub_qos_profile);
+    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+    // init subscription
+    ret = setup_subscriber(sub_qos_profile);
+    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
+
+  void setup_publisher_subscriber_events(
     const rcl_publisher_event_type_t & pub_event_type,
     const rcl_subscription_event_type_t & sub_event_type)
   {
     rcl_ret_t ret;
 
-    rmw_time_t lifespan {0, 0};
-    rmw_time_t deadline {DEADLINE_PERIOD_IN_S.count(), 0};
-    rmw_time_t lease_duration {LIVELINESS_LEASE_DURATION_IN_S.count(), 0};
-    rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
-
-    // init publisher
-    ret = setup_publisher(deadline, lifespan, lease_duration, liveliness_policy);
-    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
-
     // init publisher events
     publisher_event = rcl_get_zero_initialized_event();
     ret = rcl_publisher_event_init(&publisher_event, &publisher, pub_event_type);
     ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
 
-    // init subscription
-    ret = setup_subscriber(deadline, lifespan, lease_duration, liveliness_policy);
-    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
-
     // init subscription event
     subscription_event = rcl_get_zero_initialized_event();
     ret = rcl_subscription_event_init(&subscription_event, &subscription, sub_event_type);
     ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
 
+  void setup_publisher_subscriber_and_events_and_assert_discovery(
+    const rcl_publisher_event_type_t & pub_event_type,
+    const rcl_subscription_event_type_t & sub_event_type)
+  {
+    setup_publisher_subscriber(default_qos_profile, default_qos_profile);
+    setup_publisher_subscriber_events(pub_event_type, sub_event_type);
+
+    rcl_ret_t ret;
     // wait for discovery, time out after 10s
     static const size_t max_iterations = 1000;
     static const auto wait_period = 10ms;
@@ -173,16 +180,21 @@ public:
   {
     rcl_ret_t ret;
 
-    ret = rcl_event_fini(&subscription_event);
-    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
-
     ret = rcl_subscription_fini(&subscription, this->node_ptr);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
 
-    ret = rcl_event_fini(&publisher_event);
+    ret = rcl_publisher_fini(&publisher, this->node_ptr);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
+
+  void tear_down_publisher_subscriber_events()
+  {
+    rcl_ret_t ret;
+
+    ret = rcl_event_fini(&subscription_event);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
 
-    ret = rcl_publisher_fini(&publisher, this->node_ptr);
+    ret = rcl_event_fini(&publisher_event);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   }
 
@@ -202,6 +214,18 @@ public:
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   }
 
+  struct PrintToStringParamName
+  {
+    template<class ParamType>
+    std::string operator()(const ::testing::TestParamInfo<ParamType> & info) const
+    {
+      const auto & test_params = static_cast<const TestIncompatibleQosEventParams &>(info.param);
+      return test_params.testcase_name;
+    }
+  };
+
+  static const rmw_qos_profile_t default_qos_profile;
+
 protected:
   rcl_context_t * context_ptr;
   rcl_node_t * node_ptr;
@@ -210,11 +234,25 @@ protected:
   rcl_event_t publisher_event;
   rcl_subscription_t subscription;
   rcl_event_t subscription_event;
-  bool is_opensplice;
+  bool is_fastrtps;
   const char * topic = "rcl_test_publisher_subscription_events";
   const rosidl_message_type_support_t * ts;
 };
 
+using TestEventFixture = CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION);
+
+const rmw_qos_profile_t TestEventFixture::default_qos_profile = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,             // history
+  0,                                            // depth
+  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,       // reliability
+  RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,     // durability
+  {DEADLINE_PERIOD_IN_S.count(), 0},            // deadline
+  {0, 0},                                       // lifespan
+  RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,    // liveliness
+  {LIVELINESS_LEASE_DURATION_IN_S.count(), 0},  // liveliness_lease_duration
+  false                                         // avoid_ros_namespace_conventions
+};
+
 rcl_ret_t
 wait_for_msgs_and_events(
   rcl_context_t * context,
@@ -345,9 +383,9 @@ conditional_wait_for_msgs_and_events(
 /*
  * Basic test of publisher and subscriber deadline events, with first message sent before deadline
  */
-TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_missed)
+TEST_F(TestEventFixture, test_pubsub_no_deadline_missed)
 {
-  setup_publisher_and_subscriber(
+  setup_publisher_subscriber_and_events_and_assert_discovery(
     RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
     RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
   rcl_ret_t ret;
@@ -405,15 +443,16 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_
   }
 
   // clean up
+  tear_down_publisher_subscriber_events();
   tear_down_publisher_subscriber();
 }
 
 /*
  * Basic test of publisher and subscriber deadline events, with first message sent after deadline
  */
-TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_missed)
+TEST_F(TestEventFixture, test_pubsub_deadline_missed)
 {
-  setup_publisher_and_subscriber(
+  setup_publisher_subscriber_and_events_and_assert_discovery(
     RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
     RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
   rcl_ret_t ret;
@@ -479,15 +518,16 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis
   }
 
   // clean up
+  tear_down_publisher_subscriber_events();
   tear_down_publisher_subscriber();
 }
 
 /*
  * Basic test of publisher and subscriber liveliness events, with publisher killed
  */
-TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_kill_pub)
+TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub)
 {
-  setup_publisher_and_subscriber(
+  setup_publisher_subscriber_and_events_and_assert_discovery(
     RCL_PUBLISHER_LIVELINESS_LOST,
     RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
   rcl_ret_t ret;
@@ -541,13 +581,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k
     ret = rcl_take_event(&subscription_event, &liveliness_status);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
     EXPECT_EQ(liveliness_status.alive_count, 0);
-    // TODO(mm3188): Connext and OpenSplice seem to be tracking alive_count_change differently.
-    //               Issue has been raised at https://github.com/ADLINK-IST/opensplice/issues/88
-    if (is_opensplice) {
-      EXPECT_EQ(liveliness_status.alive_count_change, 2);
-    } else {
     EXPECT_EQ(liveliness_status.alive_count_change, 0);
-    }
     EXPECT_EQ(liveliness_status.not_alive_count, 1);
     EXPECT_EQ(liveliness_status.not_alive_count_change, 1);
   }
@@ -563,5 +597,163 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k
   }
 
   // clean up
+  tear_down_publisher_subscriber_events();
   tear_down_publisher_subscriber();
 }
+
+/*
+ * Basic test of publisher and subscriber incompatible qos callback events.
+ * Only implemented in opensplice at the moment.
+ */
+TEST_P(TestEventFixture, test_pubsub_incompatible_qos)
+{
+  const auto & input = GetParam();
+  const auto & qos_policy_kind = input.qos_policy_kind;
+  const auto & publisher_qos_profile = input.publisher_qos_profile;
+  const auto & subscription_qos_profile = input.subscription_qos_profile;
+  const auto & error_msg = input.error_msg;
+
+  setup_publisher_subscriber(publisher_qos_profile, subscription_qos_profile);
+  if (is_fastrtps) {
+    rcl_ret_t ret;
+
+    // init publisher events
+    publisher_event = rcl_get_zero_initialized_event();
+    ret = rcl_publisher_event_init(
+      &publisher_event,
+      &publisher,
+      RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    EXPECT_EQ(ret, RCL_RET_UNSUPPORTED);
+
+    // init subscription event
+    subscription_event = rcl_get_zero_initialized_event();
+    ret = rcl_subscription_event_init(
+      &subscription_event,
+      &subscription,
+      RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    EXPECT_EQ(ret, RCL_RET_UNSUPPORTED);
+
+    // clean up and exit test early
+    tear_down_publisher_subscriber();
+    return;
+  } else {
+    setup_publisher_subscriber_events(
+      RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS,
+      RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+  }
+
+  WaitConditionPredicate events_ready = [](
+    const bool & /*msg_persist_ready*/,
+    const bool & subscription_persist_ready,
+    const bool & publisher_persist_ready) {
+      return subscription_persist_ready && publisher_persist_ready;
+    };
+  bool msg_persist_ready, subscription_persist_ready, publisher_persist_ready;
+  rcl_ret_t wait_res = conditional_wait_for_msgs_and_events(
+    context_ptr, MAX_WAIT_PER_TESTCASE, events_ready,
+    &subscription, &subscription_event, &publisher_event,
+    &msg_persist_ready, &subscription_persist_ready, &publisher_persist_ready);
+  EXPECT_EQ(wait_res, RCL_RET_OK);
+
+  // test that the subscriber/datareader discovered an incompatible publisher/datawriter
+  EXPECT_TRUE(subscription_persist_ready);
+  if (subscription_persist_ready) {
+    rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status;
+    rcl_ret_t ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    const auto & pub_total_count = requested_incompatible_qos_status.total_count;
+    const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change;
+    const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind;
+    if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) {
+      EXPECT_EQ(pub_total_count, 1) << error_msg;
+      EXPECT_EQ(pub_total_count_change, 1) << error_msg;
+      EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg;
+    } else {
+      ADD_FAILURE() << "Subscription incompatible qos event timed out for: " << error_msg;
+    }
+  }
+
+  // test that the publisher/datawriter discovered an incompatible subscription/datareader
+  EXPECT_TRUE(publisher_persist_ready);
+  if (publisher_persist_ready) {
+    rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status;
+    rcl_ret_t ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    const auto & sub_total_count = offered_incompatible_qos_status.total_count;
+    const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change;
+    const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind;
+    if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) {
+      EXPECT_EQ(sub_total_count, 1) << error_msg;
+      EXPECT_EQ(sub_total_count_change, 1) << error_msg;
+      EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg;
+    } else {
+      ADD_FAILURE() << "Publisher incompatible qos event timed out for: " << error_msg;
+    }
+  }
+
+  // clean up
+  tear_down_publisher_subscriber_events();
+  tear_down_publisher_subscriber();
+}
+
+static
+std::array<TestIncompatibleQosEventParams, 5>
+get_test_pubsub_incompatible_qos_inputs()
+{
+  // an array of tuples that holds the expected qos_policy_kind, publisher qos profile,
+  // subscription qos profile and the error message, in that order.
+  std::array<TestIncompatibleQosEventParams, 5> inputs;
+
+  // durability
+  inputs[0].testcase_name = "IncompatibleQoS_Durability";
+  inputs[0].qos_policy_kind = RMW_QOS_POLICY_DURABILITY;
+  inputs[0].publisher_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[0].publisher_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+  inputs[0].subscription_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[0].subscription_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  inputs[0].error_msg = "Incompatible qos durability";
+
+  // deadline
+  inputs[1].testcase_name = "IncompatibleQoS_Deadline";
+  inputs[1].qos_policy_kind = RMW_QOS_POLICY_DEADLINE;
+  inputs[1].publisher_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[1].publisher_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0};
+  inputs[1].subscription_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[1].subscription_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0};
+  inputs[1].error_msg = "Incompatible qos deadline";
+
+  // liveliness
+  inputs[2].testcase_name = "IncompatibleQoS_LivelinessPolicy";
+  inputs[2].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS;
+  inputs[2].publisher_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[2].publisher_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE;
+  inputs[2].subscription_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[2].subscription_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
+  inputs[2].error_msg = "Incompatible qos liveliness policy";
+
+  // liveliness lease duration
+  inputs[3].testcase_name = "IncompatibleQoS_LivelinessLeaseDuration";
+  inputs[3].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS;
+  inputs[3].publisher_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[3].publisher_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0};
+  inputs[3].subscription_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[3].subscription_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0};
+  inputs[3].error_msg = "Incompatible qos liveliness lease duration";
+
+  // reliability
+  inputs[4].testcase_name = "IncompatibleQoS_Reliability";
+  inputs[4].qos_policy_kind = RMW_QOS_POLICY_RELIABILITY;
+  inputs[4].publisher_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[4].publisher_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  inputs[4].subscription_qos_profile = TestEventFixture::default_qos_profile;
+  inputs[4].subscription_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  inputs[4].error_msg = "Incompatible qos reliability";
+
+  return inputs;
+}
+
+INSTANTIATE_TEST_CASE_P(
+  TestPubSubIncompatibilityWithDifferentQosSettings,
+  TestEventFixture,
+  ::testing::ValuesIn(get_test_pubsub_incompatible_qos_inputs()),
+  TestEventFixture::PrintToStringParamName());