Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE…

…_QOS events (#459) Signed-off-by: Jaison Titus <jaisontj92@gmail.com> Signed-off-by: Miaofei <miaofei@amazon.com> Co-authored-by: Miaofei <miaofei@amazon.com>
ros2 · Apr 1, 2020 · 48768a4 · 48768a4
1 parent c7633ea
commit 48768a4
Show file tree

Hide file tree

Showing 5 changed files with 238 additions and 16 deletions.
diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py
@@ -20,8 +20,43 @@
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
 
+class QoSPolicyKind(IntEnum):
+    """
+    Enum for types of QoS policies that a Publisher or Subscription can set.
+
+    This enum matches the one defined in rmw/incompatible_qos_events_statuses.h
+    """
+
+    # TODO(mm3188): obtain these enum values from the rmw layer, instead of hardcoding
+    INVALID = 1 << 0
+    DURABILITY = 1 << 1
+    DEADLINE = 1 << 2
+    LIVELINESS = 1 << 3
+    RELIABILITY = 1 << 4
+    HISTORY = 1 << 5
+    LIFESPAN = 1 << 6
+
+
+def qos_policy_name_from_kind(policy_kind: QoSPolicyKind):
+    """Get QoS policy name from QoSPolicyKind enum."""
+    if policy_kind == QoSPolicyKind.DURABILITY:
+        return 'DURABILITY_QOS_POLICY'
+    elif policy_kind == QoSPolicyKind.DEADLINE:
+        return 'DEADLINE_QOS_POLICY'
+    elif policy_kind == QoSPolicyKind.LIVELINESS:
+        return 'LIVELINESS_QOS_POLICY'
+    elif policy_kind == QoSPolicyKind.RELIABILITY:
+        return 'RELIABILITY_QOS_POLICY'
+    elif policy_kind == QoSPolicyKind.HISTORY:
+        return 'HISTORY_QOS_POLICY'
+    elif policy_kind == QoSPolicyKind.LIFESPAN:
+        return 'LIFESPAN_QOS_POLICY'
+    else:
+        return 'INVALID_QOS_POLICY'
+
+
 class InvalidQoSProfileException(Exception):
-    """Raised when concstructing a QoSProfile with invalid arguments."""
+    """Raised when constructing a QoSProfile with invalid arguments."""
 
     def __init__(self, *args):
         Exception.__init__(self, 'Invalid QoSProfile', *args)

diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py
@@ -35,6 +35,7 @@ class QoSPublisherEventType(IntEnum):
 
     RCL_PUBLISHER_OFFERED_DEADLINE_MISSED = 0
     RCL_PUBLISHER_LIVELINESS_LOST = 1
+    RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS = 2
 
 
 class QoSSubscriptionEventType(IntEnum):
@@ -46,6 +47,7 @@ class QoSSubscriptionEventType(IntEnum):
 
     RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED = 0
     RCL_SUBSCRIPTION_LIVELINESS_CHANGED = 1
+    RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS = 2
 
 
 """
@@ -72,6 +74,18 @@ class QoSSubscriptionEventType(IntEnum):
         ('not_alive_count_change', 'int'),
     ])
 
+"""
+Payload type for Subscription Incompatible QoS callback.
+
+Mirrors rmw_requested_incompatible_qos_status_t from rmw/types.h
+"""
+QoSRequestedIncompatibleQoSInfo = NamedTuple(
+    'QoSRequestedIncompatibleQoSInfo', [
+        ('total_count', 'int'),
+        ('total_count_change', 'int'),
+        ('last_policy_kind', 'int'),
+    ])
+
 """
 Payload type for Publisher Deadline callback.
 
@@ -94,6 +108,17 @@ class QoSSubscriptionEventType(IntEnum):
         ('total_count_change', 'int'),
     ])
 
+"""
+Payload type for Publisher Incompatible QoS callback.
+
+Mirrors rmw_offered_incompatible_qos_status_t from rmw/types.h
+"""
+QoSOfferedIncompatibleQoSInfo = QoSRequestedIncompatibleQoSInfo
+
+
+"""Raised when registering a callback for an event type that is not supported."""
+UnsupportedEventTypeError = _rclpy.UnsupportedEventTypeError
+
 
 class QoSEventHandler(Waitable):
     """Waitable type to handle QoS events."""
@@ -160,6 +185,7 @@ def __init__(
         *,
         deadline: Optional[Callable[[QoSRequestedDeadlineMissedInfo], None]] = None,
         liveliness: Optional[Callable[[QoSLivelinessChangedInfo], None]] = None,
+        incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None,
     ) -> None:
         """
         Create a SubscriptionEventCallbacks container.
@@ -171,6 +197,7 @@ def __init__(
         """
         self.deadline = deadline
         self.liveliness = liveliness
+        self.incompatible_qos = incompatible_qos
 
     def create_event_handlers(
         self, callback_group: CallbackGroup, subscription_handle: Handle,
@@ -188,6 +215,12 @@ def create_event_handlers(
                 callback=self.liveliness,
                 event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED,
                 parent_handle=subscription_handle))
+        if self.incompatible_qos:
+            event_handlers.append(QoSEventHandler(
+                callback_group=callback_group,
+                callback=self.incompatible_qos,
+                event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS,
+                parent_handle=subscription_handle))
         return event_handlers
 
 
@@ -198,7 +231,8 @@ def __init__(
         self,
         *,
         deadline: Optional[Callable[[QoSOfferedDeadlineMissedInfo], None]] = None,
-        liveliness: Optional[Callable[[QoSLivelinessLostInfo], None]] = None
+        liveliness: Optional[Callable[[QoSLivelinessLostInfo], None]] = None,
+        incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None
     ) -> None:
         """
         Create and return a PublisherEventCallbacks container.
@@ -210,6 +244,7 @@ def __init__(
         """
         self.deadline = deadline
         self.liveliness = liveliness
+        self.incompatible_qos = incompatible_qos
 
     def create_event_handlers(
         self, callback_group: CallbackGroup, publisher_handle: Handle,
@@ -227,4 +262,10 @@ def create_event_handlers(
                 callback=self.liveliness,
                 event_type=QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST,
                 parent_handle=publisher_handle))
+        if self.incompatible_qos:
+            event_handlers.append(QoSEventHandler(
+                callback_group=callback_group,
+                callback=self.incompatible_qos,
+                event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS,
+                parent_handle=publisher_handle))
         return event_handlers
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
@@ -43,12 +43,14 @@
 
 #include "rclpy_common/common.h"
 #include "rclpy_common/handle.h"
-#include "./_rclpy_qos_event.c"
 
-static PyObject * RCLInvalidROSArgsError;
-static PyObject * UnknownROSArgsError;
 static PyObject * NodeNameNonExistentError;
 static PyObject * RCLError;
+static PyObject * RCLInvalidROSArgsError;
+static PyObject * UnknownROSArgsError;
+static PyObject * UnsupportedEventTypeError;
+
+#include "./_rclpy_qos_event.c"
 
 void
 _rclpy_context_handle_destructor(void * p)
@@ -5568,6 +5570,15 @@ PyMODINIT_FUNC PyInit__rclpy(void)
     return NULL;
   }
 
+  UnsupportedEventTypeError = PyErr_NewExceptionWithDoc(
+    "_rclpy.UnsupportedEventTypeError",
+    "Thrown when registering a callback for an event type that is not supported.",
+    RCLError, NULL);
+  if (PyModule_AddObject(m, "UnsupportedEventTypeError", UnsupportedEventTypeError)) {
+    Py_DECREF(m);
+    return NULL;
+  }
+
   if (PyErr_Occurred()) {
     Py_DECREF(m);
     return NULL;

diff --git a/rclpy/src/rclpy/_rclpy_qos_event.c b/rclpy/src/rclpy/_rclpy_qos_event.c
@@ -14,14 +14,17 @@
 
 #include "rcl/event.h"
 #include "rclpy_common/handle.h"
+#include "rmw/incompatible_qos_events_statuses.h"
 
 typedef union _qos_event_callback_data {
   // Subscription events
   rmw_requested_deadline_missed_status_t requested_deadline_missed;
   rmw_liveliness_changed_status_t liveliness_changed;
+  rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos;
   // Publisher events
   rmw_offered_deadline_missed_status_t offered_deadline_missed;
   rmw_liveliness_lost_status_t liveliness_lost;
+  rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos;
 } _qos_event_callback_data_t;
 
 typedef PyObject * (* _qos_event_data_filler_function)(_qos_event_callback_data_t *);
@@ -30,14 +33,15 @@ static
 bool
 _check_rcl_return(rcl_ret_t ret, const char * error_msg)
 {
-  if (RCL_RET_OK != ret) {
-    PyErr_Format(
-      PyExc_RuntimeError,
-      "%s: %s", error_msg, rcl_get_error_string().str);
-    rcl_reset_error();
-    return false;
+  if (RCL_RET_OK == ret) {
+    return true;
   }
-  return true;
+
+  PyObject * exception = (RCL_RET_UNSUPPORTED == ret) ? UnsupportedEventTypeError : RCLError;
+  PyErr_Format(exception, "%s: %s", error_msg, rcl_get_error_string().str);
+  rcl_reset_error();
+
+  return false;
 }
 
 static
@@ -159,6 +163,22 @@ _liveliness_changed_to_py_object(_qos_event_callback_data_t * data)
   return _create_py_qos_event("QoSLivelinessChangedInfo", args);
 }
 
+static
+PyObject *
+_requested_incompatible_qos_to_py_object(_qos_event_callback_data_t * data)
+{
+  rmw_requested_qos_incompatible_event_status_t * actual_data = &data->requested_incompatible_qos;
+  PyObject * args = Py_BuildValue(
+    "iii",
+    actual_data->total_count,
+    actual_data->total_count_change,
+    actual_data->last_policy_kind);
+  if (!args) {
+    return NULL;
+  }
+  return _create_py_qos_event("QoSRequestedIncompatibleQoSInfo", args);
+}
+
 static
 PyObject *
 _offered_deadline_missed_to_py_object(_qos_event_callback_data_t * data)
@@ -189,6 +209,22 @@ _liveliness_lost_to_py_object(_qos_event_callback_data_t * data)
   return _create_py_qos_event("QoSLivelinessLostInfo", args);
 }
 
+static
+PyObject *
+_offered_incompatible_qos_to_py_object(_qos_event_callback_data_t * data)
+{
+  rmw_offered_qos_incompatible_event_status_t * actual_data = &data->offered_incompatible_qos;
+  PyObject * args = Py_BuildValue(
+    "iii",
+    actual_data->total_count,
+    actual_data->total_count_change,
+    actual_data->last_policy_kind);
+  if (!args) {
+    return NULL;
+  }
+  return _create_py_qos_event("QoSOfferedIncompatibleQoSInfo", args);
+}
+
 static
 _qos_event_data_filler_function
 _get_qos_event_data_filler_function_for(PyObject * pyparent, unsigned PY_LONG_LONG event_type)
@@ -199,6 +235,8 @@ _get_qos_event_data_filler_function_for(PyObject * pyparent, unsigned PY_LONG_LO
         return &_requested_deadline_missed_to_py_object;
       case RCL_SUBSCRIPTION_LIVELINESS_CHANGED:
         return &_liveliness_changed_to_py_object;
+      case RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS:
+        return &_requested_incompatible_qos_to_py_object;
       default:
         PyErr_Format(
           PyExc_ValueError,
@@ -210,6 +248,8 @@ _get_qos_event_data_filler_function_for(PyObject * pyparent, unsigned PY_LONG_LO
         return &_offered_deadline_missed_to_py_object;
       case RCL_PUBLISHER_LIVELINESS_LOST:
         return &_liveliness_lost_to_py_object;
+      case RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS:
+        return &_offered_incompatible_qos_to_py_object;
       default:
         PyErr_Format(
           PyExc_ValueError,