diff --git a/launch_testing_ros/test/examples/check_msgs_launch_test.py b/launch_testing_ros/test/examples/check_msgs_launch_test.py index 35dd474e3..d86630648 100644 --- a/launch_testing_ros/test/examples/check_msgs_launch_test.py +++ b/launch_testing_ros/test/examples/check_msgs_launch_test.py @@ -51,11 +51,13 @@ class TestFixture(unittest.TestCase): def test_check_if_msgs_published(self, proc_output): rclpy.init() - node = MakeTestNode('test_node') - node.start_subscriber() - msgs_received_flag = node.msg_event_object.wait(timeout=5.0) - assert msgs_received_flag, 'Did not receive msgs !' - rclpy.shutdown() + try: + node = MakeTestNode('test_node') + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=5.0) + assert msgs_received_flag, 'Did not receive msgs !' + finally: + rclpy.shutdown() class MakeTestNode(Node): diff --git a/launch_testing_ros/test/examples/check_node_launch_test.py b/launch_testing_ros/test/examples/check_node_launch_test.py index ef9903b72..e7c383224 100644 --- a/launch_testing_ros/test/examples/check_node_launch_test.py +++ b/launch_testing_ros/test/examples/check_node_launch_test.py @@ -51,9 +51,11 @@ class TestFixture(unittest.TestCase): def test_node_start(self, proc_output): rclpy.init() - node = MakeTestNode('test_node') - assert node.wait_for_node('demo_node_1', 8.0), 'Node not found !' - rclpy.shutdown() + try: + node = MakeTestNode('test_node') + assert node.wait_for_node('demo_node_1', 8.0), 'Node not found !' + finally: + rclpy.shutdown() class MakeTestNode(Node): diff --git a/launch_testing_ros/test/examples/set_param_launch_test.py b/launch_testing_ros/test/examples/set_param_launch_test.py index 382a4e16c..e8b2da70d 100644 --- a/launch_testing_ros/test/examples/set_param_launch_test.py +++ b/launch_testing_ros/test/examples/set_param_launch_test.py @@ -47,10 +47,12 @@ class TestFixture(unittest.TestCase): def test_set_parameter(self, proc_output): rclpy.init() - node = MakeTestNode('test_node') - response = node.set_parameter(state=True) - assert response.successful, 'Could not set parameter!' - rclpy.shutdown() + try: + node = MakeTestNode('test_node') + response = node.set_parameter(state=True) + assert response.successful, 'Could not set parameter!' + finally: + rclpy.shutdown() class MakeTestNode(Node): @@ -71,5 +73,7 @@ def set_parameter(self, state=True, timeout=5.0): future = client.call_async(request) rclpy.spin_until_future_complete(self, future, timeout_sec=timeout) + assert future.done(), 'Client request timed out' + response = future.result() return response.results[0]