Skip to content

Commit 291c789

Browse files
authored
Switch to using the rclpy context manager everywhere. (#389)
* Switch to using the rclpy context manager everywhere. This lets us use less code, but still effectively manage the lifetime of the rclpy.init. We also change ExternalShutdownException handling to work just like a KeyboardInterrupt. Signed-off-by: Chris Lalancette <[email protected]>
1 parent f13e6d7 commit 291c789

File tree

29 files changed

+355
-477
lines changed

29 files changed

+355
-477
lines changed

rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
16-
1715
from action_msgs.msg import GoalStatus
1816
from example_interfaces.action import Fibonacci
1917

@@ -71,18 +69,15 @@ def send_goal(self):
7169

7270

7371
def main(args=None):
74-
rclpy.init(args=args)
75-
7672
try:
77-
action_client = MinimalActionClient()
73+
with rclpy.init(args=args):
74+
action_client = MinimalActionClient()
7875

79-
action_client.send_goal()
76+
action_client.send_goal()
8077

81-
rclpy.spin(action_client)
82-
except KeyboardInterrupt:
78+
rclpy.spin(action_client)
79+
except (KeyboardInterrupt, ExternalShutdownException):
8380
pass
84-
except ExternalShutdownException:
85-
sys.exit(1)
8681

8782

8883
if __name__ == '__main__':

rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_asyncio.py

Lines changed: 35 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
import rclpy
2121
from rclpy.action import ActionClient
22+
from rclpy.executors import ExternalShutdownException
2223
from rclpy.node import Node
2324

2425

@@ -69,48 +70,49 @@ async def spinning(node):
6970

7071
async def run(args, loop):
7172

72-
logger = rclpy.logging.get_logger('minimal_action_client')
73+
# init ROS 2
74+
with rclpy.init(args=args):
75+
logger = rclpy.logging.get_logger('minimal_action_client')
7376

74-
# init ros2
75-
rclpy.init(args=args)
77+
# create node
78+
action_client = MinimalActionClientAsyncIO()
7679

77-
# create node
78-
action_client = MinimalActionClientAsyncIO()
80+
# start spinning
81+
spin_task = loop.create_task(spinning(action_client))
7982

80-
# start spinning
81-
spin_task = loop.create_task(spinning(action_client))
83+
# Parallel example
84+
# execute goal request and schedule in loop
85+
my_task1 = loop.create_task(action_client.send_goal())
86+
my_task2 = loop.create_task(action_client.send_goal())
8287

83-
# Parallel example
84-
# execute goal request and schedule in loop
85-
my_task1 = loop.create_task(action_client.send_goal())
86-
my_task2 = loop.create_task(action_client.send_goal())
88+
# glue futures together and wait
89+
wait_future = asyncio.wait([my_task1, my_task2])
90+
# run event loop
91+
finished, unfinished = await wait_future
92+
logger.info(f'unfinished: {len(unfinished)}')
93+
for task in finished:
94+
logger.info('result {} and status flag {}'.format(*task.result()))
8795

88-
# glue futures together and wait
89-
wait_future = asyncio.wait([my_task1, my_task2])
90-
# run event loop
91-
finished, unfinished = await wait_future
92-
logger.info(f'unfinished: {len(unfinished)}')
93-
for task in finished:
94-
logger.info('result {} and status flag {}'.format(*task.result()))
96+
# Sequence
97+
result, status = await loop.create_task(action_client.send_goal())
98+
logger.info(f'A) result {result} and status flag {status}')
99+
result, status = await loop.create_task(action_client.send_goal())
100+
logger.info(f'B) result {result} and status flag {status}')
95101

96-
# Sequence
97-
result, status = await loop.create_task(action_client.send_goal())
98-
logger.info(f'A) result {result} and status flag {status}')
99-
result, status = await loop.create_task(action_client.send_goal())
100-
logger.info(f'B) result {result} and status flag {status}')
101-
102-
# cancel spinning task
103-
spin_task.cancel()
104-
try:
105-
await spin_task
106-
except asyncio.exceptions.CancelledError:
107-
pass
108-
rclpy.shutdown()
102+
# cancel spinning task
103+
spin_task.cancel()
104+
try:
105+
await spin_task
106+
except asyncio.exceptions.CancelledError:
107+
pass
109108

110109

111110
def main(args=None):
112-
loop = asyncio.get_event_loop()
113-
loop.run_until_complete(run(args, loop=loop))
111+
try:
112+
loop = asyncio.get_event_loop()
113+
loop.run_until_complete(run(args, loop=loop))
114+
except (KeyboardInterrupt, ExternalShutdownException):
115+
pass
114116

115117

116118
if __name__ == '__main__':

rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_cancel.py

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
16-
1715
from example_interfaces.action import Fibonacci
1816

1917
import rclpy
@@ -79,18 +77,15 @@ def send_goal(self):
7977

8078

8179
def main(args=None):
82-
rclpy.init(args=args)
83-
8480
try:
85-
action_client = MinimalActionClient()
81+
with rclpy.init(args=args):
82+
action_client = MinimalActionClient()
8683

87-
action_client.send_goal()
84+
action_client.send_goal()
8885

89-
rclpy.spin(action_client)
90-
except KeyboardInterrupt:
86+
rclpy.spin(action_client)
87+
except (KeyboardInterrupt, ExternalShutdownException):
9188
pass
92-
except ExternalShutdownException:
93-
sys.exit(1)
9489

9590

9691
if __name__ == '__main__':

rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_not_composable.py

Lines changed: 27 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
16-
1715
from action_msgs.msg import GoalStatus
1816

1917
from example_interfaces.action import Fibonacci
@@ -28,53 +26,48 @@ def feedback_cb(logger, feedback):
2826

2927

3028
def main(args=None):
31-
rclpy.init(args=args)
32-
3329
try:
34-
node = rclpy.create_node('minimal_action_client')
30+
with rclpy.init(args=args):
31+
node = rclpy.create_node('minimal_action_client')
3532

36-
action_client = ActionClient(node, Fibonacci, 'fibonacci')
33+
action_client = ActionClient(node, Fibonacci, 'fibonacci')
3734

38-
node.get_logger().info('Waiting for action server...')
35+
node.get_logger().info('Waiting for action server...')
3936

40-
action_client.wait_for_server()
37+
action_client.wait_for_server()
4138

42-
goal_msg = Fibonacci.Goal()
43-
goal_msg.order = 10
39+
goal_msg = Fibonacci.Goal()
40+
goal_msg.order = 10
4441

45-
node.get_logger().info('Sending goal request...')
42+
node.get_logger().info('Sending goal request...')
4643

47-
send_goal_future = action_client.send_goal_async(
48-
goal_msg, feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback))
44+
send_goal_future = action_client.send_goal_async(
45+
goal_msg,
46+
feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback))
4947

50-
rclpy.spin_until_future_complete(node, send_goal_future)
48+
rclpy.spin_until_future_complete(node, send_goal_future)
5149

52-
goal_handle = send_goal_future.result()
50+
goal_handle = send_goal_future.result()
5351

54-
if not goal_handle.accepted:
55-
node.get_logger().info('Goal rejected :(')
56-
action_client.destroy()
57-
node.destroy_node()
58-
rclpy.shutdown()
59-
return
52+
if not goal_handle.accepted:
53+
node.get_logger().info('Goal rejected :(')
54+
return
6055

61-
node.get_logger().info('Goal accepted :)')
56+
node.get_logger().info('Goal accepted :)')
6257

63-
get_result_future = goal_handle.get_result_async()
58+
get_result_future = goal_handle.get_result_async()
6459

65-
rclpy.spin_until_future_complete(node, get_result_future)
60+
rclpy.spin_until_future_complete(node, get_result_future)
6661

67-
result = get_result_future.result().result
68-
status = get_result_future.result().status
69-
if status == GoalStatus.STATUS_SUCCEEDED:
70-
node.get_logger().info(
71-
'Goal succeeded! Result: {0}'.format(result.sequence))
72-
else:
73-
node.get_logger().info('Goal failed with status code: {0}'.format(status))
74-
except KeyboardInterrupt:
62+
result = get_result_future.result().result
63+
status = get_result_future.result().status
64+
if status == GoalStatus.STATUS_SUCCEEDED:
65+
node.get_logger().info(
66+
'Goal succeeded! Result: {0}'.format(result.sequence))
67+
else:
68+
node.get_logger().info('Goal failed with status code: {0}'.format(status))
69+
except (KeyboardInterrupt, ExternalShutdownException):
7570
pass
76-
except ExternalShutdownException:
77-
sys.exit(1)
7871

7972

8073
if __name__ == '__main__':

rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server.py

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
1615
import time
1716

1817
from example_interfaces.action import Fibonacci
@@ -92,19 +91,16 @@ async def execute_callback(self, goal_handle):
9291

9392

9493
def main(args=None):
95-
rclpy.init(args=args)
96-
9794
try:
98-
minimal_action_server = MinimalActionServer()
95+
with rclpy.init(args=args):
96+
minimal_action_server = MinimalActionServer()
9997

100-
# Use a MultiThreadedExecutor to enable processing goals concurrently
101-
executor = MultiThreadedExecutor()
98+
# Use a MultiThreadedExecutor to enable processing goals concurrently
99+
executor = MultiThreadedExecutor()
102100

103-
rclpy.spin(minimal_action_server, executor=executor)
104-
except KeyboardInterrupt:
101+
rclpy.spin(minimal_action_server, executor=executor)
102+
except (KeyboardInterrupt, ExternalShutdownException):
105103
pass
106-
except ExternalShutdownException:
107-
sys.exit(1)
108104

109105

110106
if __name__ == '__main__':

rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_defer.py

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
1615
import time
1716

1817
from example_interfaces.action import Fibonacci
@@ -106,19 +105,16 @@ async def execute_callback(self, goal_handle):
106105

107106

108107
def main(args=None):
109-
rclpy.init(args=args)
110-
111108
try:
112-
minimal_action_server = MinimalActionServer()
109+
with rclpy.init(args=args):
110+
minimal_action_server = MinimalActionServer()
113111

114-
# Use a MultiThreadedExecutor to enable processing goals concurrently
115-
executor = MultiThreadedExecutor()
112+
# Use a MultiThreadedExecutor to enable processing goals concurrently
113+
executor = MultiThreadedExecutor()
116114

117-
rclpy.spin(minimal_action_server, executor=executor)
118-
except KeyboardInterrupt:
115+
rclpy.spin(minimal_action_server, executor=executor)
116+
except (KeyboardInterrupt, ExternalShutdownException):
119117
pass
120-
except ExternalShutdownException:
121-
sys.exit(1)
122118

123119

124120
if __name__ == '__main__':

rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_not_composable.py

Lines changed: 21 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414

15-
import sys
1615
import time
1716

1817
from example_interfaces.action import Fibonacci
@@ -71,32 +70,30 @@ async def execute_callback(goal_handle):
7170

7271
def main(args=None):
7372
global logger
74-
rclpy.init(args=args)
7573

7674
try:
77-
node = rclpy.create_node('minimal_action_server')
78-
logger = node.get_logger()
79-
80-
# Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
81-
# Default goal callback accepts all goals
82-
# Default cancel callback rejects cancel requests
83-
action_server = ActionServer(
84-
node,
85-
Fibonacci,
86-
'fibonacci',
87-
execute_callback=execute_callback,
88-
cancel_callback=cancel_callback,
89-
callback_group=ReentrantCallbackGroup())
90-
action_server # Quiet flake8 warnings about unused variable
91-
92-
# Use a MultiThreadedExecutor to enable processing goals concurrently
93-
executor = MultiThreadedExecutor()
94-
95-
rclpy.spin(node, executor=executor)
96-
except KeyboardInterrupt:
75+
with rclpy.init(args=args):
76+
node = rclpy.create_node('minimal_action_server')
77+
logger = node.get_logger()
78+
79+
# Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
80+
# Default goal callback accepts all goals
81+
# Default cancel callback rejects cancel requests
82+
action_server = ActionServer(
83+
node,
84+
Fibonacci,
85+
'fibonacci',
86+
execute_callback=execute_callback,
87+
cancel_callback=cancel_callback,
88+
callback_group=ReentrantCallbackGroup())
89+
action_server # Quiet flake8 warnings about unused variable
90+
91+
# Use a MultiThreadedExecutor to enable processing goals concurrently
92+
executor = MultiThreadedExecutor()
93+
94+
rclpy.spin(node, executor=executor)
95+
except (KeyboardInterrupt, ExternalShutdownException):
9796
pass
98-
except ExternalShutdownException:
99-
sys.exit(1)
10097

10198

10299
if __name__ == '__main__':

0 commit comments

Comments
 (0)