diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py index eacbe4378..4f46415b0 100644 --- a/ros2topic/ros2topic/verb/echo.py +++ b/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) diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 8155b5182..48aab84be 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/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: