From 8656009a748e93cf8a3a44fa6bb9b619dbd59055 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Mon, 13 Feb 2023 11:57:42 +0900 Subject: [PATCH 01/18] Get content of topic from launch_testing_ros::WaitForTopics https://github.com/ros2/launch_ros/issues/346 Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 9736e5d8..fcdd7add 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -47,6 +47,7 @@ def method_2(): print('Given topics are receiving messages !') print(wait_for_topics.topics_not_received()) # Should be an empty set print(wait_for_topics.topics_received()) # Should be {'topic_1', 'topic_2'} + print(wait_for_topics.messages_received('topic_1')) # Should be [message_1, ...] wait_for_topics.shutdown() """ @@ -87,6 +88,12 @@ def topics_not_received(self): """Topics that did not receive any messages.""" return self.__ros_node.expected_topics - self.__ros_node.received_topics + def messages_received(self, topic_name): + """List of received messages of a specific topic.""" + if topic_name not in self.__ros_node.received_messages: + raise KeyError("No message received with topic " + topic_name) + return self.__ros_node.received_messages[topic_name] + def __enter__(self): if not self.wait(): raise RuntimeError('Did not receive messages on these topics: ', @@ -108,6 +115,7 @@ def start_subscribers(self, topic_tuples): self.subscriber_list = [] self.expected_topics = {name for name, _ in topic_tuples} self.received_topics = set() + self.received_messages = {} for topic_name, topic_type in topic_tuples: # Create a subscriber @@ -126,6 +134,7 @@ def topic_callback(data): if topic_name not in self.received_topics: self.get_logger().debug('Message received for ' + topic_name) self.received_topics.add(topic_name) + self.received_messages.setdefault(topic_name, []).append(data) if self.received_topics == self.expected_topics: self.msg_event_object.set() From e6d2af3afe101bef916a43d84cdf55541a0c643c Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Wed, 22 Feb 2023 11:07:45 +0900 Subject: [PATCH 02/18] Use deque instead of list and added unit tests Signed-off-by: Pintaudi Giorgio --- .../launch_testing_ros/wait_for_topics.py | 22 ++++++++++++++----- .../examples/wait_for_topic_launch_test.py | 10 ++++++++- 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index fcdd7add..a3f74efe 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -16,6 +16,7 @@ import string from threading import Event from threading import Thread +from collections import deque import rclpy from rclpy.executors import SingleThreadedExecutor @@ -51,9 +52,10 @@ def method_2(): wait_for_topics.shutdown() """ - def __init__(self, topic_tuples, timeout=5.0): + def __init__(self, topic_tuples, timeout=5.0, max_number_of_messages=100): self.topic_tuples = topic_tuples self.timeout = timeout + self.max_number_of_messages = max_number_of_messages self.__ros_context = rclpy.Context() rclpy.init(context=self.__ros_context) self.__ros_executor = SingleThreadedExecutor(context=self.__ros_context) @@ -65,9 +67,13 @@ def __init__(self, topic_tuples, timeout=5.0): self.__ros_spin_thread.start() def _prepare_ros_node(self): - node_name = '_test_node_' +\ - ''.join(random.choices(string.ascii_uppercase + string.digits, k=10)) - self.__ros_node = _WaitForTopicsNode(name=node_name, node_context=self.__ros_context) + node_name = '_test_node_' + ''.join( + random.choices(string.ascii_uppercase + string.digits, k=10) + ) + self.__ros_node = _WaitForTopicsNode( + name=node_name, node_context=self.__ros_context, + max_number_of_messages=self.max_number_of_messages + ) self.__ros_executor.add_node(self.__ros_node) def wait(self): @@ -107,9 +113,11 @@ def __exit__(self, exep_type, exep_value, trace): class _WaitForTopicsNode(Node): """Internal node used for subscribing to a set of topics.""" - def __init__(self, name='test_node', node_context=None): + def __init__(self, name='test_node', node_context=None, + max_number_of_messages=None): super().__init__(node_name=name, context=node_context) self.msg_event_object = Event() + self.max_number_of_messages = max_number_of_messages def start_subscribers(self, topic_tuples): self.subscriber_list = [] @@ -127,6 +135,8 @@ def start_subscribers(self, topic_tuples): 10 ) ) + # Initialize ring buffer of received messages + self.received_messages[topic_name] = deque(maxlen=self.max_number_of_messages) def callback_template(self, topic_name): @@ -134,7 +144,7 @@ def topic_callback(data): if topic_name not in self.received_topics: self.get_logger().debug('Message received for ' + topic_name) self.received_topics.add(topic_name) - self.received_messages.setdefault(topic_name, []).append(data) + self.received_messages[topic_name].append(data) if self.received_topics == self.expected_topics: self.msg_event_object.set() diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index 3485843f..4241de1c 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -15,6 +15,7 @@ import os import sys import unittest +import re import launch import launch.actions @@ -57,11 +58,15 @@ def test_topics_successful(self, count): """All the supplied topics should be read successfully.""" topic_list = [('chatter_' + str(i), String) for i in range(count)] expected_topics = {'chatter_' + str(i) for i in range(count)} + message_pattern = re.compile('Hello World: \d+') # Method 1 : Using the magic methods and 'with' keyword - with WaitForTopics(topic_list, timeout=10.0) as wait_for_node_object_1: + with WaitForTopics(topic_list, timeout=2.0, max_number_of_messages=10) 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() + for topic_name, _ in topic_list: + message = wait_for_node_object_1.messages_received(topic_name).pop().data + assert message_pattern.match(message) # Multiple instances of WaitForNode() can be created safely as # their internal nodes spin in separate contexts @@ -70,6 +75,9 @@ def test_topics_successful(self, count): assert wait_for_node_object_2.wait() assert wait_for_node_object_2.topics_received() == expected_topics assert wait_for_node_object_2.topics_not_received() == set() + for topic_name, _ in topic_list: + message = wait_for_node_object_2.messages_received(topic_name).pop().data + assert message_pattern.match(message) wait_for_node_object_2.shutdown() def test_topics_unsuccessful(self, count): From 3ad48ec59e8e17d8e6576abf2fd00ed2037a1763 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Tue, 28 Feb 2023 15:54:52 +0900 Subject: [PATCH 03/18] return a list to not expose implementation details (deque) Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index a3f74efe..1db98d56 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -98,7 +98,7 @@ def messages_received(self, topic_name): """List of received messages of a specific topic.""" if topic_name not in self.__ros_node.received_messages: raise KeyError("No message received with topic " + topic_name) - return self.__ros_node.received_messages[topic_name] + return list(self.__ros_node.received_messages[topic_name]) def __enter__(self): if not self.wait(): From 41643b630c939e4c27a97178fc25a95bed37a5fe Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Wed, 1 Mar 2023 10:44:32 +0900 Subject: [PATCH 04/18] Fixed flake8 errors Signed-off-by: Pintaudi Giorgio --- .../launch_testing_ros/wait_for_topics.py | 2 +- .../test/examples/wait_for_topic_launch_test.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 1db98d56..c2f037f5 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -14,9 +14,9 @@ import random import string +from collections import deque from threading import Event from threading import Thread -from collections import deque import rclpy from rclpy.executors import SingleThreadedExecutor diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index 4241de1c..a5a63315 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -13,19 +13,20 @@ # limitations under the License. import os +import re import sys import unittest -import re 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 +from launch_testing_ros import WaitForTopics + def generate_node(i): """Return node and remap the topic based on the index provided.""" @@ -58,10 +59,11 @@ def test_topics_successful(self, count): """All the supplied topics should be read successfully.""" topic_list = [('chatter_' + str(i), String) for i in range(count)] expected_topics = {'chatter_' + str(i) for i in range(count)} - message_pattern = re.compile('Hello World: \d+') + message_pattern = re.compile(r'Hello World: \d+') # Method 1 : Using the magic methods and 'with' keyword - with WaitForTopics(topic_list, timeout=2.0, max_number_of_messages=10) as wait_for_node_object_1: + with WaitForTopics(topic_list, timeout=2.0, + max_number_of_messages=10) 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() for topic_name, _ in topic_list: From 9f214fc22985b4447be97586471cf9e67f595bc5 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Thu, 2 Mar 2023 11:01:15 +0900 Subject: [PATCH 05/18] WaitForTopic: better naming of variables and methods Signed-off-by: Pintaudi Giorgio --- .../launch_testing_ros/wait_for_topics.py | 18 +++++++++--------- .../examples/wait_for_topic_launch_test.py | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index c2f037f5..6c966479 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -52,10 +52,10 @@ def method_2(): wait_for_topics.shutdown() """ - def __init__(self, topic_tuples, timeout=5.0, max_number_of_messages=100): + def __init__(self, topic_tuples, timeout=5.0, messages_received_buffer_length=10): self.topic_tuples = topic_tuples self.timeout = timeout - self.max_number_of_messages = max_number_of_messages + self.messages_received_buffer_length = messages_received_buffer_length self.__ros_context = rclpy.Context() rclpy.init(context=self.__ros_context) self.__ros_executor = SingleThreadedExecutor(context=self.__ros_context) @@ -72,7 +72,7 @@ def _prepare_ros_node(self): ) self.__ros_node = _WaitForTopicsNode( name=node_name, node_context=self.__ros_context, - max_number_of_messages=self.max_number_of_messages + max_number_of_messages=self.messages_received_buffer_length ) self.__ros_executor.add_node(self.__ros_node) @@ -94,11 +94,11 @@ def topics_not_received(self): """Topics that did not receive any messages.""" return self.__ros_node.expected_topics - self.__ros_node.received_topics - def messages_received(self, topic_name): + def received_messages(self, topic_name): """List of received messages of a specific topic.""" - if topic_name not in self.__ros_node.received_messages: + if topic_name not in self.__ros_node.received_messages_buffer: raise KeyError("No message received with topic " + topic_name) - return list(self.__ros_node.received_messages[topic_name]) + return list(self.__ros_node.received_messages_buffer[topic_name]) def __enter__(self): if not self.wait(): @@ -123,7 +123,7 @@ def start_subscribers(self, topic_tuples): self.subscriber_list = [] self.expected_topics = {name for name, _ in topic_tuples} self.received_topics = set() - self.received_messages = {} + self.received_messages_buffer = {} for topic_name, topic_type in topic_tuples: # Create a subscriber @@ -136,7 +136,7 @@ def start_subscribers(self, topic_tuples): ) ) # Initialize ring buffer of received messages - self.received_messages[topic_name] = deque(maxlen=self.max_number_of_messages) + self.received_messages_buffer[topic_name] = deque(maxlen=self.max_number_of_messages) def callback_template(self, topic_name): @@ -144,7 +144,7 @@ def topic_callback(data): if topic_name not in self.received_topics: self.get_logger().debug('Message received for ' + topic_name) self.received_topics.add(topic_name) - self.received_messages[topic_name].append(data) + self.received_messages_buffer[topic_name].append(data) if self.received_topics == self.expected_topics: self.msg_event_object.set() diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index a5a63315..b15afbc7 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -67,7 +67,7 @@ def test_topics_successful(self, count): assert wait_for_node_object_1.topics_received() == expected_topics assert wait_for_node_object_1.topics_not_received() == set() for topic_name, _ in topic_list: - message = wait_for_node_object_1.messages_received(topic_name).pop().data + message = wait_for_node_object_1.received_messages(topic_name).pop().data assert message_pattern.match(message) # Multiple instances of WaitForNode() can be created safely as @@ -78,7 +78,7 @@ def test_topics_successful(self, count): assert wait_for_node_object_2.topics_received() == expected_topics assert wait_for_node_object_2.topics_not_received() == set() for topic_name, _ in topic_list: - message = wait_for_node_object_2.messages_received(topic_name).pop().data + message = wait_for_node_object_2.received_messages(topic_name).pop().data assert message_pattern.match(message) wait_for_node_object_2.shutdown() From 5b015fc0626414a71f5534530a1b30df72d164f1 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Thu, 2 Mar 2023 11:07:26 +0900 Subject: [PATCH 06/18] WaitForTopic: better naming of variables and methods (bugfix) Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/test/examples/wait_for_topic_launch_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index b15afbc7..770a3620 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -63,7 +63,7 @@ def test_topics_successful(self, count): # Method 1 : Using the magic methods and 'with' keyword with WaitForTopics(topic_list, timeout=2.0, - max_number_of_messages=10) as wait_for_node_object_1: + messages_received_buffer_length=10) 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() for topic_name, _ in topic_list: From de7266c83e6868b1ec1908c1de847fdcf7c61ab8 Mon Sep 17 00:00:00 2001 From: Giorgio Pintaudi Date: Fri, 10 Mar 2023 08:09:02 +0100 Subject: [PATCH 07/18] Update launch_testing_ros/launch_testing_ros/wait_for_topics.py Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 6c966479..65350bb8 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -71,8 +71,9 @@ def _prepare_ros_node(self): random.choices(string.ascii_uppercase + string.digits, k=10) ) self.__ros_node = _WaitForTopicsNode( - name=node_name, node_context=self.__ros_context, - max_number_of_messages=self.messages_received_buffer_length + name=node_name, + node_context=self.__ros_context, + max_number_of_messages=self.messages_received_buffer_length, ) self.__ros_executor.add_node(self.__ros_node) From 595a1c796679615bbd394f42cbce852264a89fc0 Mon Sep 17 00:00:00 2001 From: Giorgio Pintaudi Date: Fri, 10 Mar 2023 08:09:42 +0100 Subject: [PATCH 08/18] Update launch_testing_ros/launch_testing_ros/wait_for_topics.py Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 65350bb8..74bd50be 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -114,8 +114,11 @@ def __exit__(self, exep_type, exep_value, trace): class _WaitForTopicsNode(Node): """Internal node used for subscribing to a set of topics.""" - def __init__(self, name='test_node', node_context=None, - max_number_of_messages=None): + def __init__(self, + name='test_node', + node_context=None, + max_number_of_messages=None, + ): super().__init__(node_name=name, context=node_context) self.msg_event_object = Event() self.max_number_of_messages = max_number_of_messages From 64a4dcfeb6708a83cb4e5fac6b560c6538d036e1 Mon Sep 17 00:00:00 2001 From: Giorgio Pintaudi Date: Fri, 10 Mar 2023 08:10:29 +0100 Subject: [PATCH 09/18] Update launch_testing_ros/test/examples/wait_for_topic_launch_test.py Signed-off-by: Pintaudi Giorgio --- .../test/examples/wait_for_topic_launch_test.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index 770a3620..ff92cb5d 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -62,8 +62,9 @@ def test_topics_successful(self, count): message_pattern = re.compile(r'Hello World: \d+') # Method 1 : Using the magic methods and 'with' keyword - with WaitForTopics(topic_list, timeout=2.0, - messages_received_buffer_length=10) as wait_for_node_object_1: + with WaitForTopics( + topic_list, timeout=2.0, messages_received_buffer_length=10 + ) 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() for topic_name, _ in topic_list: From a9a56bdbc3eb8138490523e3f912cecbbaeb87f8 Mon Sep 17 00:00:00 2001 From: Giorgio Pintaudi Date: Fri, 10 Mar 2023 08:13:14 +0100 Subject: [PATCH 10/18] Update launch_testing_ros/launch_testing_ros/wait_for_topics.py Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 74bd50be..8b45debb 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -98,7 +98,7 @@ def topics_not_received(self): def received_messages(self, topic_name): """List of received messages of a specific topic.""" if topic_name not in self.__ros_node.received_messages_buffer: - raise KeyError("No message received with topic " + topic_name) + raise KeyError("No messages received for topic: " + topic_name) return list(self.__ros_node.received_messages_buffer[topic_name]) def __enter__(self): From e1a7013b48d01c47a08ba3a4760b791d883ad9b2 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Mon, 13 Mar 2023 11:41:01 +0900 Subject: [PATCH 11/18] Bugfix: save all incoming messages and not only the first Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 8b45debb..d8ee37e3 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -145,10 +145,10 @@ def start_subscribers(self, topic_tuples): def callback_template(self, topic_name): def topic_callback(data): + self.get_logger().debug('Message received for ' + topic_name) + self.received_messages_buffer[topic_name].append(data) if topic_name not in self.received_topics: - self.get_logger().debug('Message received for ' + topic_name) self.received_topics.add(topic_name) - self.received_messages_buffer[topic_name].append(data) if self.received_topics == self.expected_topics: self.msg_event_object.set() From a84047cb98401c2243d3abf46a07a5b71b3799fc Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Mon, 13 Mar 2023 11:46:34 +0900 Subject: [PATCH 12/18] check the buffer length Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/test/examples/wait_for_topic_launch_test.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index ff92cb5d..d6130521 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -68,6 +68,7 @@ def test_topics_successful(self, count): assert wait_for_node_object_1.topics_received() == expected_topics assert wait_for_node_object_1.topics_not_received() == set() for topic_name, _ in topic_list: + assert len(wait_for_node_object_1.received_messages(topic_name)) >= 1 message = wait_for_node_object_1.received_messages(topic_name).pop().data assert message_pattern.match(message) @@ -79,6 +80,7 @@ def test_topics_successful(self, count): assert wait_for_node_object_2.topics_received() == expected_topics assert wait_for_node_object_2.topics_not_received() == set() for topic_name, _ in topic_list: + assert len(wait_for_node_object_1.received_messages(topic_name)) >= 1 message = wait_for_node_object_2.received_messages(topic_name).pop().data assert message_pattern.match(message) wait_for_node_object_2.shutdown() From 1cd227b9695f53b930f46f3186845f04072f86ac Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Wed, 15 Mar 2023 10:04:12 +0900 Subject: [PATCH 13/18] Do not recreate the same subscribers when recalling the wait method Signed-off-by: Pintaudi Giorgio --- .../launch_testing_ros/wait_for_topics.py | 52 +++++++++++-------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index d8ee37e3..912b8c0c 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -114,39 +114,47 @@ def __exit__(self, exep_type, exep_value, trace): class _WaitForTopicsNode(Node): """Internal node used for subscribing to a set of topics.""" - def __init__(self, - name='test_node', - node_context=None, - max_number_of_messages=None, + def __init__( + self, name="test_node", node_context=None, max_number_of_messages=None ): - super().__init__(node_name=name, context=node_context) + super().__init__(node_name=name, context=node_context) # type: ignore self.msg_event_object = Event() self.max_number_of_messages = max_number_of_messages - - def start_subscribers(self, topic_tuples): self.subscriber_list = [] - self.expected_topics = {name for name, _ in topic_tuples} + self.topic_tuples = [] + self.expected_topics = set() + self.received_topics = set() + self.received_messages = {} + + def _reset(self): self.received_topics = set() - self.received_messages_buffer = {} + for buffer in self.received_messages.values(): + buffer.clear() + def start_subscribers(self, topic_tuples): + self._reset() for topic_name, topic_type in topic_tuples: - # Create a subscriber - self.subscriber_list.append( - self.create_subscription( - topic_type, - topic_name, - self.callback_template(topic_name), - 10 + if (topic_name, topic_type) not in self.topic_tuples: + self.topic_tuples.append((topic_name, topic_type)) + self.expected_topics.add(topic_name) + # Initialize ring buffer of received messages + self.received_messages[topic_name] = deque( + maxlen=self.max_number_of_messages + ) + # Create a subscriber + self.subscriber_list.append( + self.create_subscription( + topic_type, + topic_name, + self.callback_template(topic_name), + 10 + ) ) - ) - # Initialize ring buffer of received messages - self.received_messages_buffer[topic_name] = deque(maxlen=self.max_number_of_messages) def callback_template(self, topic_name): - def topic_callback(data): - self.get_logger().debug('Message received for ' + topic_name) - self.received_messages_buffer[topic_name].append(data) + self.get_logger().debug("Message received for " + topic_name) + self.received_messages[topic_name].append(data) if topic_name not in self.received_topics: self.received_topics.add(topic_name) if self.received_topics == self.expected_topics: From ad4557ef63d8f17421e030ab37bc05ec44114ddc Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Wed, 15 Mar 2023 15:24:18 +0900 Subject: [PATCH 14/18] Clear msg_event_object too Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 1 + 1 file changed, 1 insertion(+) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 912b8c0c..23504788 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -127,6 +127,7 @@ def __init__( self.received_messages = {} def _reset(self): + self.msg_event_object.clear() self.received_topics = set() for buffer in self.received_messages.values(): buffer.clear() From 9d3c5099d289c46212976e5ac09073cb2e86cc67 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Thu, 25 May 2023 13:16:16 +0900 Subject: [PATCH 15/18] Forgot to rename received_messages into received_messages_buffer Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 23504788..496623ee 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -124,12 +124,12 @@ def __init__( self.topic_tuples = [] self.expected_topics = set() self.received_topics = set() - self.received_messages = {} + self.received_messages_buffer = {} def _reset(self): self.msg_event_object.clear() self.received_topics = set() - for buffer in self.received_messages.values(): + for buffer in self.received_messages_buffer.values(): buffer.clear() def start_subscribers(self, topic_tuples): @@ -139,7 +139,7 @@ def start_subscribers(self, topic_tuples): self.topic_tuples.append((topic_name, topic_type)) self.expected_topics.add(topic_name) # Initialize ring buffer of received messages - self.received_messages[topic_name] = deque( + self.received_messages_buffer[topic_name] = deque( maxlen=self.max_number_of_messages ) # Create a subscriber @@ -155,7 +155,7 @@ def start_subscribers(self, topic_tuples): def callback_template(self, topic_name): def topic_callback(data): self.get_logger().debug("Message received for " + topic_name) - self.received_messages[topic_name].append(data) + self.received_messages_buffer[topic_name].append(data) if topic_name not in self.received_topics: self.received_topics.add(topic_name) if self.received_topics == self.expected_topics: From 6228855463e7197cf1cb285ca8428af8f224673b Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Fri, 26 May 2023 17:14:32 +0900 Subject: [PATCH 16/18] Renamed max_number_of_messages into messages_received_buffer_length Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index 496623ee..a578fdf2 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -73,7 +73,7 @@ def _prepare_ros_node(self): self.__ros_node = _WaitForTopicsNode( name=node_name, node_context=self.__ros_context, - max_number_of_messages=self.messages_received_buffer_length, + messages_received_buffer_length=self.messages_received_buffer_length, ) self.__ros_executor.add_node(self.__ros_node) @@ -115,11 +115,11 @@ class _WaitForTopicsNode(Node): """Internal node used for subscribing to a set of topics.""" def __init__( - self, name="test_node", node_context=None, max_number_of_messages=None + self, name="test_node", node_context=None, messages_received_buffer_length=None ): super().__init__(node_name=name, context=node_context) # type: ignore self.msg_event_object = Event() - self.max_number_of_messages = max_number_of_messages + self.messages_received_buffer_length = messages_received_buffer_length self.subscriber_list = [] self.topic_tuples = [] self.expected_topics = set() @@ -140,7 +140,7 @@ def start_subscribers(self, topic_tuples): self.expected_topics.add(topic_name) # Initialize ring buffer of received messages self.received_messages_buffer[topic_name] = deque( - maxlen=self.max_number_of_messages + maxlen=self.messages_received_buffer_length ) # Create a subscriber self.subscriber_list.append( From e1564baffda9db6e44ee8b645b48ca0a71b55f43 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Fri, 9 Jun 2023 11:53:32 +0900 Subject: [PATCH 17/18] Trying to fix flake8 warnings Signed-off-by: Pintaudi Giorgio --- launch_testing_ros/launch_testing_ros/wait_for_topics.py | 8 ++++---- .../test/examples/wait_for_topic_launch_test.py | 3 +-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/launch_testing_ros/launch_testing_ros/wait_for_topics.py b/launch_testing_ros/launch_testing_ros/wait_for_topics.py index a578fdf2..5cf2c9cf 100644 --- a/launch_testing_ros/launch_testing_ros/wait_for_topics.py +++ b/launch_testing_ros/launch_testing_ros/wait_for_topics.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from collections import deque import random import string -from collections import deque from threading import Event from threading import Thread @@ -98,7 +98,7 @@ def topics_not_received(self): def received_messages(self, topic_name): """List of received messages of a specific topic.""" if topic_name not in self.__ros_node.received_messages_buffer: - raise KeyError("No messages received for topic: " + topic_name) + raise KeyError('No messages received for topic: ' + topic_name) return list(self.__ros_node.received_messages_buffer[topic_name]) def __enter__(self): @@ -115,7 +115,7 @@ class _WaitForTopicsNode(Node): """Internal node used for subscribing to a set of topics.""" def __init__( - self, name="test_node", node_context=None, messages_received_buffer_length=None + self, name='test_node', node_context=None, messages_received_buffer_length=None ): super().__init__(node_name=name, context=node_context) # type: ignore self.msg_event_object = Event() @@ -154,7 +154,7 @@ def start_subscribers(self, topic_tuples): def callback_template(self, topic_name): def topic_callback(data): - self.get_logger().debug("Message received for " + topic_name) + self.get_logger().debug('Message received for ' + topic_name) self.received_messages_buffer[topic_name].append(data) if topic_name not in self.received_topics: self.received_topics.add(topic_name) diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index d6130521..be9da8f9 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -23,9 +23,8 @@ import launch_testing.actions import launch_testing.markers import pytest -from std_msgs.msg import String - from launch_testing_ros import WaitForTopics +from std_msgs.msg import String def generate_node(i): From 1144f613ec7db6ba46bd2611bb014eae46d7d5c7 Mon Sep 17 00:00:00 2001 From: Pintaudi Giorgio Date: Mon, 19 Jun 2023 10:06:49 +0900 Subject: [PATCH 18/18] Fixed import ordering Signed-off-by: Pintaudi Giorgio --- .../test/examples/wait_for_topic_launch_test.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py index be9da8f9..ad7288f0 100644 --- a/launch_testing_ros/test/examples/wait_for_topic_launch_test.py +++ b/launch_testing_ros/test/examples/wait_for_topic_launch_test.py @@ -22,8 +22,10 @@ import launch_ros.actions import launch_testing.actions import launch_testing.markers -import pytest from launch_testing_ros import WaitForTopics + +import pytest + from std_msgs.msg import String