WaitForTopics: let the user inject a callaback to be executed after starting the subscribers

launch_testing_ros/launch_testing_ros/wait_for_topics.py

@@ -52,10 +52,17 @@ def method_2():
             wait_for_topics.shutdown()
     """
 
-    def __init__(self, topic_tuples, timeout=5.0, messages_received_buffer_length=10):
+    def __init__(self, topic_tuples, timeout=5.0, messages_received_buffer_length=10,
+                 callback=None, callback_arguments=None):
         self.topic_tuples = topic_tuples
         self.timeout = timeout
         self.messages_received_buffer_length = messages_received_buffer_length
+        self.callback = callback
+        if self.callback is not None and not callable(self.callback):
+            raise TypeError('The passed callback is not callable')
+        self.callback_arguments = (
+            callback_arguments if callback_arguments is not None else []
+        )
         self.__ros_context = rclpy.Context()
         rclpy.init(context=self.__ros_context)
         self.__ros_executor = SingleThreadedExecutor(context=self.__ros_context)
@@ -85,6 +92,13 @@ def _prepare_ros_node(self):
 
     def wait(self):
         self.__ros_node.start_subscribers(self.topic_tuples)
+        if self.callback:
+            if isinstance(self.callback_arguments, dict):
+                self.callback(**self.callback_arguments)
+            elif isinstance(self.callback_arguments, (list, set, tuple)):
+                self.callback(*self.callback_arguments)
+            else:
+                self.callback(self.callback_arguments)
         return self.__ros_node.msg_event_object.wait(self.timeout)
 
     def shutdown(self):

launch_testing_ros/test/examples/repeater.py

@@ -0,0 +1,53 @@
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import String
+
+
+class Repeater(Node):
+
+    def __init__(self):
+        super().__init__('repeater')
+        self.count = 0
+        self.subscription = self.create_subscription(
+            String, 'input', self.callback, 10
+        )
+        self.publisher = self.create_publisher(String, 'output', 10)
+
+    def callback(self, input_msg):
+        self.get_logger().info('I heard: [%s]' % input_msg.data)
+        output_msg_data = input_msg.data
+        self.get_logger().info('Publishing: "{0}"'.format(output_msg_data))
+        self.publisher.publish(String(data=output_msg_data))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = Repeater()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

launch_testing_ros/test/examples/wait_for_topic_inject_callback.py

@@ -0,0 +1,67 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+import unittest
+
+import launch
+import launch.actions
+import launch_ros.actions
+import launch_testing.actions
+import launch_testing.markers
+from launch_testing_ros import WaitForTopics
+import pytest
+from std_msgs.msg import String
+
+
+def generate_node():
+    """Return node and remap the topic based on the index provided."""
+    path_to_test = os.path.dirname(__file__)
+    return launch_ros.actions.Node(
+        executable=sys.executable,
+        arguments=[os.path.join(path_to_test, 'repeater.py')],
+        name='demo_node',
+        additional_env={'PYTHONUNBUFFERED': '1'},
+    )
+
+
+def trigger_callback():
+    os.system('ros2 topic pub --once /input std_msgs/msg/String "data: Hello World"')
+
+
+@pytest.mark.launch_test
+@launch_testing.markers.keep_alive
+def generate_test_description():
+    description = [generate_node(), launch_testing.actions.ReadyToTest()]
+    return launch.LaunchDescription(description)
+
+
+# TODO: Test cases fail on Windows debug builds
+# https://github.com/ros2/launch_ros/issues/292
+if os.name != 'nt':
+
+    class TestFixture(unittest.TestCase):
+
+        def test_topics_successful(self):
+            """All the supplied topics should be read successfully."""
+            topic_list = [('output', String)]
+            expected_topics = {'output'}
+
+            # Method 1 : Using the magic methods and 'with' keyword
+            with WaitForTopics(
+                topic_list, timeout=2000.0, callback=trigger_callback
+            ) as wait_for_node_object_1:
+                assert wait_for_node_object_1.topics_received() == expected_topics
+                assert wait_for_node_object_1.topics_not_received() == set()

launch_testing_ros/test/examples/wait_for_topic_launch_test.py

@@ -104,3 +104,20 @@ def test_topics_unsuccessful(self, count: int):
             assert wait_for_node_object.topics_received() == expected_topics
             assert wait_for_node_object.topics_not_received() == {'invalid_topic'}
             wait_for_node_object.shutdown()
+
+        def test_callback(self, count):
+            topic_list = [('chatter_' + str(i), String) for i in range(count)]
+            expected_topics = {'chatter_' + str(i) for i in range(count)}
+
+            # Method 1 : Using the magic methods and 'with' keyword
+
+            is_callback_called = [[False]]
+
+            def callback(arg):
+                arg[0] = True
+
+            with WaitForTopics(topic_list, timeout=2.0, callback=callback,
+                               callback_arguments=is_callback_called) as wait_for_node_object_1:
+                assert wait_for_node_object_1.topics_received() == expected_topics
+                assert wait_for_node_object_1.topics_not_received() == set()
+                assert is_callback_called[0]