diff --git a/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_asyncio.py b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_asyncio.py new file mode 100644 index 00000000..9584bd79 --- /dev/null +++ b/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_asyncio.py @@ -0,0 +1,123 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import asyncio + +import rclpy +from example_interfaces.action import Fibonacci +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + + +def asyncio_loop(func): + def wrapper(*args, **kwargs): + return asyncio.new_event_loop().run_until_complete( + func(*args, **kwargs)) + + return wrapper + + +class MinimalActionServerAsyncIO(Node): + + def __init__(self): + super().__init__('minimal_action_server_asyncio') + + self._action_server = ActionServer( + self, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_callback, + callback_group=ReentrantCallbackGroup(), + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback) + + def destroy(self): + self._action_server.destroy() + super().destroy_node() + + def goal_callback(self, goal_request): + """Accept or reject a client request to begin an action.""" + # This server allows multiple goals in parallel + self.get_logger().info('Received goal request') + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle): + """Accept or reject a client request to cancel an action.""" + self.get_logger().info('Received cancel request') + return CancelResponse.ACCEPT + + @asyncio_loop + async def execute_callback(self, goal_handle): + """Execute a goal.""" + self.get_logger().info('Executing goal...') + + # Append the seeds for the Fibonacci sequence + feedback_msg = Fibonacci.Feedback() + feedback_msg.sequence = [0, 1] + + # Start executing the action + for i in range(1, goal_handle.request.order): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info('Goal canceled') + return Fibonacci.Result() + + # Update Fibonacci sequence + feedback_msg.sequence.append( + feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) + + self.get_logger().info( + 'Publishing feedback: {0}'.format(feedback_msg.sequence)) + + # Publish the feedback + goal_handle.publish_feedback(feedback_msg) + + # Sleep for demonstration purposes + await asyncio.sleep(1) + + goal_handle.succeed() + + # Populate result message + result = Fibonacci.Result() + result.sequence = feedback_msg.sequence + + self.get_logger().info('Returning result: {0}'.format(result.sequence)) + + return result + + +def main(args=None): + # init ros2 + rclpy.init(args=args) + + # Use a MultiThreadedExecutor to enable processing goals concurrently + executor = MultiThreadedExecutor() + + # create node + action_server = MinimalActionServerAsyncIO() + + # add node + executor.add_node(action_server) + + # spin + executor.spin() + + action_server.destroy() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rclpy/actions/minimal_action_server/setup.py b/rclpy/actions/minimal_action_server/setup.py index f45200c9..c097f906 100644 --- a/rclpy/actions/minimal_action_server/setup.py +++ b/rclpy/actions/minimal_action_server/setup.py @@ -34,6 +34,7 @@ 'server_not_composable = ' + package_name + '.server_not_composable:main', 'server_queue_goals = ' + package_name + '.server_queue_goals:main', 'server_single_goal = ' + package_name + '.server_single_goal:main', + 'server_asyncio = ' + package_name + '.server_asyncio:main', ], }, )