Added incompatible event support to ros2 topic echo and ros2 topic pub (

ros2#410)

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

ros2topic/ros2topic/verb/echo.py

@@ -21,7 +21,10 @@
 import rclpy
 from rclpy.expand_topic_name import expand_topic_name
 from rclpy.node import Node
+from rclpy.qos import qos_policy_name_from_kind
 from rclpy.qos import QoSProfile
+from rclpy.qos_event import SubscriptionEventCallbacks
+from rclpy.qos_event import UnsupportedEventTypeError
 from rclpy.validate_full_topic_name import validate_full_topic_name
 from ros2cli.node.direct import DirectNode
 from ros2topic.api import add_qos_arguments_to_argument_parser
@@ -97,6 +100,11 @@ def main(args):
             node.node, args.topic_name, args.message_type, callback, qos_profile)
 
 
+def handle_incompatible_qos_event(event):
+    incompatible_qos_name = qos_policy_name_from_kind(event.last_policy_kind)
+    print(f'Incompatible QoS Policy detected: {incompatible_qos_name}')
+
+
 def subscriber(
     node: Node,
     topic_name: str,
@@ -130,8 +138,13 @@ def subscriber(
 
     msg_module = get_message(message_type)
 
-    node.create_subscription(
-        msg_module, topic_name, callback, qos_profile)
+    subscription_callbacks = SubscriptionEventCallbacks(
+        incompatible_qos=handle_incompatible_qos_event)
+    try:
+        node.create_subscription(
+            msg_module, topic_name, callback, qos_profile, event_callbacks=subscription_callbacks)
+    except UnsupportedEventTypeError:
+        node.create_subscription(msg_module, topic_name, callback, qos_profile)
 
     rclpy.spin(node)
 

ros2topic/ros2topic/verb/pub.py

@@ -18,7 +18,10 @@
 
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import qos_policy_name_from_kind
 from rclpy.qos import QoSProfile
+from rclpy.qos_event import PublisherEventCallbacks
+from rclpy.qos_event import UnsupportedEventTypeError
 from ros2cli.node.direct import DirectNode
 from ros2topic.api import add_qos_arguments_to_argument_parser
 from ros2topic.api import qos_profile_from_short_keys
@@ -91,6 +94,11 @@ def main(args):
             qos_profile)
 
 
+def handle_incompatible_qos_event(event):
+    incompatible_qos_name = qos_policy_name_from_kind(event.last_policy_kind)
+    print(f'Incompatible QoS Policy detected: {incompatible_qos_name}')
+
+
 def publisher(
     node: Node,
     message_type: MsgType,
@@ -107,7 +115,13 @@ def publisher(
     if not isinstance(values_dictionary, dict):
         return 'The passed value needs to be a dictionary in YAML format'
 
-    pub = node.create_publisher(msg_module, topic_name, qos_profile)
+    publisher_callbacks = PublisherEventCallbacks(
+        incompatible_qos=handle_incompatible_qos_event)
+    try:
+        pub = node.create_publisher(
+            msg_module, topic_name, qos_profile, event_callbacks=publisher_callbacks)
+    except UnsupportedEventTypeError:
+        pub = node.create_publisher(msg_module, topic_name, qos_profile)
 
     msg = msg_module()
     try: