From 05aa97e547fd7f18c53e9936d78a6380819eb843 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 9 May 2024 08:05:19 -0400 Subject: [PATCH] Update the shutdown handling in all of the Python examples. (#379) * Update the shutdown handling in all of the Python examples. In particular, we should deal with KeyboardInterrupt (by just doing 'pass'), as well as ExternalShutdownException (which can come from the executor). In all cases, we should call 'rclpy.try_shutdown()' to cleanup at the end. Note well that I removed some of the *destroy() calls in the cleanup. That's because they weren't correct in all cases, and to fix them up properly would really require us to have a nested set of try..except statements. Given that these are examples, having that complex set of exception handling didn't seem like the correct way to go here. Signed-off-by: Chris Lalancette --- .../client.py | 14 +++- .../client_cancel.py | 14 +++- .../client_not_composable.py | 66 +++++++++-------- .../server.py | 18 +++-- .../server_defer.py | 18 +++-- .../server_not_composable.py | 48 ++++++------ .../server_queue_goals.py | 16 ++-- .../server_single_goal.py | 18 +++-- .../callback_group.py | 16 ++-- .../examples_rclpy_executors/composed.py | 11 +-- .../custom_callback_group.py | 3 - .../custom_executor.py | 16 ++-- .../examples_rclpy_executors/listener.py | 3 - .../examples_rclpy_executors/talker.py | 3 - .../trigger_guard_condition.py | 57 +++++++------- .../examples_rclpy_minimal_client/client.py | 42 ++++++----- .../client_async.py | 49 ++++++------ .../client_async_callback.py | 74 ++++++++++--------- .../client_async_member_function.py | 33 +++++---- .../examples_rclpy_minimal_service/service.py | 22 +++--- .../service_member_function.py | 14 +++- .../publisher_local_function.py | 50 +++++++------ .../publisher_member_function.py | 18 +++-- .../publisher_old_school.py | 33 +++++---- .../subscriber_lambda.py | 24 +++--- .../subscriber_member_function.py | 18 +++-- .../subscriber_old_school.py | 24 +++--- .../pointcloud_publisher.py | 14 +++- 28 files changed, 407 insertions(+), 329 deletions(-) diff --git a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py index 2b8dc18f..384672d5 100644 --- a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py +++ b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py @@ -12,11 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from action_msgs.msg import GoalStatus from example_interfaces.action import Fibonacci import rclpy from rclpy.action import ActionClient +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -70,11 +73,16 @@ def send_goal(self): def main(args=None): rclpy.init(args=args) - action_client = MinimalActionClient() + try: + action_client = MinimalActionClient() - action_client.send_goal() + action_client.send_goal() - rclpy.spin(action_client) + rclpy.spin(action_client) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_cancel.py b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_cancel.py index 5258faf2..35e3bd6f 100644 --- a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_cancel.py +++ b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_cancel.py @@ -12,10 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from example_interfaces.action import Fibonacci import rclpy from rclpy.action import ActionClient +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -78,11 +81,16 @@ def send_goal(self): def main(args=None): rclpy.init(args=args) - action_client = MinimalActionClient() + try: + action_client = MinimalActionClient() - action_client.send_goal() + action_client.send_goal() - rclpy.spin(action_client) + rclpy.spin(action_client) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_not_composable.py b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_not_composable.py index 26af729b..e1ddbe89 100644 --- a/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_not_composable.py +++ b/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_not_composable.py @@ -12,12 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from action_msgs.msg import GoalStatus from example_interfaces.action import Fibonacci import rclpy from rclpy.action import ActionClient +from rclpy.executors import ExternalShutdownException def feedback_cb(logger, feedback): @@ -27,50 +30,51 @@ def feedback_cb(logger, feedback): def main(args=None): rclpy.init(args=args) - node = rclpy.create_node('minimal_action_client') - - action_client = ActionClient(node, Fibonacci, 'fibonacci') + try: + node = rclpy.create_node('minimal_action_client') - node.get_logger().info('Waiting for action server...') + action_client = ActionClient(node, Fibonacci, 'fibonacci') - action_client.wait_for_server() + node.get_logger().info('Waiting for action server...') - goal_msg = Fibonacci.Goal() - goal_msg.order = 10 + action_client.wait_for_server() - node.get_logger().info('Sending goal request...') + goal_msg = Fibonacci.Goal() + goal_msg.order = 10 - send_goal_future = action_client.send_goal_async( - goal_msg, feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback)) + node.get_logger().info('Sending goal request...') - rclpy.spin_until_future_complete(node, send_goal_future) + send_goal_future = action_client.send_goal_async( + goal_msg, feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback)) - goal_handle = send_goal_future.result() + rclpy.spin_until_future_complete(node, send_goal_future) - if not goal_handle.accepted: - node.get_logger().info('Goal rejected :(') - action_client.destroy() - node.destroy_node() - rclpy.shutdown() - return + goal_handle = send_goal_future.result() - node.get_logger().info('Goal accepted :)') + if not goal_handle.accepted: + node.get_logger().info('Goal rejected :(') + action_client.destroy() + node.destroy_node() + rclpy.shutdown() + return - get_result_future = goal_handle.get_result_async() + node.get_logger().info('Goal accepted :)') - rclpy.spin_until_future_complete(node, get_result_future) + get_result_future = goal_handle.get_result_async() - result = get_result_future.result().result - status = get_result_future.result().status - if status == GoalStatus.STATUS_SUCCEEDED: - node.get_logger().info( - 'Goal succeeded! Result: {0}'.format(result.sequence)) - else: - node.get_logger().info('Goal failed with status code: {0}'.format(status)) + rclpy.spin_until_future_complete(node, get_result_future) - action_client.destroy() - node.destroy_node() - rclpy.shutdown() + result = get_result_future.result().result + status = get_result_future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + node.get_logger().info( + 'Goal succeeded! Result: {0}'.format(result.sequence)) + else: + node.get_logger().info('Goal failed with status code: {0}'.format(status)) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server.py index 47fb42ad..599a367a 100644 --- a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server.py +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys import time from example_interfaces.action import Fibonacci @@ -19,6 +20,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -92,15 +94,17 @@ async def execute_callback(self, goal_handle): def main(args=None): rclpy.init(args=args) - minimal_action_server = MinimalActionServer() + try: + minimal_action_server = MinimalActionServer() - # Use a MultiThreadedExecutor to enable processing goals concurrently - executor = MultiThreadedExecutor() + # Use a MultiThreadedExecutor to enable processing goals concurrently + executor = MultiThreadedExecutor() - rclpy.spin(minimal_action_server, executor=executor) - - minimal_action_server.destroy() - rclpy.shutdown() + rclpy.spin(minimal_action_server, executor=executor) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_defer.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_defer.py index 21c93d4c..b6abcb01 100644 --- a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_defer.py +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_defer.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys import time from example_interfaces.action import Fibonacci @@ -19,6 +20,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -106,15 +108,17 @@ async def execute_callback(self, goal_handle): def main(args=None): rclpy.init(args=args) - minimal_action_server = MinimalActionServer() + try: + minimal_action_server = MinimalActionServer() - # Use a MultiThreadedExecutor to enable processing goals concurrently - executor = MultiThreadedExecutor() + # Use a MultiThreadedExecutor to enable processing goals concurrently + executor = MultiThreadedExecutor() - rclpy.spin(minimal_action_server, executor=executor) - - minimal_action_server.destroy() - rclpy.shutdown() + rclpy.spin(minimal_action_server, executor=executor) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_not_composable.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_not_composable.py index 61ef22fc..60bb3705 100644 --- a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_not_composable.py +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_not_composable.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys import time from example_interfaces.action import Fibonacci @@ -19,6 +20,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor @@ -71,28 +73,30 @@ def main(args=None): global logger rclpy.init(args=args) - node = rclpy.create_node('minimal_action_server') - logger = node.get_logger() - - # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently - # Default goal callback accepts all goals - # Default cancel callback rejects cancel requests - action_server = ActionServer( - node, - Fibonacci, - 'fibonacci', - execute_callback=execute_callback, - cancel_callback=cancel_callback, - callback_group=ReentrantCallbackGroup()) - - # Use a MultiThreadedExecutor to enable processing goals concurrently - executor = MultiThreadedExecutor() - - rclpy.spin(node, executor=executor) - - action_server.destroy() - node.destroy_node() - rclpy.shutdown() + try: + node = rclpy.create_node('minimal_action_server') + logger = node.get_logger() + + # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently + # Default goal callback accepts all goals + # Default cancel callback rejects cancel requests + action_server = ActionServer( + node, + Fibonacci, + 'fibonacci', + execute_callback=execute_callback, + cancel_callback=cancel_callback, + callback_group=ReentrantCallbackGroup()) + action_server # Quiet flake8 warnings about unused variable + + # Use a MultiThreadedExecutor to enable processing goals concurrently + executor = MultiThreadedExecutor() + + rclpy.spin(node, executor=executor) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_queue_goals.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_queue_goals.py index abcde954..eae970fb 100644 --- a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_queue_goals.py +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_queue_goals.py @@ -13,6 +13,7 @@ # limitations under the License. import collections +import sys import threading import time @@ -21,6 +22,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -123,14 +125,16 @@ def execute_callback(self, goal_handle): def main(args=None): rclpy.init(args=args) - minimal_action_server = MinimalActionServer() + try: + minimal_action_server = MinimalActionServer() - executor = MultiThreadedExecutor() + executor = MultiThreadedExecutor() - rclpy.spin(minimal_action_server, executor=executor) - - minimal_action_server.destroy() - rclpy.shutdown() + rclpy.spin(minimal_action_server, executor=executor) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py index e7fad810..fe8824c6 100644 --- a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys import threading import time @@ -20,6 +21,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -112,14 +114,16 @@ def execute_callback(self, goal_handle): def main(args=None): rclpy.init(args=args) - action_server = MinimalActionServer() + try: + action_server = MinimalActionServer() - # We use a MultiThreadedExecutor to handle incoming goal requests concurrently - executor = MultiThreadedExecutor() - rclpy.spin(action_server, executor=executor) - - action_server.destroy() - rclpy.shutdown() + # We use a MultiThreadedExecutor to handle incoming goal requests concurrently + executor = MultiThreadedExecutor() + rclpy.spin(action_server, executor=executor) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/callback_group.py b/rclpy/executors/examples_rclpy_executors/callback_group.py index a13c654f..3a71a894 100644 --- a/rclpy/executors/examples_rclpy_executors/callback_group.py +++ b/rclpy/executors/examples_rclpy_executors/callback_group.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from examples_rclpy_executors.listener import Listener import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_msgs.msg import String @@ -58,14 +61,11 @@ def main(args=None): executor.add_node(talker) executor.add_node(listener) - try: - executor.spin() - finally: - executor.shutdown() - listener.destroy_node() - talker.destroy_node() - finally: - rclpy.shutdown() + executor.spin() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/composed.py b/rclpy/executors/examples_rclpy_executors/composed.py index d203efde..c88fa6e0 100644 --- a/rclpy/executors/examples_rclpy_executors/composed.py +++ b/rclpy/executors/examples_rclpy_executors/composed.py @@ -33,19 +33,12 @@ def main(args=None): executor.add_node(talker) executor.add_node(listener) - try: - # Execute callbacks for both nodes as they become ready - executor.spin() - finally: - executor.shutdown() - listener.destroy_node() - talker.destroy_node() + # Execute callbacks for both nodes as they become ready + executor.spin() except KeyboardInterrupt: pass except ExternalShutdownException: sys.exit(1) - finally: - rclpy.try_shutdown() if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/custom_callback_group.py b/rclpy/executors/examples_rclpy_executors/custom_callback_group.py index 2971b85a..056ce3a2 100644 --- a/rclpy/executors/examples_rclpy_executors/custom_callback_group.py +++ b/rclpy/executors/examples_rclpy_executors/custom_callback_group.py @@ -107,9 +107,6 @@ def main(args=None): pass except ExternalShutdownException: sys.exit(1) - finally: - rclpy.try_shutdown() - talker.destroy_node() if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/custom_executor.py b/rclpy/executors/examples_rclpy_executors/custom_executor.py index 5a3d8ad0..c6048abd 100644 --- a/rclpy/executors/examples_rclpy_executors/custom_executor.py +++ b/rclpy/executors/examples_rclpy_executors/custom_executor.py @@ -14,11 +14,13 @@ from concurrent.futures import ThreadPoolExecutor import os +import sys from examples_rclpy_executors.listener import Listener from examples_rclpy_executors.talker import Talker import rclpy from rclpy.executors import Executor +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from std_msgs.msg import String @@ -88,15 +90,11 @@ def main(args=None): executor.add_high_priority_node(estopper) executor.add_node(listener) executor.add_node(talker) - try: - executor.spin() - finally: - executor.shutdown() - estopper.destroy_node() - talker.destroy_node() - listener.destroy_node() - finally: - rclpy.shutdown() + executor.spin() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/listener.py b/rclpy/executors/examples_rclpy_executors/listener.py index 9cb5dc2a..4db789e7 100644 --- a/rclpy/executors/examples_rclpy_executors/listener.py +++ b/rclpy/executors/examples_rclpy_executors/listener.py @@ -56,9 +56,6 @@ def main(args=None): pass except ExternalShutdownException: sys.exit(1) - finally: - rclpy.try_shutdown() - listener.destroy_node() if __name__ == '__main__': diff --git a/rclpy/executors/examples_rclpy_executors/talker.py b/rclpy/executors/examples_rclpy_executors/talker.py index 73ce23c5..c2951f34 100644 --- a/rclpy/executors/examples_rclpy_executors/talker.py +++ b/rclpy/executors/examples_rclpy_executors/talker.py @@ -66,9 +66,6 @@ def main(args=None): pass except ExternalShutdownException: sys.exit(1) - finally: - rclpy.try_shutdown() - talker.destroy_node() if __name__ == '__main__': diff --git a/rclpy/guard_conditions/examples_rclpy_guard_conditions/trigger_guard_condition.py b/rclpy/guard_conditions/examples_rclpy_guard_conditions/trigger_guard_condition.py index 9d7e7c91..1f839221 100644 --- a/rclpy/guard_conditions/examples_rclpy_guard_conditions/trigger_guard_condition.py +++ b/rclpy/guard_conditions/examples_rclpy_guard_conditions/trigger_guard_condition.py @@ -12,36 +12,43 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + import rclpy +from rclpy.executors import ExternalShutdownException def main(args=None): - rclpy.init(args=args) - node = rclpy.create_node('demo_guard_condition') - executor = rclpy.executors.SingleThreadedExecutor() - executor.add_node(node) - - def guard_condition_callback(): - rclpy.shutdown() - node.get_logger().info('guard callback called shutdown') - - def timer_callback(): - guard_condition.trigger() - node.get_logger().info('timer callback triggered guard condition') - - node.create_timer(timer_period_sec=2, callback=timer_callback) - guard_condition = node.create_guard_condition(guard_condition_callback) - - while rclpy.ok(): - # First loop: `spin_once` waits for timer to be ready, then calls - # the timer's callback, which triggers the guard condition. - # Second loop: The guard condition is ready so it's callback is - # called. The callback calls shutdown, so the loop doesn't run - # again and the program exits. - node.get_logger().info("waiting for 'spin_once' to finish...") - executor.spin_once() - node.get_logger().info("...'spin_once' finished!\n") + try: + node = rclpy.create_node('demo_guard_condition') + executor = rclpy.executors.SingleThreadedExecutor() + executor.add_node(node) + + def guard_condition_callback(): + rclpy.shutdown() + node.get_logger().info('guard callback called shutdown') + + def timer_callback(): + guard_condition.trigger() + node.get_logger().info('timer callback triggered guard condition') + + node.create_timer(timer_period_sec=2, callback=timer_callback) + guard_condition = node.create_guard_condition(guard_condition_callback) + + while rclpy.ok(): + # First loop: `spin_once` waits for timer to be ready, then calls + # the timer's callback, which triggers the guard condition. + # Second loop: The guard condition is ready so it's callback is + # called. The callback calls shutdown, so the loop doesn't run + # again and the program exits. + node.get_logger().info("waiting for 'spin_once' to finish...") + executor.spin_once() + node.get_logger().info("...'spin_once' finished!\n") + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client.py b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client.py index 16ed5908..db81dd22 100644 --- a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client.py +++ b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client.py @@ -12,32 +12,38 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + 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('minimal_client') - cli = node.create_client(AddTwoInts, 'add_two_ints') - - req = AddTwoInts.Request() - req.a = 41 - req.b = 1 - while not cli.wait_for_service(timeout_sec=1.0): - node.get_logger().info('service not available, waiting again...') - - future = cli.call_async(req) - rclpy.spin_until_future_complete(node, future) - - result = future.result() - node.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (req.a, req.b, result.sum)) - node.destroy_node() - rclpy.shutdown() + try: + node = rclpy.create_node('minimal_client') + cli = node.create_client(AddTwoInts, 'add_two_ints') + + req = AddTwoInts.Request() + req.a = 41 + req.b = 1 + while not cli.wait_for_service(timeout_sec=1.0): + node.get_logger().info('service not available, waiting again...') + + future = cli.call_async(req) + rclpy.spin_until_future_complete(node, future) + + result = future.result() + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, result.sum)) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async.py b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async.py index e154d4fa..6d19a58d 100644 --- a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async.py +++ b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async.py @@ -12,36 +12,41 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + 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('minimal_client_async') - - cli = node.create_client(AddTwoInts, 'add_two_ints') - - req = AddTwoInts.Request() - req.a = 41 - req.b = 1 - while not cli.wait_for_service(timeout_sec=1.0): - node.get_logger().info('service not available, waiting again...') - - future = cli.call_async(req) - while rclpy.ok(): - rclpy.spin_once(node) - if future.done(): - result = future.result() - node.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (req.a, req.b, result.sum)) - break - - node.destroy_node() - rclpy.shutdown() + try: + node = rclpy.create_node('minimal_client_async') + + cli = node.create_client(AddTwoInts, 'add_two_ints') + + req = AddTwoInts.Request() + req.a = 41 + req.b = 1 + while not cli.wait_for_service(timeout_sec=1.0): + node.get_logger().info('service not available, waiting again...') + + future = cli.call_async(req) + while rclpy.ok(): + rclpy.spin_once(node) + if future.done(): + result = future.result() + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, result.sum)) + break + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_callback.py b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_callback.py index 7e8cd887..b7d56fd9 100644 --- a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_callback.py +++ b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_callback.py @@ -12,55 +12,61 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from example_interfaces.srv import AddTwoInts import rclpy from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import ExternalShutdownException def main(args=None): rclpy.init(args=args) - node = rclpy.create_node('minimal_client') - # Node's default callback group is mutually exclusive. This would prevent the client response - # from being processed until the timer callback finished, but the timer callback in this - # example is waiting for the client response - cb_group = ReentrantCallbackGroup() - cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group) - did_run = False - did_get_result = False - async def call_service(): - nonlocal cli, node, did_run, did_get_result - did_run = True - try: - req = AddTwoInts.Request() - req.a = 41 - req.b = 1 - future = cli.call_async(req) - result = await future - node.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (req.a, req.b, result.sum)) - finally: - did_get_result = True + try: + node = rclpy.create_node('minimal_client') + # Node's default callback group is mutually exclusive. This would prevent the client + # response from being processed until the timer callback finished, but the timer callback + # int this example is waiting for the client response + cb_group = ReentrantCallbackGroup() + cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group) + did_run = False + did_get_result = False - while not cli.wait_for_service(timeout_sec=1.0): - node.get_logger().info('service not available, waiting again...') + async def call_service(): + nonlocal cli, node, did_run, did_get_result + did_run = True + try: + req = AddTwoInts.Request() + req.a = 41 + req.b = 1 + future = cli.call_async(req) + result = await future + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, result.sum)) + finally: + did_get_result = True - timer = node.create_timer(0.5, call_service, callback_group=cb_group) + while not cli.wait_for_service(timeout_sec=1.0): + node.get_logger().info('service not available, waiting again...') - while rclpy.ok() and not did_run: - rclpy.spin_once(node) + timer = node.create_timer(0.5, call_service, callback_group=cb_group) - if did_run: - # call timer callback only once - timer.cancel() + while rclpy.ok() and not did_run: + rclpy.spin_once(node) - while rclpy.ok() and not did_get_result: - rclpy.spin_once(node) + if did_run: + # call timer callback only once + timer.cancel() - node.destroy_node() - rclpy.shutdown() + while rclpy.ok() and not did_get_result: + rclpy.spin_once(node) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_member_function.py b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_member_function.py index ae7fc9d8..a23ef8bf 100644 --- a/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_member_function.py +++ b/rclpy/services/minimal_client/examples_rclpy_minimal_client/client_async_member_function.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from example_interfaces.srv import AddTwoInts import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -36,20 +39,22 @@ def send_request(self): def main(args=None): rclpy.init(args=args) - minimal_client = MinimalClientAsync() - minimal_client.send_request() - - while rclpy.ok(): - rclpy.spin_once(minimal_client) - if minimal_client.future.done(): - response = minimal_client.future.result() - minimal_client.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (minimal_client.req.a, minimal_client.req.b, response.sum)) - break - - minimal_client.destroy_node() - rclpy.shutdown() + try: + minimal_client = MinimalClientAsync() + minimal_client.send_request() + + while rclpy.ok(): + rclpy.spin_once(minimal_client) + if minimal_client.future.done(): + response = minimal_client.future.result() + minimal_client.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (minimal_client.req.a, minimal_client.req.b, response.sum)) + break + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_service/examples_rclpy_minimal_service/service.py b/rclpy/services/minimal_service/examples_rclpy_minimal_service/service.py index f80df116..5ade5c50 100644 --- a/rclpy/services/minimal_service/examples_rclpy_minimal_service/service.py +++ b/rclpy/services/minimal_service/examples_rclpy_minimal_service/service.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from example_interfaces.srv import AddTwoInts import rclpy +from rclpy.executors import ExternalShutdownException g_node = None @@ -32,17 +35,18 @@ def main(args=None): global g_node rclpy.init(args=args) - g_node = rclpy.create_node('minimal_service') + try: + g_node = rclpy.create_node('minimal_service') - srv = g_node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback) - while rclpy.ok(): - rclpy.spin_once(g_node) + srv = g_node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback) + srv # Quiet flake8 warnings about unused variable + while rclpy.ok(): + rclpy.spin_once(g_node) - # Destroy the service attached to the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - g_node.destroy_service(srv) - rclpy.shutdown() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/services/minimal_service/examples_rclpy_minimal_service/service_member_function.py b/rclpy/services/minimal_service/examples_rclpy_minimal_service/service_member_function.py index 3f1dc80f..6eedd736 100644 --- a/rclpy/services/minimal_service/examples_rclpy_minimal_service/service_member_function.py +++ b/rclpy/services/minimal_service/examples_rclpy_minimal_service/service_member_function.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + from example_interfaces.srv import AddTwoInts import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node @@ -34,11 +37,14 @@ def add_two_ints_callback(self, request, response): def main(args=None): rclpy.init(args=args) - minimal_service = MinimalService() - - rclpy.spin(minimal_service) + try: + minimal_service = MinimalService() - rclpy.shutdown() + rclpy.spin(minimal_service) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_local_function.py b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_local_function.py index 002ce3b6..88f3c942 100644 --- a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_local_function.py +++ b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_local_function.py @@ -12,7 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + import rclpy +from rclpy.executors import ExternalShutdownException from std_msgs.msg import String @@ -20,30 +23,29 @@ def main(args=None): rclpy.init(args=args) - node = rclpy.create_node('minimal_publisher') - publisher = node.create_publisher(String, 'topic', 10) - - msg = String() - i = 0 - - def timer_callback(): - nonlocal i - msg.data = 'Hello World: %d' % i - i += 1 - node.get_logger().info('Publishing: "%s"' % msg.data) - publisher.publish(msg) - - timer_period = 0.5 # seconds - timer = node.create_timer(timer_period, timer_callback) - - rclpy.spin(node) - - # Destroy the timer attached to the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - node.destroy_timer(timer) - node.destroy_node() - rclpy.shutdown() + try: + node = rclpy.create_node('minimal_publisher') + publisher = node.create_publisher(String, 'topic', 10) + + msg = String() + i = 0 + + def timer_callback(): + nonlocal i + msg.data = 'Hello World: %d' % i + i += 1 + node.get_logger().info('Publishing: "%s"' % msg.data) + publisher.publish(msg) + + timer_period = 0.5 # seconds + timer = node.create_timer(timer_period, timer_callback) + timer # Quiet flake8 warnings about unused variable + + rclpy.spin(node) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py index dfdb0320..45ae3f5b 100644 --- a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py +++ b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py @@ -12,7 +12,10 @@ # 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 @@ -38,15 +41,14 @@ def timer_callback(self): def main(args=None): rclpy.init(args=args) - minimal_publisher = MinimalPublisher() - - rclpy.spin(minimal_publisher) + try: + minimal_publisher = MinimalPublisher() - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() + rclpy.spin(minimal_publisher) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_old_school.py b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_old_school.py index 23ab3ccf..8873fad9 100644 --- a/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_old_school.py +++ b/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_old_school.py @@ -12,9 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys from time import sleep import rclpy +from rclpy.executors import ExternalShutdownException from std_msgs.msg import String @@ -27,25 +29,24 @@ def main(args=None): rclpy.init(args=args) - node = rclpy.create_node('minimal_publisher') + try: + node = rclpy.create_node('minimal_publisher') - publisher = node.create_publisher(String, 'topic', 10) + publisher = node.create_publisher(String, 'topic', 10) - msg = String() + msg = String() - i = 0 - while rclpy.ok(): - msg.data = 'Hello World: %d' % i - i += 1 - node.get_logger().info('Publishing: "%s"' % msg.data) - publisher.publish(msg) - sleep(0.5) # seconds - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - node.destroy_node() - rclpy.shutdown() + i = 0 + while rclpy.ok(): + msg.data = 'Hello World: %d' % i + i += 1 + node.get_logger().info('Publishing: "%s"' % msg.data) + publisher.publish(msg) + sleep(0.5) # seconds + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_lambda.py b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_lambda.py index 6d8e84da..83331f1c 100644 --- a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_lambda.py +++ b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_lambda.py @@ -12,7 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + import rclpy +from rclpy.executors import ExternalShutdownException from std_msgs.msg import String @@ -20,19 +23,18 @@ def main(args=None): rclpy.init(args=args) - node = rclpy.create_node('minimal_subscriber') - - subscription = node.create_subscription( - String, 'topic', lambda msg: node.get_logger().info('I heard: "%s"' % msg.data), 10) - subscription # prevent unused variable warning + try: + node = rclpy.create_node('minimal_subscriber') - rclpy.spin(node) + subscription = node.create_subscription( + String, 'topic', lambda msg: node.get_logger().info('I heard: "%s"' % msg.data), 10) + subscription # prevent unused variable warning - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - node.destroy_node() - rclpy.shutdown() + rclpy.spin(node) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py index 78c72668..4346f4ce 100644 --- a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py +++ b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py @@ -12,7 +12,10 @@ # 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 @@ -36,15 +39,14 @@ def listener_callback(self, msg): def main(args=None): rclpy.init(args=args) - minimal_subscriber = MinimalSubscriber() - - rclpy.spin(minimal_subscriber) + try: + minimal_subscriber = MinimalSubscriber() - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_subscriber.destroy_node() - rclpy.shutdown() + rclpy.spin(minimal_subscriber) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_old_school.py b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_old_school.py index a15e789f..06bda5fd 100644 --- a/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_old_school.py +++ b/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_old_school.py @@ -12,7 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + import rclpy +from rclpy.executors import ExternalShutdownException from std_msgs.msg import String @@ -29,19 +32,18 @@ def main(args=None): global g_node rclpy.init(args=args) - g_node = rclpy.create_node('minimal_subscriber') - - subscription = g_node.create_subscription(String, 'topic', chatter_callback, 10) - subscription # prevent unused variable warning + try: + g_node = rclpy.create_node('minimal_subscriber') - while rclpy.ok(): - rclpy.spin_once(g_node) + subscription = g_node.create_subscription(String, 'topic', chatter_callback, 10) + subscription # prevent unused variable warning - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - g_node.destroy_node() - rclpy.shutdown() + while rclpy.ok(): + rclpy.spin_once(g_node) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__': diff --git a/rclpy/topics/pointcloud_publisher/examples_rclpy_pointcloud_publisher/pointcloud_publisher.py b/rclpy/topics/pointcloud_publisher/examples_rclpy_pointcloud_publisher/pointcloud_publisher.py index 6152f673..8638a51e 100644 --- a/rclpy/topics/pointcloud_publisher/examples_rclpy_pointcloud_publisher/pointcloud_publisher.py +++ b/rclpy/topics/pointcloud_publisher/examples_rclpy_pointcloud_publisher/pointcloud_publisher.py @@ -12,9 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys + import numpy as np import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointField @@ -60,10 +63,13 @@ def timer_callback(self): def main(args=None): rclpy.init(args=args) - pc_publisher = PointCloudPublisher() - rclpy.spin(pc_publisher) - pc_publisher.destroy_node() - rclpy.shutdown() + try: + pc_publisher = PointCloudPublisher() + rclpy.spin(pc_publisher) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) if __name__ == '__main__':