-
Notifications
You must be signed in to change notification settings - Fork 318
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
2 changed files
with
124 additions
and
0 deletions.
There are no files selected for viewing
123 changes: 123 additions & 0 deletions
123
rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_asyncio.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters