Revert "Revert "Create a default warning for qos incompatibility"" (#544

)

* Revert "Revert "Create a default warning for qos incompatibility" (#543)"

This reverts commit d1c8f4d.

Signed-off-by: Miaofei <miaofei@amazon.com>

* remove warning message from failing to register default callback

Signed-off-by: Miaofei <miaofei@amazon.com>

rclpy/rclpy/qos_event.py

@@ -22,6 +22,8 @@
 from rclpy.callback_groups import CallbackGroup
 from rclpy.handle import Handle
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.logging import get_logger
+from rclpy.qos import qos_policy_name_from_kind
 from rclpy.waitable import NumberOfEntities
 from rclpy.waitable import Waitable
 
@@ -186,6 +188,7 @@ def __init__(
         deadline: Optional[Callable[[QoSRequestedDeadlineMissedInfo], None]] = None,
         liveliness: Optional[Callable[[QoSLivelinessChangedInfo], None]] = None,
         incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None,
+        use_default_callbacks: bool = True,
     ) -> None:
         """
         Create a SubscriptionEventCallbacks container.
@@ -194,33 +197,63 @@ def __init__(
             requested Deadline.
         :param liveliness: A user-defined callback that is called when the Liveliness of
             a Publisher on subscribed topic changes.
+        :param incompatible_qos: A user-defined callback that is called when a Publisher
+            with incompatible QoS policies is discovered on subscribed topic.
+        :param use_default_callbacks: Whether or not to use default callbacks when the user
+            doesn't supply one
         """
         self.deadline = deadline
         self.liveliness = liveliness
         self.incompatible_qos = incompatible_qos
+        self.use_default_callbacks = use_default_callbacks
 
     def create_event_handlers(
         self, callback_group: CallbackGroup, subscription_handle: Handle,
     ) -> List[QoSEventHandler]:
+        with subscription_handle as subscription_capsule:
+            logger = get_logger(_rclpy.rclpy_get_subscription_logger_name(subscription_capsule))
+
         event_handlers = []
         if self.deadline:
             event_handlers.append(QoSEventHandler(
                 callback_group=callback_group,
                 callback=self.deadline,
                 event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED,
                 parent_handle=subscription_handle))
+
         if self.liveliness:
             event_handlers.append(QoSEventHandler(
                 callback_group=callback_group,
                 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))
+        elif self.use_default_callbacks:
+            # Register default callback when not specified
+            try:
+                def _default_incompatible_qos_callback(event):
+                    policy_name = qos_policy_name_from_kind(event.last_policy_kind)
+                    logger.warn(
+                        'New publisher discovered on this topic, offering incompatible QoS. '
+                        'No messages will be received from it. '
+                        'Last incompatible policy: {}'.format(policy_name))
+
+                event_type = QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS
+                event_handlers.append(QoSEventHandler(
+                    callback_group=callback_group,
+                    callback=_default_incompatible_qos_callback,
+                    event_type=event_type,
+                    parent_handle=subscription_handle))
+
+            except UnsupportedEventTypeError:
+                pass
+
         return event_handlers
 
 
@@ -232,7 +265,8 @@ def __init__(
         *,
         deadline: Optional[Callable[[QoSOfferedDeadlineMissedInfo], None]] = None,
         liveliness: Optional[Callable[[QoSLivelinessLostInfo], None]] = None,
-        incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None
+        incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None,
+        use_default_callbacks: bool = True,
     ) -> None:
         """
         Create and return a PublisherEventCallbacks container.
@@ -241,31 +275,60 @@ def __init__(
             its offered Deadline.
         :param liveliness: A user-defined callback that is called when this Publisher
             fails to signal its Liveliness and is reported as not-alive.
+        :param incompatible_qos: A user-defined callback that is called when a Subscription
+            with incompatible QoS policies is discovered on subscribed topic.
+        :param use_default_callbacks: Whether or not to use default callbacks when the user
+            doesn't supply one
         """
         self.deadline = deadline
         self.liveliness = liveliness
         self.incompatible_qos = incompatible_qos
+        self.use_default_callbacks = use_default_callbacks
 
     def create_event_handlers(
         self, callback_group: CallbackGroup, publisher_handle: Handle,
     ) -> List[QoSEventHandler]:
+        with publisher_handle as publisher_capsule:
+            logger = get_logger(_rclpy.rclpy_get_publisher_logger_name(publisher_capsule))
+
         event_handlers = []
         if self.deadline:
             event_handlers.append(QoSEventHandler(
                 callback_group=callback_group,
                 callback=self.deadline,
                 event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
                 parent_handle=publisher_handle))
+
         if self.liveliness:
             event_handlers.append(QoSEventHandler(
                 callback_group=callback_group,
                 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))
+        elif self.use_default_callbacks:
+            # Register default callback when not specified
+            try:
+                def _default_incompatible_qos_callback(event):
+                    policy_name = qos_policy_name_from_kind(event.last_policy_kind)
+                    logger.warn(
+                        'New subscription discovered on this topic, requesting incompatible QoS. '
+                        'No messages will be sent to it. '
+                        'Last incompatible policy: {}'.format(policy_name))
+
+                event_handlers.append(QoSEventHandler(
+                    callback_group=callback_group,
+                    callback=_default_incompatible_qos_callback,
+                    event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS,
+                    parent_handle=publisher_handle))
+
+            except UnsupportedEventTypeError:
+                pass
+
         return event_handlers

rclpy/src/rclpy/_rclpy.c

@@ -866,6 +866,65 @@ rclpy_get_node_logger_name(PyObject * Py_UNUSED(self), PyObject * args)
   return PyUnicode_FromString(node_logger_name);
 }
 
+/// Get the name of the logger associated with the node of the publisher.
+/**
+ * Raises ValueError if pypublisher is not a publisher capsule
+ *
+ * \param[in] pypublisher Capsule pointing to the publisher to get the logger name of
+ * \return logger_name, or
+ * \return None on failure
+ */
+static PyObject *
+rclpy_get_publisher_logger_name(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pypublisher;
+  if (!PyArg_ParseTuple(args, "O", &pypublisher)) {
+    return NULL;
+  }
+
+  rclpy_publisher_t * pub = rclpy_handle_get_pointer_from_capsule(pypublisher, "rclpy_publisher_t");
+  if (NULL == pub) {
+    return NULL;
+  }
+
+  const char * node_logger_name = rcl_node_get_logger_name(pub->node);
+  if (NULL == node_logger_name) {
+    Py_RETURN_NONE;
+  }
+
+  return PyUnicode_FromString(node_logger_name);
+}
+
+/// Get the name of the logger associated with the node of the subscription.
+/**
+ * Raises ValueError if pysubscription is not a subscription capsule
+ *
+ * \param[in] pysubscription Capsule pointing to the subscription to get the logger name of
+ * \return logger_name, or
+ * \return None on failure
+ */
+static PyObject *
+rclpy_get_subscription_logger_name(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pysubscription;
+  if (!PyArg_ParseTuple(args, "O", &pysubscription)) {
+    return NULL;
+  }
+
+  rclpy_subscription_t * sub =
+    rclpy_handle_get_pointer_from_capsule(pysubscription, "rclpy_subscription_t");
+  if (NULL == sub) {
+    return NULL;
+  }
+
+  const char * node_logger_name = rcl_node_get_logger_name(sub->node);
+  if (NULL == node_logger_name) {
+    Py_RETURN_NONE;
+  }
+
+  return PyUnicode_FromString(node_logger_name);
+}
+
 typedef rcl_ret_t (* count_func)(const rcl_node_t * node, const char * topic_name, size_t * count);
 
 static PyObject *
@@ -5218,6 +5277,14 @@ static PyMethodDef rclpy_methods[] = {
     "rclpy_get_node_logger_name", rclpy_get_node_logger_name, METH_VARARGS,
     "Get the logger name associated with a node."
   },
+  {
+    "rclpy_get_publisher_logger_name", rclpy_get_publisher_logger_name, METH_VARARGS,
+    "Get the logger name associated with the node of a publisher."
+  },
+  {
+    "rclpy_get_subscription_logger_name", rclpy_get_subscription_logger_name, METH_VARARGS,
+    "Get the logger name associated with the node of a subscription."
+  },
   {
     "rclpy_count_publishers", rclpy_count_publishers, METH_VARARGS,
     "Count publishers for a topic."