diff --git a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py index c532c163d..6cc86c0e8 100755 --- a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py +++ b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py @@ -17,6 +17,7 @@ import rclpy from rclpy.action import ActionClient +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -62,13 +63,15 @@ def feedback_callback(self, feedback_msg): def main(args=None): - rclpy.init(args=args) + try: + with rclpy.init(args=args): + action_client = FibonacciActionClient() - action_client = FibonacciActionClient() + action_client.send_goal(10) - action_client.send_goal(10) - - rclpy.spin(action_client) + rclpy.spin(action_client) + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py index bb5fc693b..b8f852cff 100755 --- a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py +++ b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py @@ -19,6 +19,7 @@ import rclpy from rclpy.action import ActionServer +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -53,13 +54,11 @@ def execute_callback(self, goal_handle): def main(args=None): - rclpy.init(args=args) - - fibonacci_action_server = FibonacciActionServer() - try: - rclpy.spin(fibonacci_action_server) - except KeyboardInterrupt: + with rclpy.init(args=args): + fibonacci_action_server = FibonacciActionServer() + rclpy.spin(fibonacci_action_server) + except (KeyboardInterrupt, ExternalShutdownException): pass diff --git a/demo_nodes_py/demo_nodes_py/events/matched_event_detect.py b/demo_nodes_py/demo_nodes_py/events/matched_event_detect.py index 39171976c..9e3dc5f08 100644 --- a/demo_nodes_py/demo_nodes_py/events/matched_event_detect.py +++ b/demo_nodes_py/demo_nodes_py/events/matched_event_detect.py @@ -17,6 +17,7 @@ from rclpy.event_handler import QoSPublisherMatchedInfo from rclpy.event_handler import QoSSubscriptionMatchedInfo from rclpy.event_handler import SubscriptionEventCallbacks +from rclpy.executors import ExternalShutdownException from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.publisher import Publisher @@ -133,72 +134,69 @@ def destroy_one_pub(self, pub: Publisher): def main(args=None): - rclpy.init(args=args) - - topic_name_for_detect_pub_matched_event = 'pub_topic_matched_event_detect' - topic_name_for_detect_sub_matched_event = 'sub_topic_matched_event_detect' - - matched_event_detect_node = MatchedEventDetectNode( - topic_name_for_detect_pub_matched_event, topic_name_for_detect_sub_matched_event) - multi_subs_node = MultiSubNode(topic_name_for_detect_pub_matched_event) - multi_pubs_node = MultiPubNode(topic_name_for_detect_sub_matched_event) - - maximum_wait_time = 10 # 10s - - executor = SingleThreadedExecutor() - - executor.add_node(matched_event_detect_node) - executor.add_node(multi_subs_node) - executor.add_node(multi_pubs_node) - - # MatchedEventDetectNode will output: - # First subscription is connected. - sub1 = multi_subs_node.create_one_sub() - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # The changed number of connected subscription is 1 and current number of connected - # subscription is 2. - sub2 = multi_subs_node.create_one_sub() - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # The changed number of connected subscription is -1 and current number of connected - # subscription is 1. - multi_subs_node.destroy_one_sub(sub1) - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # Last subscription is disconnected. - multi_subs_node.destroy_one_sub(sub2) - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # First publisher is connected. - pub1 = multi_pubs_node.create_one_pub() - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # The changed number of connected publisher is 1 and current number of connected publisher - # is 2. - pub2 = multi_pubs_node.create_one_pub() - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # The changed number of connected publisher is -1 and current number of connected publisher - # is 1. - multi_pubs_node.destroy_one_pub(pub1) - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - # MatchedEventDetectNode will output: - # Last publisher is disconnected. - multi_pubs_node.destroy_one_pub(pub2) - executor.spin_until_future_complete(matched_event_detect_node.get_future(), maximum_wait_time) - - multi_pubs_node.destroy_node() - multi_subs_node.destroy_node() - matched_event_detect_node.destroy_node() - rclpy.try_shutdown() + try: + with rclpy.init(args=args): + topic_name_for_detect_pub_matched_event = 'pub_topic_matched_event_detect' + topic_name_for_detect_sub_matched_event = 'sub_topic_matched_event_detect' + + matched_node = MatchedEventDetectNode( + topic_name_for_detect_pub_matched_event, topic_name_for_detect_sub_matched_event) + multi_subs_node = MultiSubNode(topic_name_for_detect_pub_matched_event) + multi_pubs_node = MultiPubNode(topic_name_for_detect_sub_matched_event) + + max_wait_time = 10 # 10s + + executor = SingleThreadedExecutor() + + executor.add_node(matched_node) + executor.add_node(multi_subs_node) + executor.add_node(multi_pubs_node) + + # MatchedEventDetectNode will output: + # First subscription is connected. + sub1 = multi_subs_node.create_one_sub() + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # The changed number of connected subscription is 1 and current number of connected + # subscription is 2. + sub2 = multi_subs_node.create_one_sub() + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # The changed number of connected subscription is -1 and current number of connected + # subscription is 1. + multi_subs_node.destroy_one_sub(sub1) + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # Last subscription is disconnected. + multi_subs_node.destroy_one_sub(sub2) + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # First publisher is connected. + pub1 = multi_pubs_node.create_one_pub() + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # The changed number of connected publisher is 1 and current number of connected + # publisher is 2. + pub2 = multi_pubs_node.create_one_pub() + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # The changed number of connected publisher is -1 and current number of connected + # publisher is 1. + multi_pubs_node.destroy_one_pub(pub1) + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + + # MatchedEventDetectNode will output: + # Last publisher is disconnected. + multi_pubs_node.destroy_one_pub(pub2) + executor.spin_until_future_complete(matched_node.get_future(), max_wait_time) + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/logging/use_logger_service.py b/demo_nodes_py/demo_nodes_py/logging/use_logger_service.py index df979e903..fe161d606 100644 --- a/demo_nodes_py/demo_nodes_py/logging/use_logger_service.py +++ b/demo_nodes_py/demo_nodes_py/logging/use_logger_service.py @@ -19,6 +19,7 @@ from rcl_interfaces.srv import GetLoggerLevels from rcl_interfaces.srv import SetLoggerLevels import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.executors import SingleThreadedExecutor from rclpy.impl.logging_severity import LoggingSeverity from rclpy.node import Node @@ -106,70 +107,73 @@ def get_logger_level_func(test_node): def main(args=None): - rclpy.init(args=args) - - logger_service_node = LoggerServiceNode() - test_node = TestNode('LoggerServiceNode') - - executor = SingleThreadedExecutor() - executor.add_node(logger_service_node) - - thread = threading.Thread(target=executor.spin) - thread.start() - - # Output with default logger level - test_node.get_logger().info('Output with default logger level:') - msg = String() - msg.data = 'Output 1' - test_node.pub.publish(msg) - time.sleep(0.5) - - # Get logger level. Logger level should be 0 (Unset) - get_logger_level_func(test_node) - - # Output with debug logger level - test_node.get_logger().info('Output with debug logger level:') - if test_node.set_logger_level_on_remote_node(LoggingSeverity.DEBUG): - msg = String() - msg.data = 'Output 2' - test_node.pub.publish(msg) - time.sleep(0.5) - else: - test_node.get_logger().error('Failed to set debug logger level via logger service !') - - # Get logger level. Logger level should be 10 (Debug) - get_logger_level_func(test_node) - - # Output with warn logger level - test_node.get_logger().info('Output with warn logger level:') - if test_node.set_logger_level_on_remote_node(LoggingSeverity.WARN): - msg = String() - msg.data = 'Output 3' - test_node.pub.publish(msg) - time.sleep(0.5) - else: - test_node.get_logger().error('Failed to set warn logger level via logger service !') - - # Get logger level. Logger level should be 30 (warn) - get_logger_level_func(test_node) - - # Output with error logger level - test_node.get_logger().info('Output with error logger level:') - if test_node.set_logger_level_on_remote_node(LoggingSeverity.ERROR): - msg = String() - msg.data = 'Output 4' - test_node.pub.publish(msg) - time.sleep(0.5) - else: - test_node.get_logger().error('Failed to set error logger level via logger service !') - - # Get logger level. Logger level should be 40 (Error) - get_logger_level_func(test_node) - - executor.shutdown() - if thread.is_alive(): - thread.join() - rclpy.try_shutdown() + try: + with rclpy.init(args=args): + logger_service_node = LoggerServiceNode() + test_node = TestNode('LoggerServiceNode') + + executor = SingleThreadedExecutor() + executor.add_node(logger_service_node) + + thread = threading.Thread(target=executor.spin) + thread.start() + + logger = test_node.get_logger() + + # Output with default logger level + logger.info('Output with default logger level:') + msg = String() + msg.data = 'Output 1' + test_node.pub.publish(msg) + time.sleep(0.5) + + # Get logger level. Logger level should be 0 (Unset) + get_logger_level_func(test_node) + + # Output with debug logger level + logger.info('Output with debug logger level:') + if test_node.set_logger_level_on_remote_node(LoggingSeverity.DEBUG): + msg = String() + msg.data = 'Output 2' + test_node.pub.publish(msg) + time.sleep(0.5) + else: + logger.error('Failed to set debug logger level via logger service !') + + # Get logger level. Logger level should be 10 (Debug) + get_logger_level_func(test_node) + + # Output with warn logger level + logger.info('Output with warn logger level:') + if test_node.set_logger_level_on_remote_node(LoggingSeverity.WARN): + msg = String() + msg.data = 'Output 3' + test_node.pub.publish(msg) + time.sleep(0.5) + else: + logger.error('Failed to set warn logger level via logger service !') + + # Get logger level. Logger level should be 30 (warn) + get_logger_level_func(test_node) + + # Output with error logger level + logger.info('Output with error logger level:') + if test_node.set_logger_level_on_remote_node(LoggingSeverity.ERROR): + msg = String() + msg.data = 'Output 4' + test_node.pub.publish(msg) + time.sleep(0.5) + else: + logger.error('Failed to set error logger level via logger service !') + + # Get logger level. Logger level should be 40 (Error) + get_logger_level_func(test_node) + + executor.shutdown() + if thread.is_alive(): + thread.join() + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/parameters/async_param_client.py b/demo_nodes_py/demo_nodes_py/parameters/async_param_client.py index 1893d29e1..597bdf5f7 100644 --- a/demo_nodes_py/demo_nodes_py/parameters/async_param_client.py +++ b/demo_nodes_py/demo_nodes_py/parameters/async_param_client.py @@ -16,6 +16,7 @@ from ament_index_python import get_package_share_directory import rclpy import rclpy.context +from rclpy.executors import ExternalShutdownException from rclpy.parameter import Parameter from rclpy.parameter import parameter_dict_from_yaml_file from rclpy.parameter import parameter_value_to_python @@ -23,80 +24,84 @@ def main(args=None): - rclpy.init() - node = rclpy.create_node('async_param_client') - client = AsyncParameterClient(node, 'parameter_blackboard') - if not client.wait_for_services(3.0): - raise RuntimeError( - 'parameter_blackboard is not available, start parameter blackboard with ' - '`ros2 run demo_nodes_cpp parameter_blackboard` before running this demo') - param_list = ['int_parameter', 'bool_parameter', 'string_parameter'] + try: + with rclpy.init(): + node = rclpy.create_node('async_param_client') + client = AsyncParameterClient(node, 'parameter_blackboard') + if not client.wait_for_services(3.0): + raise RuntimeError( + 'parameter_blackboard is not available, start parameter blackboard with ' + '`ros2 run demo_nodes_cpp parameter_blackboard` before running this demo') + param_list = ['int_parameter', 'bool_parameter', 'string_parameter'] - node.get_logger().info('Setting parameters:') - future = client.set_parameters([ - Parameter('int_parameter', Parameter.Type.INTEGER, 10), - Parameter('bool_parameter', Parameter.Type.BOOL, False), - Parameter('string_parameter', Parameter.Type.STRING, 'Fee Fi Fo Fum'), ]) - rclpy.spin_until_future_complete(node, future) - set_parameters_result = future.result() - if set_parameters_result is not None: - for i, v in enumerate(set_parameters_result.results): - node.get_logger().info(f' {param_list[i]}:') - node.get_logger().info(f' successful: {v.successful}') - else: - node.get_logger().error(f'Error setting parameters: {future.exception()}') + logger = node.get_logger() - node.get_logger().info('Listing Parameters:') - future = client.list_parameters(param_list, 10) - rclpy.spin_until_future_complete(node, future) - list_parameters_result = future.result() - if list_parameters_result is not None: - for param_name in list_parameters_result.result.names: - node.get_logger().info(f' - {param_name}') - else: - node.get_logger().error(f'Error listing parameters: {future.exception()}') + logger.info('Setting parameters:') + future = client.set_parameters([ + Parameter('int_parameter', Parameter.Type.INTEGER, 10), + Parameter('bool_parameter', Parameter.Type.BOOL, False), + Parameter('string_parameter', Parameter.Type.STRING, 'Fee Fi Fo Fum'), ]) + rclpy.spin_until_future_complete(node, future) + set_parameters_result = future.result() + if set_parameters_result is not None: + for i, v in enumerate(set_parameters_result.results): + logger.info(f' {param_list[i]}:') + logger.info(f' successful: {v.successful}') + else: + logger.error(f'Error setting parameters: {future.exception()}') - node.get_logger().info('Getting parameters:') - future = client.get_parameters(param_list) - rclpy.spin_until_future_complete(node, future) - get_parameters_result = future.result() - if get_parameters_result is not None: - for i, v in enumerate(get_parameters_result.values): - node.get_logger().info(f' - {param_list[i]}: {parameter_value_to_python(v)}') - else: - node.get_logger().error(f'Error getting parameters: {future.exception()}') + logger.info('Listing Parameters:') + future = client.list_parameters(param_list, 10) + rclpy.spin_until_future_complete(node, future) + list_parameters_result = future.result() + if list_parameters_result is not None: + for param_name in list_parameters_result.result.names: + logger.info(f' - {param_name}') + else: + logger.error(f'Error listing parameters: {future.exception()}') - node.get_logger().info('Loading parameters: ') - param_dir = get_package_share_directory('demo_nodes_py') - param_file_path = os.path.join(param_dir, 'params.yaml') - future = client.load_parameter_file(param_file_path) - rclpy.spin_until_future_complete(node, future) - load_parameter_results = future.result() - if load_parameter_results is not None: - param_file_dict = parameter_dict_from_yaml_file(param_file_path) - for i, v in enumerate(param_file_dict.keys()): - node.get_logger().info(f' {v}:') - node.get_logger().info(f' successful: ' - f'{load_parameter_results.results[i].successful}') - node.get_logger().info(f' value: ' - f'{parameter_value_to_python(param_file_dict[v].value)}') - else: - node.get_logger().error(f'Error loading parameters: {future.exception()}') + logger.info('Getting parameters:') + future = client.get_parameters(param_list) + rclpy.spin_until_future_complete(node, future) + get_parameters_result = future.result() + if get_parameters_result is not None: + for i, v in enumerate(get_parameters_result.values): + logger.info(f' - {param_list[i]}: {parameter_value_to_python(v)}') + else: + logger.error(f'Error getting parameters: {future.exception()}') - node.get_logger().info('Deleting parameters: ') - params_to_delete = ['other_int_parameter', 'other_string_parameter', 'string_parameter'] - future = client.delete_parameters(params_to_delete) - rclpy.spin_until_future_complete(node, future) - delete_parameters_result = future.result() - if delete_parameters_result is not None: - for i, v in enumerate(delete_parameters_result.results): - node.get_logger().info(f' {params_to_delete[i]}:') - node.get_logger().info(f' successful: {v.successful}') - node.get_logger().info(f' reason: {v.reason}') - else: - node.get_logger().error(f'Error deleting parameters: {future.exception()}') + logger.info('Loading parameters: ') + param_dir = get_package_share_directory('demo_nodes_py') + param_file_path = os.path.join(param_dir, 'params.yaml') + future = client.load_parameter_file(param_file_path) + rclpy.spin_until_future_complete(node, future) + load_parameter_results = future.result() + if load_parameter_results is not None: + param_file_dict = parameter_dict_from_yaml_file(param_file_path) + for i, v in enumerate(param_file_dict.keys()): + logger.info(f' {v}:') + logger.info(f' successful: ' + f'{load_parameter_results.results[i].successful}') + logger.info(f' value: ' + f'{parameter_value_to_python(param_file_dict[v].value)}') + else: + logger.error(f'Error loading parameters: {future.exception()}') - rclpy.shutdown() + logger.info('Deleting parameters: ') + params_to_delete = ['other_int_parameter', 'other_string_parameter', + 'string_parameter'] + future = client.delete_parameters(params_to_delete) + rclpy.spin_until_future_complete(node, future) + delete_parameters_result = future.result() + if delete_parameters_result is not None: + for i, v in enumerate(delete_parameters_result.results): + logger.info(f' {params_to_delete[i]}:') + logger.info(f' successful: {v.successful}') + logger.info(f' reason: {v.reason}') + else: + logger.error(f'Error deleting parameters: {future.exception()}') + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/parameters/set_parameters_callback.py b/demo_nodes_py/demo_nodes_py/parameters/set_parameters_callback.py index 2e9c815ef..884a74721 100644 --- a/demo_nodes_py/demo_nodes_py/parameters/set_parameters_callback.py +++ b/demo_nodes_py/demo_nodes_py/parameters/set_parameters_callback.py @@ -73,17 +73,12 @@ def post_set_parameter_callback(parameter_list): def main(args=None): - rclpy.init(args=args) - - node = SetParametersCallback() - try: - rclpy.spin(node) + with rclpy.init(args=args): + node = SetParametersCallback() + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/services/add_two_ints_client.py b/demo_nodes_py/demo_nodes_py/services/add_two_ints_client.py index 5c6eab9d1..07ca330dc 100644 --- a/demo_nodes_py/demo_nodes_py/services/add_two_ints_client.py +++ b/demo_nodes_py/demo_nodes_py/services/add_two_ints_client.py @@ -15,28 +15,28 @@ from example_interfaces.srv import AddTwoInts import rclpy +from rclpy.executors import ExternalShutdownException def main(args=None): - rclpy.init(args=args) - - node = rclpy.create_node('add_two_ints_client') - - cli = node.create_client(AddTwoInts, 'add_two_ints') - while not cli.wait_for_service(timeout_sec=1.0): - print('service not available, waiting again...') - req = AddTwoInts.Request() - req.a = 2 - req.b = 3 - future = cli.call_async(req) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - node.get_logger().info('Result of add_two_ints: %d' % future.result().sum) - else: - node.get_logger().error('Exception while calling service: %r' % future.exception()) - - node.destroy_node() - rclpy.try_shutdown() + try: + with rclpy.init(args=args): + node = rclpy.create_node('add_two_ints_client') + + cli = node.create_client(AddTwoInts, 'add_two_ints') + while not cli.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + req = AddTwoInts.Request() + req.a = 2 + req.b = 3 + future = cli.call_async(req) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + node.get_logger().info('Result of add_two_ints: %d' % future.result().sum) + else: + node.get_logger().error('Exception while calling service: %r' % future.exception()) + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/services/add_two_ints_client_async.py b/demo_nodes_py/demo_nodes_py/services/add_two_ints_client_async.py index 20296f89a..8412df405 100644 --- a/demo_nodes_py/demo_nodes_py/services/add_two_ints_client_async.py +++ b/demo_nodes_py/demo_nodes_py/services/add_two_ints_client_async.py @@ -15,33 +15,35 @@ from example_interfaces.srv import AddTwoInts import rclpy +from rclpy.executors import ExternalShutdownException def main(args=None): - - rclpy.init(args=args) - - node = rclpy.create_node('add_two_ints_client') - - cli = node.create_client(AddTwoInts, 'add_two_ints') - - req = AddTwoInts.Request() - req.a = 2 - req.b = 3 - while not cli.wait_for_service(timeout_sec=1.0): - print('service not available, waiting again...') - future = cli.call_async(req) - while rclpy.ok(): - rclpy.spin_once(node) - if future.done(): - if future.result() is not None: - node.get_logger().info('Result of add_two_ints: %d' % future.result().sum) - else: - node.get_logger().error('Exception while calling service: %r' % future.exception()) - break - - node.destroy_node() - rclpy.try_shutdown() + try: + with rclpy.init(args=args): + node = rclpy.create_node('add_two_ints_client') + + cli = node.create_client(AddTwoInts, 'add_two_ints') + + req = AddTwoInts.Request() + req.a = 2 + req.b = 3 + while not cli.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + future = cli.call_async(req) + + logger = node.get_logger() + + while rclpy.ok(): + rclpy.spin_once(node) + if future.done(): + if future.result() is not None: + logger.info('Result of add_two_ints: %d' % future.result().sum) + else: + logger.error('Exception while calling service: %r' % future.exception()) + break + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/services/add_two_ints_server.py b/demo_nodes_py/demo_nodes_py/services/add_two_ints_server.py index 806e41bc5..92799b666 100644 --- a/demo_nodes_py/demo_nodes_py/services/add_two_ints_server.py +++ b/demo_nodes_py/demo_nodes_py/services/add_two_ints_server.py @@ -33,19 +33,13 @@ def add_two_ints_callback(self, request, response): def main(args=None): - rclpy.init(args=args) - - node = AddTwoIntsServer() - try: - rclpy.spin(node) + with rclpy.init(args=args): + node = AddTwoIntsServer() + + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - # Destroy the node explicitly - # (optional - Done automatically when node is garbage collected) - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/services/introspection.py b/demo_nodes_py/demo_nodes_py/services/introspection.py index 15bfe3efb..4712902ff 100644 --- a/demo_nodes_py/demo_nodes_py/services/introspection.py +++ b/demo_nodes_py/demo_nodes_py/services/introspection.py @@ -186,25 +186,19 @@ def add_two_ints_callback(self, request, response): def main(args=None): - rclpy.init(args=args) - - service_node = IntrospectionServiceNode() + try: + with rclpy.init(args=args): + service_node = IntrospectionServiceNode() - client_node = IntrospectionClientNode() + client_node = IntrospectionClientNode() - executor = SingleThreadedExecutor() - executor.add_node(service_node) - executor.add_node(client_node) + executor = SingleThreadedExecutor() + executor.add_node(service_node) + executor.add_node(client_node) - try: - executor.spin() + executor.spin() except (KeyboardInterrupt, ExternalShutdownException): - executor.remove_node(client_node) - executor.remove_node(service_node) - executor.shutdown() - service_node.destroy_node() - client_node.destroy_node() - rclpy.try_shutdown() + pass if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/topics/listener.py b/demo_nodes_py/demo_nodes_py/topics/listener.py index e52354a2b..2b8885385 100644 --- a/demo_nodes_py/demo_nodes_py/topics/listener.py +++ b/demo_nodes_py/demo_nodes_py/topics/listener.py @@ -30,16 +30,12 @@ def chatter_callback(self, msg): def main(args=None): - rclpy.init(args=args) - - node = Listener() try: - rclpy.spin(node) + with rclpy.init(args=args): + node = Listener() + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/topics/listener_qos.py b/demo_nodes_py/demo_nodes_py/topics/listener_qos.py index 5c9ce74d9..af0dd3489 100644 --- a/demo_nodes_py/demo_nodes_py/topics/listener_qos.py +++ b/demo_nodes_py/demo_nodes_py/topics/listener_qos.py @@ -52,27 +52,23 @@ def main(argv=sys.argv[1:]): help='max number of spin_once iterations') args = parser.parse_args(remove_ros_args(args=argv)) - rclpy.init(args=argv) - - if args.reliable: - custom_qos_profile = QoSProfile( - depth=10, - reliability=QoSReliabilityPolicy.RELIABLE) - else: - custom_qos_profile = qos_profile_sensor_data + try: + with rclpy.init(args=argv): + if args.reliable: + custom_qos_profile = QoSProfile( + depth=10, + reliability=QoSReliabilityPolicy.RELIABLE) + else: + custom_qos_profile = qos_profile_sensor_data - node = ListenerQos(custom_qos_profile) + node = ListenerQos(custom_qos_profile) - cycle_count = 0 - try: - while rclpy.ok() and cycle_count < args.number_of_cycles: - rclpy.spin_once(node) - cycle_count += 1 + cycle_count = 0 + while rclpy.ok() and cycle_count < args.number_of_cycles: + rclpy.spin_once(node) + cycle_count += 1 except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/topics/listener_serialized.py b/demo_nodes_py/demo_nodes_py/topics/listener_serialized.py index d768cf23e..2fc68e037 100644 --- a/demo_nodes_py/demo_nodes_py/topics/listener_serialized.py +++ b/demo_nodes_py/demo_nodes_py/topics/listener_serialized.py @@ -35,20 +35,13 @@ def listener_callback(self, msg): def main(args=None): - rclpy.init(args=args) - - serialized_subscriber = SerializedSubscriber() - try: - rclpy.spin(serialized_subscriber) + with rclpy.init(args=args): + serialized_subscriber = SerializedSubscriber() + + rclpy.spin(serialized_subscriber) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - serialized_subscriber.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/topics/talker.py b/demo_nodes_py/demo_nodes_py/topics/talker.py index 119c342a0..3d9437d48 100644 --- a/demo_nodes_py/demo_nodes_py/topics/talker.py +++ b/demo_nodes_py/demo_nodes_py/topics/talker.py @@ -37,17 +37,13 @@ def timer_callback(self): def main(args=None): - rclpy.init(args=args) - - node = Talker() - try: - rclpy.spin(node) + with rclpy.init(args=args): + node = Talker() + + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/demo_nodes_py/demo_nodes_py/topics/talker_qos.py b/demo_nodes_py/demo_nodes_py/topics/talker_qos.py index ee2b9a166..3079fd98c 100644 --- a/demo_nodes_py/demo_nodes_py/topics/talker_qos.py +++ b/demo_nodes_py/demo_nodes_py/topics/talker_qos.py @@ -59,27 +59,23 @@ def main(argv=sys.argv[1:]): help='number of sending attempts') args = parser.parse_args(remove_ros_args(args=argv)) - rclpy.init(args=argv) - - if args.reliable: - custom_qos_profile = QoSProfile( - depth=10, - reliability=QoSReliabilityPolicy.RELIABLE) - else: - custom_qos_profile = qos_profile_sensor_data - - node = TalkerQos(custom_qos_profile) - - cycle_count = 0 try: - while rclpy.ok() and cycle_count < args.number_of_cycles: - rclpy.spin_once(node) - cycle_count += 1 + with rclpy.init(args=argv): + if args.reliable: + custom_qos_profile = QoSProfile( + depth=10, + reliability=QoSReliabilityPolicy.RELIABLE) + else: + custom_qos_profile = qos_profile_sensor_data + + node = TalkerQos(custom_qos_profile) + + cycle_count = 0 + while rclpy.ok() and cycle_count < args.number_of_cycles: + rclpy.spin_once(node) + cycle_count += 1 except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/lifecycle_py/launch/lifecycle_demo_launch.py b/lifecycle_py/launch/lifecycle_demo_launch.py index 16dbc03ed..309373e8b 100755 --- a/lifecycle_py/launch/lifecycle_demo_launch.py +++ b/lifecycle_py/launch/lifecycle_demo_launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): return LaunchDescription([ - LifecycleNode(package='lifecycle', executable='lifecycle_talker', + LifecycleNode(package='lifecycle_py', executable='lifecycle_talker', name='lc_talker', namespace='', output='screen'), Node(package='lifecycle', executable='lifecycle_listener', output='screen'), Node(package='lifecycle', executable='lifecycle_service_client', output='screen') diff --git a/lifecycle_py/lifecycle_py/talker.py b/lifecycle_py/lifecycle_py/talker.py index c825bd58d..29dc7c1c6 100644 --- a/lifecycle_py/lifecycle_py/talker.py +++ b/lifecycle_py/lifecycle_py/talker.py @@ -15,6 +15,8 @@ from typing import Optional import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.executors import SingleThreadedExecutor # Node, State and Publisher are aliases for LifecycleNode, LifecycleState and LifecyclePublisher # respectively. @@ -142,15 +144,14 @@ def on_shutdown(self, state: State) -> TransitionCallbackReturn: # node, give it a name and add it to the executor. def main(): - rclpy.init() - - executor = rclpy.executors.SingleThreadedExecutor() - lc_node = LifecycleTalker('lc_talker') - executor.add_node(lc_node) try: - executor.spin() - except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): - lc_node.destroy_node() + with rclpy.init(): + executor = SingleThreadedExecutor() + lc_node = LifecycleTalker('lc_talker') + executor.add_node(lc_node) + executor.spin() + except (KeyboardInterrupt, ExternalShutdownException): + pass if __name__ == '__main__': diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/deadline.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/deadline.py index 5550bbe74..d23822299 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/deadline.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/deadline.py @@ -44,47 +44,46 @@ def parse_args(): def main(args=None): - parsed_args = parse_args() - rclpy.init(args=args) - - topic = 'qos_deadline_chatter' - deadline = Duration(seconds=parsed_args.deadline / 1000.0) - - qos_profile = QoSProfile( - depth=10, - deadline=deadline) - - def sub_deadline_event(event): - count = event.total_count - delta = event.total_count_change - get_logger('listener').info(f'Requested deadline missed - total {count} delta {delta}') - - subscription_callbacks = SubscriptionEventCallbacks(deadline=sub_deadline_event) - listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) - - def pub_deadline_event(event): - count = event.total_count - delta = event.total_count_change - get_logger('talker').info(f'Offered deadline missed - total {count} delta {delta}') - - publisher_callbacks = PublisherEventCallbacks(deadline=pub_deadline_event) - talker = Talker(topic, qos_profile, event_callbacks=publisher_callbacks) - - publish_for_seconds = parsed_args.publish_for / 1000.0 - pause_for_seconds = parsed_args.pause_for / 1000.0 - pause_timer = talker.create_timer( # noqa: F841 - publish_for_seconds, - lambda: talker.pause_for(pause_for_seconds)) - - executor = SingleThreadedExecutor() - executor.add_node(listener) - executor.add_node(talker) try: - executor.spin() + parsed_args = parse_args() + + with rclpy.init(args=args): + topic = 'qos_deadline_chatter' + deadline = Duration(seconds=parsed_args.deadline / 1000.0) + + qos_profile = QoSProfile( + depth=10, + deadline=deadline) + + def sub_deadline_event(event): + count = event.total_count + delta = event.total_count_change + get_logger('listener').info( + f'Requested deadline missed - total {count} delta {delta}') + + subscription_callbacks = SubscriptionEventCallbacks(deadline=sub_deadline_event) + listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) + + def pub_deadline_event(event): + count = event.total_count + delta = event.total_count_change + get_logger('talker').info(f'Offered deadline missed - total {count} delta {delta}') + + publisher_callbacks = PublisherEventCallbacks(deadline=pub_deadline_event) + talker = Talker(topic, qos_profile, event_callbacks=publisher_callbacks) + + publish_for_seconds = parsed_args.publish_for / 1000.0 + pause_for_seconds = parsed_args.pause_for / 1000.0 + pause_timer = talker.create_timer( # noqa: F841 + publish_for_seconds, + lambda: talker.pause_for(pause_for_seconds)) + + executor = SingleThreadedExecutor() + executor.add_node(listener) + executor.add_node(talker) + executor.spin() except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/incompatible_qos.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/incompatible_qos.py index f1d3a8b02..1efcc4f9d 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/incompatible_qos.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/incompatible_qos.py @@ -44,120 +44,120 @@ def get_parser(): def main(args=None): - # Argument parsing and usage - parser = get_parser() - parsed_args = parser.parse_args() - - # Configuration variables - qos_policy_name = parsed_args.incompatible_qos_policy_name - qos_profile_publisher = QoSProfile(depth=10) - qos_profile_subscription = QoSProfile(depth=10) - - if qos_policy_name == 'durability': - print( - 'Durability incompatibility selected.\n' - 'Incompatibility condition: publisher durability kind < ' - 'subscription durability kind.\n' - 'Setting publisher durability to: VOLATILE\n' - 'Setting subscription durability to: TRANSIENT_LOCAL\n' - ) - qos_profile_publisher.durability = \ - QoSDurabilityPolicy.VOLATILE - qos_profile_subscription.durability = \ - QoSDurabilityPolicy.TRANSIENT_LOCAL - elif qos_policy_name == 'deadline': - print( - 'Deadline incompatibility selected.\n' - 'Incompatibility condition: publisher deadline > subscription deadline.\n' - 'Setting publisher durability to: 2 seconds\n' - 'Setting subscription durability to: 1 second\n' - ) - qos_profile_publisher.deadline = Duration(seconds=2) - qos_profile_subscription.deadline = Duration(seconds=1) - elif qos_policy_name == 'liveliness_policy': - print( - 'Liveliness Policy incompatibility selected.\n' - 'Incompatibility condition: publisher liveliness policy <' - 'subscripition liveliness policy.\n' - 'Setting publisher liveliness policy to: AUTOMATIC\n' - 'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n' - ) - qos_profile_publisher.liveliness = \ - QoSLivelinessPolicy.AUTOMATIC - qos_profile_subscription.liveliness = \ - QoSLivelinessPolicy.MANUAL_BY_TOPIC - elif qos_policy_name == 'liveliness_lease_duration': - print( - 'Liveliness lease duration incompatibility selected.\n' - 'Incompatibility condition: publisher liveliness lease duration >' - 'subscription liveliness lease duration.\n' - 'Setting publisher liveliness lease duration to: 2 seconds\n' - 'Setting subscription liveliness lease duration to: 1 second\n' - ) - qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2) - qos_profile_subscription.liveliness_lease_duration = Duration(seconds=1) - elif qos_policy_name == 'reliability': - print( - 'Reliability incompatibility selected.\n' - 'Incompatibility condition: publisher reliability < subscripition reliability.\n' - 'Setting publisher reliability to: BEST_EFFORT\n' - 'Setting subscription reliability to: RELIABLE\n' - ) - qos_profile_publisher.reliability = \ - QoSReliabilityPolicy.BEST_EFFORT - qos_profile_subscription.reliability = \ - QoSReliabilityPolicy.RELIABLE - else: - print('{name} not recognised.'.format(name=qos_policy_name)) - parser.print_help() - return 1 - - # Initialization and configuration - rclpy.init(args=args) - topic = 'incompatible_qos_chatter' - num_msgs = 5 - - def sub_incompatible_qos_event(event): - count = event.total_count - delta = event.total_count_change - policy = event.last_policy_kind - get_logger('listener').info( - f'Requested incompatible qos - total {count} delta {delta} last_policy_kind: {policy}') - - def pub_incompatible_qos_event(event): - count = event.total_count - delta = event.total_count_change - policy = event.last_policy_kind - get_logger('talker').info( - f'Offered incompatible qos - total {count} delta {delta} last_policy_kind: {policy}') - - publisher_callbacks = PublisherEventCallbacks(incompatible_qos=pub_incompatible_qos_event) - subscription_callbacks = SubscriptionEventCallbacks( - incompatible_qos=sub_incompatible_qos_event) - - try: - talker = Talker( - topic, qos_profile_publisher, event_callbacks=publisher_callbacks, - publish_count=num_msgs) - listener = Listener( - topic, qos_profile_subscription, event_callbacks=subscription_callbacks) - except UnsupportedEventTypeError as exc: - print() - print(exc, end='\n\n') - print('Please try this demo using a different RMW implementation') - return 1 - - executor = SingleThreadedExecutor() - executor.add_node(listener) - executor.add_node(talker) - try: - while talker.publish_count < num_msgs: - executor.spin_once() + # Argument parsing and usage + parser = get_parser() + parsed_args = parser.parse_args() + + # Configuration variables + qos_policy_name = parsed_args.incompatible_qos_policy_name + qos_profile_publisher = QoSProfile(depth=10) + qos_profile_subscription = QoSProfile(depth=10) + + if qos_policy_name == 'durability': + print( + 'Durability incompatibility selected.\n' + 'Incompatibility condition: publisher durability kind < ' + 'subscription durability kind.\n' + 'Setting publisher durability to: VOLATILE\n' + 'Setting subscription durability to: TRANSIENT_LOCAL\n' + ) + qos_profile_publisher.durability = \ + QoSDurabilityPolicy.VOLATILE + qos_profile_subscription.durability = \ + QoSDurabilityPolicy.TRANSIENT_LOCAL + elif qos_policy_name == 'deadline': + print( + 'Deadline incompatibility selected.\n' + 'Incompatibility condition: publisher deadline > subscription deadline.\n' + 'Setting publisher durability to: 2 seconds\n' + 'Setting subscription durability to: 1 second\n' + ) + qos_profile_publisher.deadline = Duration(seconds=2) + qos_profile_subscription.deadline = Duration(seconds=1) + elif qos_policy_name == 'liveliness_policy': + print( + 'Liveliness Policy incompatibility selected.\n' + 'Incompatibility condition: publisher liveliness policy <' + 'subscripition liveliness policy.\n' + 'Setting publisher liveliness policy to: AUTOMATIC\n' + 'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n' + ) + qos_profile_publisher.liveliness = \ + QoSLivelinessPolicy.AUTOMATIC + qos_profile_subscription.liveliness = \ + QoSLivelinessPolicy.MANUAL_BY_TOPIC + elif qos_policy_name == 'liveliness_lease_duration': + print( + 'Liveliness lease duration incompatibility selected.\n' + 'Incompatibility condition: publisher liveliness lease duration >' + 'subscription liveliness lease duration.\n' + 'Setting publisher liveliness lease duration to: 2 seconds\n' + 'Setting subscription liveliness lease duration to: 1 second\n' + ) + qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2) + qos_profile_subscription.liveliness_lease_duration = Duration(seconds=1) + elif qos_policy_name == 'reliability': + print( + 'Reliability incompatibility selected.\n' + 'Incompatibility condition: publisher reliability < subscripition reliability.\n' + 'Setting publisher reliability to: BEST_EFFORT\n' + 'Setting subscription reliability to: RELIABLE\n' + ) + qos_profile_publisher.reliability = \ + QoSReliabilityPolicy.BEST_EFFORT + qos_profile_subscription.reliability = \ + QoSReliabilityPolicy.RELIABLE + else: + print('{name} not recognised.'.format(name=qos_policy_name)) + parser.print_help() + return 1 + + # Initialization and configuration + with rclpy.init(args=args): + topic = 'incompatible_qos_chatter' + num_msgs = 5 + + def sub_incompatible_qos_event(event): + count = event.total_count + delta = event.total_count_change + policy = event.last_policy_kind + get_logger('listener').info( + f'Requested incompatible qos - total {count} delta {delta} ' + f'last_policy_kind: {policy}') + + def pub_incompatible_qos_event(event): + count = event.total_count + delta = event.total_count_change + policy = event.last_policy_kind + get_logger('talker').info( + f'Offered incompatible qos - total {count} delta {delta} ' + f'last_policy_kind: {policy}') + + pub_callbacks = PublisherEventCallbacks(incompatible_qos=pub_incompatible_qos_event) + subscription_callbacks = SubscriptionEventCallbacks( + incompatible_qos=sub_incompatible_qos_event) + + try: + talker = Talker( + topic, qos_profile_publisher, event_callbacks=pub_callbacks, + publish_count=num_msgs) + listener = Listener( + topic, qos_profile_subscription, event_callbacks=subscription_callbacks) + except UnsupportedEventTypeError as exc: + print() + print(exc, end='\n\n') + print('Please try this demo using a different RMW implementation') + return 1 + + executor = SingleThreadedExecutor() + executor.add_node(listener) + executor.add_node(talker) + + while talker.publish_count < num_msgs: + executor.spin_once() except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/lifespan.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/lifespan.py index ede64250e..a7838f360 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/lifespan.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/lifespan.py @@ -47,37 +47,34 @@ def parse_args(): def main(args=None): - parsed_args = parse_args() - rclpy.init(args=args) - - topic = 'qos_lifespan_chatter' - lifespan = Duration(seconds=parsed_args.lifespan / 1000.0) + try: + parsed_args = parse_args() + with rclpy.init(args=args): + topic = 'qos_lifespan_chatter' + lifespan = Duration(seconds=parsed_args.lifespan / 1000.0) - qos_profile = QoSProfile( - depth=parsed_args.history, - # Guaranteed delivery is needed to send messages to late-joining subscription. - reliability=QoSReliabilityPolicy.RELIABLE, - # Store messages on the publisher so that they can be affected by Lifespan. - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - lifespan=lifespan) + qos_profile = QoSProfile( + depth=parsed_args.history, + # Guaranteed delivery is needed to send messages to late-joining subscription. + reliability=QoSReliabilityPolicy.RELIABLE, + # Store messages on the publisher so that they can be affected by Lifespan. + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + lifespan=lifespan) - listener = Listener( - topic, qos_profile, event_callbacks=None, defer_subscribe=True) - talker = Talker( - topic, qos_profile, event_callbacks=None, publish_count=parsed_args.publish_count) - subscribe_timer = listener.create_timer( # noqa: F841 - parsed_args.subscribe_after / 1000.0, - lambda: listener.start_listening()) + listener = Listener( + topic, qos_profile, event_callbacks=None, defer_subscribe=True) + talker = Talker( + topic, qos_profile, event_callbacks=None, publish_count=parsed_args.publish_count) + subscribe_timer = listener.create_timer( # noqa: F841 + parsed_args.subscribe_after / 1000.0, + lambda: listener.start_listening()) - executor = SingleThreadedExecutor() - executor.add_node(listener) - executor.add_node(talker) - try: - executor.spin() + executor = SingleThreadedExecutor() + executor.add_node(listener) + executor.add_node(talker) + executor.spin() except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/liveliness.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/liveliness.py index 9a4ebec6e..147aa6dfa 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/liveliness.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/liveliness.py @@ -54,64 +54,64 @@ def parse_args(): def main(args=None): - parsed_args = parse_args() - rclpy.init(args=args) - - topic = 'qos_liveliness_chatter' - liveliness_lease_duration = Duration(seconds=parsed_args.liveliness_lease_duration / 1000.0) - liveliness_policy = POLICY_MAP[parsed_args.policy] - - qos_profile = QoSProfile( - depth=10, - liveliness=liveliness_policy, - liveliness_lease_duration=liveliness_lease_duration) - - def sub_liveliness_event(event): - get_logger('listener').info('Liveliness changed event:') - get_logger('listener').info(f' alive_count: {event.alive_count}') - get_logger('listener').info(f' not_alive_count: {event.not_alive_count}') - get_logger('listener').info(f' alive_count_change: {event.alive_count_change}') - get_logger('listener').info(f' not_alive_count_change: {event.not_alive_count_change}') - - subscription_callbacks = SubscriptionEventCallbacks(liveliness=sub_liveliness_event) - listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) - - def pub_liveliness_event(event): - get_logger('talker').info('Liveliness changed event:') - get_logger('talker').info(f' alive_count: {event.alive_count}') - get_logger('talker').info(f' not_alive_count: {event.not_alive_count}') - get_logger('talker').info(f' alive_count_change: {event.alive_count_change}') - get_logger('talker').info(f' not_alive_count_change: {event.not_alive_count_change}') - - publisher_callbacks = PublisherEventCallbacks(liveliness=pub_liveliness_event) - talker = Talker( - topic, qos_profile, - event_callbacks=publisher_callbacks, - assert_topic_period=parsed_args.topic_assert_period / 1000.0) - - executor = SingleThreadedExecutor() - - def kill_talker(): - if liveliness_policy == QoSLivelinessPolicy.AUTOMATIC: - executor.remove_node(talker) - talker.destroy_node() - elif liveliness_policy == QoSLivelinessPolicy.MANUAL_BY_TOPIC: - talker.stop() - kill_timer.cancel() - - if parsed_args.kill_publisher_after > 0: - kill_timer = listener.create_timer( # noqa: F841 - parsed_args.kill_publisher_after / 1000.0, - kill_talker) - - executor.add_node(listener) - executor.add_node(talker) try: - executor.spin() + parsed_args = parse_args() + with rclpy.init(args=args): + topic = 'qos_liveliness_chatter' + liveliness_lease_duration = Duration( + seconds=parsed_args.liveliness_lease_duration / 1000.0) + liveliness_policy = POLICY_MAP[parsed_args.policy] + + qos_profile = QoSProfile( + depth=10, + liveliness=liveliness_policy, + liveliness_lease_duration=liveliness_lease_duration) + + def sub_liveliness_event(event): + get_logger('listener').info('Liveliness changed event:') + get_logger('listener').info(f' alive_count: {event.alive_count}') + get_logger('listener').info(f' not_alive_count: {event.not_alive_count}') + get_logger('listener').info(f' alive_count_change: {event.alive_count_change}') + get_logger('listener').info( + f' not_alive_count_change: {event.not_alive_count_change}') + + subscription_callbacks = SubscriptionEventCallbacks(liveliness=sub_liveliness_event) + listener = Listener(topic, qos_profile, event_callbacks=subscription_callbacks) + + def pub_liveliness_event(event): + get_logger('talker').info('Liveliness changed event:') + get_logger('talker').info(f' alive_count: {event.alive_count}') + get_logger('talker').info(f' not_alive_count: {event.not_alive_count}') + get_logger('talker').info(f' alive_count_change: {event.alive_count_change}') + get_logger('talker').info( + f' not_alive_count_change: {event.not_alive_count_change}') + + publisher_callbacks = PublisherEventCallbacks(liveliness=pub_liveliness_event) + talker = Talker( + topic, qos_profile, + event_callbacks=publisher_callbacks, + assert_topic_period=parsed_args.topic_assert_period / 1000.0) + + executor = SingleThreadedExecutor() + + def kill_talker(): + if liveliness_policy == QoSLivelinessPolicy.AUTOMATIC: + executor.remove_node(talker) + talker.destroy_node() + elif liveliness_policy == QoSLivelinessPolicy.MANUAL_BY_TOPIC: + talker.stop() + kill_timer.cancel() + + if parsed_args.kill_publisher_after > 0: + kill_timer = listener.create_timer( # noqa: F841 + parsed_args.kill_publisher_after / 1000.0, + kill_talker) + + executor.add_node(listener) + executor.add_node(talker) + executor.spin() except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py index 7a34a739c..caf991c64 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py @@ -61,18 +61,15 @@ def _message_lost_event_callback(self, message_lost_status): def main(args=None): - rclpy.init(args=args) - - listener = MessageLostListener() - executor = SingleThreadedExecutor() - executor.add_node(listener) - try: - executor.spin() + with rclpy.init(args=args): + listener = MessageLostListener() + executor = SingleThreadedExecutor() + executor.add_node(listener) + + executor.spin() except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_listener.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_listener.py index 15e92c9a6..08f697389 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_listener.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_listener.py @@ -48,16 +48,12 @@ def qos_callback(self, qos): def main(args=None): - rclpy.init(args=args) - - node = Listener() try: - rclpy.spin(node) + with rclpy.init(args=args): + node = Listener() + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() return 0 diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_talker.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_talker.py index 9747c3208..ac7cf02a7 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_talker.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_talker.py @@ -55,17 +55,13 @@ def qos_callback(self, qos): def main(args=None): - rclpy.init(args=args) - - node = Talker() - try: - rclpy.spin(node) + with rclpy.init(args=args): + node = Talker() + + rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass - finally: - node.destroy_node() - rclpy.try_shutdown() return 0