From fe5fe5bf505cfa6eee1a796af76cd7fcda0a4aed Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 8 Jul 2024 20:37:37 -0400 Subject: [PATCH] Use rclpy.init context manager where we can. (#547) This allows us to cleanup, while using a lot less code to try and track it. Signed-off-by: Chris Lalancette --- test_cli/test/initial_params.py | 20 ++++--- test_cli/test/test_params_yaml.py | 14 ++--- test_cli_remapping/test/name_maker.py | 15 ++--- test_communication/test/action_server_py.py | 64 ++++++++------------- test_communication/test/replier_py.py | 43 ++++++-------- test_communication/test/requester_py.py | 56 ++++++++---------- test_communication/test/subscriber_py.py | 55 ++++++++---------- 7 files changed, 113 insertions(+), 154 deletions(-) diff --git a/test_cli/test/initial_params.py b/test_cli/test/initial_params.py index 70e39398..de692c7f 100644 --- a/test_cli/test/initial_params.py +++ b/test_cli/test/initial_params.py @@ -15,19 +15,21 @@ import sys import rclpy +from rclpy.executors import ExternalShutdownException def main(argv=sys.argv): - rclpy.init(args=argv) + try: + with rclpy.init(args=argv): + node = rclpy.create_node( + 'initial_params_node', + namespace='/', + allow_undeclared_parameters=True, + automatically_declare_parameters_from_overrides=True) + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass - node = rclpy.create_node( - 'initial_params_node', - namespace='/', - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=True) - rclpy.spin(node) - - rclpy.shutdown() return 0 diff --git a/test_cli/test/test_params_yaml.py b/test_cli/test/test_params_yaml.py index 3fb28bca..89a48bb1 100644 --- a/test_cli/test/test_params_yaml.py +++ b/test_cli/test/test_params_yaml.py @@ -31,19 +31,15 @@ @pytest.fixture(scope='module', params=CLIENT_LIBRARY_EXECUTABLES) def node_fixture(request): """Create a fixture with a node and helper executable.""" - rclpy.init() - node = rclpy.create_node( - 'tests_yaml', - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=True) - try: + with rclpy.init(): + node = rclpy.create_node( + 'tests_yaml', + allow_undeclared_parameters=True, + automatically_declare_parameters_from_overrides=True) yield { 'node': node, 'executable': request.param, } - finally: - node.destroy_node() - rclpy.shutdown() def get_params(node, node_name, param_names, wfs_timeout=5.0): diff --git a/test_cli_remapping/test/name_maker.py b/test_cli_remapping/test/name_maker.py index f21e26b8..f7920185 100644 --- a/test_cli_remapping/test/name_maker.py +++ b/test_cli_remapping/test/name_maker.py @@ -13,6 +13,7 @@ # limitations under the License. import rclpy +from rclpy.executors import ExternalShutdownException import rclpy.node from test_msgs.msg import Empty as EmptyMsg from test_msgs.srv import Empty as EmptySrv @@ -35,15 +36,9 @@ def __init__(self): if __name__ == '__main__': - rclpy.init() - - node = NameMaker() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(): + node = NameMaker() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('Shutting down name_maker.py') - else: - rclpy.shutdown() - finally: - node.destroy_node() diff --git a/test_communication/test/action_server_py.py b/test_communication/test/action_server_py.py index 4fa0e141..986785fa 100644 --- a/test_communication/test/action_server_py.py +++ b/test_communication/test/action_server_py.py @@ -16,6 +16,8 @@ import sys import time +import rclpy +from rclpy.action import ActionServer from rclpy.executors import ExternalShutdownException from test_msgs.action import Fibonacci @@ -32,8 +34,6 @@ def execute_goal(self, goal_handle): def receive_goals(node, action_type, expected_goals): - from rclpy.action import ActionServer - def execute_callback(goal_handle): for expected_goal in expected_goals: if expected_goal.is_goal_expected(goal_handle.request): @@ -48,8 +48,6 @@ def execute_callback(goal_handle): def generate_expected_fibonacci_goals(): - import rclpy - expected_goals = [] def is_goal_expected(goal): @@ -101,8 +99,6 @@ def execute_goal(goal_handle): def generate_expected_nested_message_goals(): - import rclpy - expected_goals = [] def is_goal_expected(goal): @@ -151,39 +147,27 @@ def execute_goal(goal_handle): if __name__ == '__main__': - import rclpy - - parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) - parser.add_argument('action_type', help='type of the ROS action') - parser.add_argument('namespace', help='namespace of the ROS node') - args = parser.parse_args() - - rclpy.init(args=[]) - node = rclpy.create_node('action_server', namespace=args.namespace) - - if 'Fibonacci' == args.action_type: - action_server = receive_goals(node, Fibonacci, generate_expected_fibonacci_goals()) - elif 'NestedMessage' == args.action_type: - action_server = receive_goals( - node, - NestedMessage, - generate_expected_nested_message_goals(), - ) - else: - print('Unknown action type {!r}'.format(args.action_type), file=sys.stderr) - node.destroy_node() - rclpy.shutdown() - sys.exit(1) - try: - rclpy.spin(node) - except KeyboardInterrupt: + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('action_type', help='type of the ROS action') + parser.add_argument('namespace', help='namespace of the ROS node') + args = parser.parse_args() + + with rclpy.init(args=[]): + node = rclpy.create_node('action_server', namespace=args.namespace) + + if 'Fibonacci' == args.action_type: + action_server = receive_goals(node, Fibonacci, generate_expected_fibonacci_goals()) + elif 'NestedMessage' == args.action_type: + action_server = receive_goals( + node, + NestedMessage, + generate_expected_nested_message_goals(), + ) + else: + print('Unknown action type {!r}'.format(args.action_type), file=sys.stderr) + sys.exit(1) + + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('Action server stopped cleanly') - except ExternalShutdownException: - sys.exit(1) - except BaseException: - print('Exception in action server:', file=sys.stderr) - raise - finally: - rclpy.try_shutdown() - node.destroy_node() diff --git a/test_communication/test/replier_py.py b/test_communication/test/replier_py.py index 3ff979ee..878ad75e 100644 --- a/test_communication/test/replier_py.py +++ b/test_communication/test/replier_py.py @@ -15,7 +15,11 @@ import argparse import functools import importlib -import sys + +import rclpy +from rclpy.executors import ExternalShutdownException + +from test_msgs.service_fixtures import get_test_srv def replier_callback(request, response, srv_fixtures): @@ -27,33 +31,27 @@ def replier_callback(request, response, srv_fixtures): def replier(service_name, number_of_cycles, namespace): - from test_msgs.service_fixtures import get_test_srv - import rclpy - service_pkg = 'test_msgs' module = importlib.import_module(service_pkg + '.srv') srv_mod = getattr(module, service_name) - rclpy.init(args=[]) - - node = rclpy.create_node('replier', namespace=namespace) + with rclpy.init(args=[]): + node = rclpy.create_node('replier', namespace=namespace) - srv_fixtures = get_test_srv(service_name) + srv_fixtures = get_test_srv(service_name) - chatter_callback = functools.partial( - replier_callback, srv_fixtures=srv_fixtures) + chatter_callback = functools.partial( + replier_callback, srv_fixtures=srv_fixtures) - node.create_service( - srv_mod, 'test/service/' + service_name, chatter_callback) + node.create_service( + srv_mod, 'test/service/' + service_name, chatter_callback) - spin_count = 1 - print('replier: beginning loop') - while rclpy.ok() and spin_count < number_of_cycles: - rclpy.spin_once(node, timeout_sec=2) - spin_count += 1 - print('spin_count: ' + str(spin_count)) - node.destroy_node() - rclpy.shutdown() + spin_count = 1 + print('replier: beginning loop') + while rclpy.ok() and spin_count < number_of_cycles: + rclpy.spin_once(node, timeout_sec=2) + spin_count += 1 + print('spin_count: ' + str(spin_count)) if __name__ == '__main__': @@ -69,8 +67,5 @@ def replier(service_name, number_of_cycles, namespace): number_of_cycles=args.number_of_cycles, namespace=args.namespace ) - except KeyboardInterrupt: + except (KeyboardInterrupt, ExternalShutdownException): print('server stopped cleanly') - except BaseException: - print('exception in server:', file=sys.stderr) - raise diff --git a/test_communication/test/requester_py.py b/test_communication/test/requester_py.py index 16ef2588..f67e45d3 100644 --- a/test_communication/test/requester_py.py +++ b/test_communication/test/requester_py.py @@ -14,13 +14,14 @@ import argparse import importlib -import sys +import rclpy +from rclpy.executors import ExternalShutdownException + +from test_msgs.service_fixtures import get_test_srv -def requester(service_name, namespace): - import rclpy - from test_msgs.service_fixtures import get_test_srv +def requester(service_name, namespace): # Import the service service_pkg = 'test_msgs' module = importlib.import_module(service_pkg + '.srv') @@ -29,31 +30,25 @@ def requester(service_name, namespace): srv_fixtures = get_test_srv(service_name) service_name = 'test/service/' + service_name - rclpy.init(args=[]) - try: + with rclpy.init(args=[]): node = rclpy.create_node('requester', namespace=namespace) - try: - # wait for the service to be available - client = node.create_client(srv_mod, service_name) - tries = 15 - while rclpy.ok() and not client.wait_for_service(timeout_sec=1.0) and tries > 0: - print('service not available, waiting again...') - tries -= 1 - assert tries > 0, 'service still not available, aborting test' - - print('requester: beginning request') - # Make one call to that service - for req, resp in srv_fixtures: - future = client.call_async(req) - rclpy.spin_until_future_complete(node, future) - assert repr(future.result()) == repr(resp), \ - 'unexpected response %r\n\nwas expecting %r' % (future.result(), resp) - print('received reply #%d of %d' % ( - srv_fixtures.index([req, resp]) + 1, len(srv_fixtures))) - finally: - node.destroy_node() - finally: - rclpy.shutdown() + # wait for the service to be available + client = node.create_client(srv_mod, service_name) + tries = 15 + while rclpy.ok() and not client.wait_for_service(timeout_sec=1.0) and tries > 0: + print('service not available, waiting again...') + tries -= 1 + assert tries > 0, 'service still not available, aborting test' + + print('requester: beginning request') + # Make one call to that service + for req, resp in srv_fixtures: + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future) + assert repr(future.result()) == repr(resp), \ + 'unexpected response %r\n\nwas expecting %r' % (future.result(), resp) + print('received reply #%d of %d' % ( + srv_fixtures.index([req, resp]) + 1, len(srv_fixtures))) if __name__ == '__main__': @@ -63,8 +58,5 @@ def requester(service_name, namespace): args = parser.parse_args() try: requester(service_name=args.service_name, namespace=args.namespace) - except KeyboardInterrupt: + except (KeyboardInterrupt, ExternalShutdownException): print('requester stopped cleanly') - except BaseException: - print('exception in requester:', file=sys.stderr) - raise diff --git a/test_communication/test/subscriber_py.py b/test_communication/test/subscriber_py.py index 57d8d4cf..bdf55fe9 100644 --- a/test_communication/test/subscriber_py.py +++ b/test_communication/test/subscriber_py.py @@ -15,7 +15,11 @@ import argparse import functools import importlib -import sys + +import rclpy +from rclpy.executors import ExternalShutdownException + +from test_msgs.message_fixtures import get_test_msg def listener_cb(msg, received_messages, expected_msgs): @@ -38,38 +42,32 @@ def listener_cb(msg, received_messages, expected_msgs): raise RuntimeError('received unexpected message %r' % msg) -def listener(message_name, namespace): - from test_msgs.message_fixtures import get_test_msg - import rclpy - +def listener(msg_name, namespace): message_pkg = 'test_msgs' module = importlib.import_module(message_pkg + '.msg') - msg_mod = getattr(module, message_name) - - rclpy.init(args=[]) + msg_mod = getattr(module, msg_name) - node = rclpy.create_node('listener', namespace=namespace) + with rclpy.init(args=[]): + node = rclpy.create_node('listener', namespace=namespace) - received_messages = [] - expected_msgs = [(i, repr(msg)) for i, msg in enumerate(get_test_msg(message_name))] + received_messages = [] + expected_msgs = [(i, repr(msg)) for i, msg in enumerate(get_test_msg(msg_name))] - chatter_callback = functools.partial( - listener_cb, received_messages=received_messages, expected_msgs=expected_msgs) + chatter_callback = functools.partial( + listener_cb, received_messages=received_messages, expected_msgs=expected_msgs) - node.create_subscription( - msg_mod, 'test/message/' + message_name, chatter_callback, 10) + node.create_subscription( + msg_mod, 'test/message/' + msg_name, chatter_callback, 10) - spin_count = 1 - print('subscriber: beginning loop') - while (rclpy.ok() and len(received_messages) != len(expected_msgs)): - rclpy.spin_once(node) - spin_count += 1 - print('spin_count: ' + str(spin_count)) - node.destroy_node() - rclpy.shutdown() + spin_count = 1 + print('subscriber: beginning loop') + while (rclpy.ok() and len(received_messages) != len(expected_msgs)): + rclpy.spin_once(node) + spin_count += 1 + print('spin_count: ' + str(spin_count)) - assert len(received_messages) == len(expected_msgs), \ - 'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name) + assert len(received_messages) == len(expected_msgs), \ + 'Should have received {} {} messages from talker'.format(len(expected_msgs), msg_name) if __name__ == '__main__': @@ -78,9 +76,6 @@ def listener(message_name, namespace): parser.add_argument('namespace', help='namespace of the ROS node') args = parser.parse_args() try: - listener(message_name=args.message_name, namespace=args.namespace) - except KeyboardInterrupt: + listener(msg_name=args.message_name, namespace=args.namespace) + except (KeyboardInterrupt, ExternalShutdownException): print('subscriber stopped cleanly') - except BaseException: - print('exception in subscriber:', file=sys.stderr) - raise