Skip to content

Commit

Permalink
Add asyncio action server example
Browse files Browse the repository at this point in the history
  • Loading branch information
tupiznak committed Jul 16, 2021
1 parent b83b185 commit 687560d
Show file tree
Hide file tree
Showing 2 changed files with 124 additions and 0 deletions.
Original file line number Diff line number Diff line change
@@ -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()
1 change: 1 addition & 0 deletions rclpy/actions/minimal_action_server/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -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',
],
},
)

0 comments on commit 687560d

Please sign in to comment.