diff --git a/ros2action/test/fixtures/fibonacci_action_server.py b/ros2action/test/fixtures/fibonacci_action_server.py index 19f3948f2..396685d17 100644 --- a/ros2action/test/fixtures/fibonacci_action_server.py +++ b/ros2action/test/fixtures/fibonacci_action_server.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,10 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License.import time -import sys - import rclpy from rclpy.action import ActionServer +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from test_msgs.action import Fibonacci @@ -52,19 +50,12 @@ def execute_callback(self, goal_handle): def main(args=None): - rclpy.init(args=args) - - node = FibonacciActionServer() try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = FibonacciActionServer() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('server stopped cleanly') - except BaseException: - print('exception in server:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2doctor/test/fixtures/listener_node_with_reliable_qos.py b/ros2doctor/test/fixtures/listener_node_with_reliable_qos.py index 8fbd2227f..91f6f1e0f 100644 --- a/ros2doctor/test/fixtures/listener_node_with_reliable_qos.py +++ b/ros2doctor/test/fixtures/listener_node_with_reliable_qos.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy @@ -39,20 +38,12 @@ def callback(self, msg): def main(args=None): - rclpy.init(args=args) - - node = ListenerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = ListenerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('listener stopped cleanly') - except BaseException: - print('exception in listener:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2doctor/test/fixtures/talker_node_with_best_effort_qos.py b/ros2doctor/test/fixtures/talker_node_with_best_effort_qos.py index 23dc9ac07..aca62555c 100644 --- a/ros2doctor/test/fixtures/talker_node_with_best_effort_qos.py +++ b/ros2doctor/test/fixtures/talker_node_with_best_effort_qos.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy @@ -40,20 +39,12 @@ def callback(self): def main(args=None): - rclpy.init(args=args) - - node = TalkerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = TalkerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('talker stopped cleanly') - except BaseException: - print('exception in talker:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2doctor/test/fixtures/talker_node_with_reliable_qos.py b/ros2doctor/test/fixtures/talker_node_with_reliable_qos.py index 6b63e616e..7198b724f 100644 --- a/ros2doctor/test/fixtures/talker_node_with_reliable_qos.py +++ b/ros2doctor/test/fixtures/talker_node_with_reliable_qos.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy @@ -40,20 +39,12 @@ def callback(self): def main(args=None): - rclpy.init(args=args) - - node = TalkerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = TalkerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('talker stopped cleanly') - except BaseException: - print('exception in talker:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2node/test/fixtures/complex_node.py b/ros2node/test/fixtures/complex_node.py index d715e218b..7915f10cf 100644 --- a/ros2node/test/fixtures/complex_node.py +++ b/ros2node/test/fixtures/complex_node.py @@ -12,10 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy from rclpy.action import ActionServer +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import qos_profile_system_default @@ -56,20 +55,12 @@ def action_callback(self, goal_handle): def main(args=None): - rclpy.init(args=args) - - node = ComplexNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = ComplexNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('node stopped cleanly') - except BaseException: - print('exception in node:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2param/test/fixtures/parameter_node.py b/ros2param/test/fixtures/parameter_node.py index 9df269ac0..6e5e9f8af 100644 --- a/ros2param/test/fixtures/parameter_node.py +++ b/ros2param/test/fixtures/parameter_node.py @@ -13,33 +13,30 @@ # limitations under the License. import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.parameter import PARAMETER_SEPARATOR_STRING def main(args=None): - rclpy.init(args=args) - - node = rclpy.create_node('parameter_node') - node.declare_parameter('bool_param', True) - node.declare_parameter('int_param', 42) - node.declare_parameter('double_param', 1.23) - node.declare_parameter('str_param', 'Hello World') - node.declare_parameter('bool_array_param', [False, False, True]) - node.declare_parameter('int_array_param', [1, 2, 3]) - node.declare_parameter('str_array_param', ['foo', 'bar', 'baz']) - node.declare_parameter('double_array_param', [3.125, 6.25, 12.5]) - node.declare_parameter('foo' + PARAMETER_SEPARATOR_STRING + 'str_param', 'foo') - node.declare_parameter('foo' + PARAMETER_SEPARATOR_STRING + - 'bar' + PARAMETER_SEPARATOR_STRING + - 'str_param', 'foobar') - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = rclpy.create_node('parameter_node') + node.declare_parameter('bool_param', True) + node.declare_parameter('int_param', 42) + node.declare_parameter('double_param', 1.23) + node.declare_parameter('str_param', 'Hello World') + node.declare_parameter('bool_array_param', [False, False, True]) + node.declare_parameter('int_array_param', [1, 2, 3]) + node.declare_parameter('str_array_param', ['foo', 'bar', 'baz']) + node.declare_parameter('double_array_param', [3.125, 6.25, 12.5]) + node.declare_parameter('foo' + PARAMETER_SEPARATOR_STRING + 'str_param', 'foo') + node.declare_parameter('foo' + PARAMETER_SEPARATOR_STRING + + 'bar' + PARAMETER_SEPARATOR_STRING + + 'str_param', 'foobar') + + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('parameter node stopped cleanly') - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2service/ros2service/verb/call.py b/ros2service/ros2service/verb/call.py index c5a99e865..4f75f5462 100644 --- a/ros2service/ros2service/verb/call.py +++ b/ros2service/ros2service/verb/call.py @@ -89,37 +89,33 @@ def requester(service_type, service_name, values, period): values_dictionary = yaml.safe_load(values) - rclpy.init() - - node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name)) - - cli = node.create_client(srv_module, service_name) - - request = srv_module.Request() - - try: - set_message_fields(request, values_dictionary) - except Exception as e: - return 'Failed to populate field: {0}'.format(e) - - if not cli.service_is_ready(): - print('waiting for service to become available...') - cli.wait_for_service() - - while True: - print('requester: making request: %r\n' % request) - last_call = time.time() - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - print('response:\n%r\n' % future.result()) - else: - raise RuntimeError('Exception while calling service: %r' % future.exception()) - if period is None or not rclpy.ok(): - break - time_until_next_period = (last_call + period) - time.time() - if time_until_next_period > 0: - time.sleep(time_until_next_period) - - node.destroy_node() - rclpy.shutdown() + with rclpy.init(): + node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name)) + + cli = node.create_client(srv_module, service_name) + + request = srv_module.Request() + + try: + set_message_fields(request, values_dictionary) + except Exception as e: + return 'Failed to populate field: {0}'.format(e) + + if not cli.service_is_ready(): + print('waiting for service to become available...') + cli.wait_for_service() + + while True: + print('requester: making request: %r\n' % request) + last_call = time.time() + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response:\n%r\n' % future.result()) + else: + raise RuntimeError('Exception while calling service: %r' % future.exception()) + if period is None or not rclpy.ok(): + break + time_until_next_period = (last_call + period) - time.time() + if time_until_next_period > 0: + time.sleep(time_until_next_period) diff --git a/ros2service/test/fixtures/echo_server.py b/ros2service/test/fixtures/echo_server.py index ca3584d7c..a4df9b733 100644 --- a/ros2service/test/fixtures/echo_server.py +++ b/ros2service/test/fixtures/echo_server.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from test_msgs.srv import BasicTypes @@ -33,21 +32,12 @@ def callback(self, request, response): def main(args=None): - rclpy.init(args=args) - - node = EchoServer() try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = EchoServer() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('server stopped cleanly') - except BaseException: - print('exception in server:', file=sys.stderr) - raise - finally: - # Destroy the node explicitly - # (optional - Done automatically when node is garbage collected) - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2topic/test/fixtures/controller_node.py b/ros2topic/test/fixtures/controller_node.py index 1dd926597..9c085eb1d 100644 --- a/ros2topic/test/fixtures/controller_node.py +++ b/ros2topic/test/fixtures/controller_node.py @@ -12,11 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - from geometry_msgs.msg import TwistStamped import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import qos_profile_system_default @@ -38,20 +37,12 @@ def callback(self): def main(args=None): - rclpy.init(args=args) - - node = ControllerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = ControllerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('controller stopped cleanly') - except BaseException: - print('exception in controller:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2topic/test/fixtures/listener_node.py b/ros2topic/test/fixtures/listener_node.py index 71fa79b9f..1459f29eb 100644 --- a/ros2topic/test/fixtures/listener_node.py +++ b/ros2topic/test/fixtures/listener_node.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from ros2topic.api import qos_profile_from_short_keys @@ -37,20 +36,12 @@ def callback(self, msg): def main(args=None): - rclpy.init(args=args) - - node = ListenerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = ListenerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('listener stopped cleanly') - except BaseException: - print('exception in listener:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2topic/test/fixtures/repeater_node.py b/ros2topic/test/fixtures/repeater_node.py index 338142790..476fe687d 100644 --- a/ros2topic/test/fixtures/repeater_node.py +++ b/ros2topic/test/fixtures/repeater_node.py @@ -16,6 +16,7 @@ import sys import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import qos_profile_system_default from rclpy.utilities import remove_ros_args @@ -46,22 +47,14 @@ def parse_arguments(args=None): def main(args=None): - parsed_args = parse_arguments(args=args) - - rclpy.init(args=args) - - node = RepeaterNode(message_type=parsed_args.message_type) - try: - rclpy.spin(node) - except KeyboardInterrupt: + parsed_args = parse_arguments(args=args) + + with rclpy.init(args=args): + node = RepeaterNode(message_type=parsed_args.message_type) + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('repeater stopped cleanly') - except BaseException: - print('exception in repeater:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2topic/test/fixtures/talker_node.py b/ros2topic/test/fixtures/talker_node.py index a29f6ecdc..6ad3b3292 100644 --- a/ros2topic/test/fixtures/talker_node.py +++ b/ros2topic/test/fixtures/talker_node.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys - import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from std_msgs.msg import String @@ -34,20 +33,12 @@ def callback(self): def main(args=None): - rclpy.init(args=args) - - node = TalkerNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: + with rclpy.init(args=args): + node = TalkerNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): print('talker stopped cleanly') - except BaseException: - print('exception in talker:', file=sys.stderr) - raise - finally: - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': diff --git a/ros2topic/test/qos_subscription.py b/ros2topic/test/qos_subscription.py index aed7df0ca..0ac572b73 100644 --- a/ros2topic/test/qos_subscription.py +++ b/ros2topic/test/qos_subscription.py @@ -13,6 +13,7 @@ # limitations under the License. import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy @@ -38,62 +39,61 @@ def onMsg(self, param, msg): def main(args=None): - rclpy.init(args=args) - node = rclpy.create_node('qos_subscription') - - counter = Counter() - - sub_default = node.create_subscription( - String, - 'topic', - lambda msg: counter.onMsg('default', msg), - QoSProfile(depth=10)) - - sub_transient = node.create_subscription( - String, - 'topic', - lambda msg: counter.onMsg('transient', msg), - QoSProfile( - depth=10, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)) - - sub_volatile = node.create_subscription( - String, - 'topic', - lambda msg: counter.onMsg('volatile', msg), - QoSProfile( - depth=10, - durability=QoSDurabilityPolicy.VOLATILE)) - - sub_reliable = node.create_subscription( - String, - 'topic', - lambda msg: counter.onMsg('reliable', msg), - QoSProfile( - depth=10, - reliability=QoSReliabilityPolicy.RELIABLE)) - - sub_best_effort = node.create_subscription( - String, - 'topic', - lambda msg: counter.onMsg('best_effort', msg), - QoSProfile( - depth=10, - reliability=QoSReliabilityPolicy.BEST_EFFORT)) - - # Asserts to suppress unused variable warnings. - assert sub_default - assert sub_transient - assert sub_volatile - assert sub_reliable - assert sub_best_effort - - timer = node.create_timer(0.5, lambda: print(counter.count)) - - rclpy.spin(node) - node.destroy_timer(timer) - node.destroy_node() - rclpy.shutdown() + try: + with rclpy.init(args=args): + node = rclpy.create_node('qos_subscription') + + counter = Counter() + + sub_default = node.create_subscription( + String, + 'topic', + lambda msg: counter.onMsg('default', msg), + QoSProfile(depth=10)) + + sub_transient = node.create_subscription( + String, + 'topic', + lambda msg: counter.onMsg('transient', msg), + QoSProfile( + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)) + + sub_volatile = node.create_subscription( + String, + 'topic', + lambda msg: counter.onMsg('volatile', msg), + QoSProfile( + depth=10, + durability=QoSDurabilityPolicy.VOLATILE)) + + sub_reliable = node.create_subscription( + String, + 'topic', + lambda msg: counter.onMsg('reliable', msg), + QoSProfile( + depth=10, + reliability=QoSReliabilityPolicy.RELIABLE)) + + sub_best_effort = node.create_subscription( + String, + 'topic', + lambda msg: counter.onMsg('best_effort', msg), + QoSProfile( + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT)) + + timer = node.create_timer(0.5, lambda: print(counter.count)) + + # Asserts to suppress unused variable warnings. + assert sub_default + assert sub_transient + assert sub_volatile + assert sub_reliable + assert sub_best_effort + assert timer + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__':