├── .github └── workflows │ └── main.yaml ├── Jenkinsfile ├── LICENSE ├── README.md ├── aiorospy ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── requirements.in ├── requirements.txt ├── setup.py ├── src │ └── aiorospy │ │ ├── __init__.py │ │ ├── action.py │ │ ├── helpers.py │ │ ├── service.py │ │ └── topic.py └── tests │ ├── test_action_client.py │ ├── test_action_server.py │ ├── test_aiorospy.test │ ├── test_service.py │ ├── test_service_proxy.py │ └── test_subscriber.py ├── aiorospy_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── requirements.in ├── requirements.txt └── scripts │ ├── actions │ ├── aiorospy_telephone │ ├── services │ ├── simple_actions │ └── topics └── setup.cfg /.github/workflows/main.yaml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - '*' 7 | 8 | jobs: 9 | ci: 10 | strategy: 11 | matrix: 12 | include: 13 | - os: ubuntu-20.04 14 | distro: noetic 15 | fail-fast: false 16 | runs-on: ${{ matrix.os }} 17 | steps: 18 | - uses: actions/checkout@v1 19 | - uses: ros-tooling/setup-ros@v0.7 20 | with: 21 | required-ros-distributions: ${{ matrix.distro }} 22 | - run: sudo apt remove python3-openssl -y 23 | - uses: ros-tooling/action-ros-ci@v0.4 24 | with: 25 | target-ros1-distro: ${{ matrix.distro }} 26 | -------------------------------------------------------------------------------- /Jenkinsfile: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env groovy 2 | @Library('tailor-meta@0.1.27')_ 3 | tailorTestPipeline( 4 | // Name of job that generated this test definition. 5 | rosdistro_job: '/ci/rosdistro/master', 6 | // Distribution name 7 | rosdistro_name: 'ros1', 8 | // Release track to test branch against. 9 | release_track: 'hotdog', 10 | // Release label to pull test images from. 11 | release_label: 'hotdog', 12 | // OS distributions to test. 13 | distributions: ['jammy'], 14 | // Version of tailor_meta to build against 15 | tailor_meta: '0.1.27', 16 | // Master or release branch associated with this track 17 | source_branch: 'master', 18 | // Docker registry where test image is stored 19 | docker_registry: 'https://084758475884.dkr.ecr.us-east-1.amazonaws.com/locus-tailor' 20 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Locus Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # aiorospy 2 | 3 | An asyncio wrapper around rospy I/O interfaces for Python >=3.6 to enable use of the co-operative multitasking concurrency model. 4 | 5 | 6 | ## Rationale 7 | 8 | Rospy implements its I/O interfaces using callbacks and therefore handles concurrency with a preemptive multitasking model. This means that complex nodes need to worry about threading concerns such as locking (and therefore deadlocks), as well as how to structure callback-based control flow in a sane manner. If you've attempted to write a sufficiently complex rospy node that handles I/O from different sources, you probably understand how painful this can get. 9 | 10 | Asyncio was added to the Python 3.5 standard library on a provisional bases, formalized in Python 3.6. It implements the capabilities to handle concurrency with a co-operative multitasking model. This means that a complex node can more easily manage its core loop through the use of `awaitables` in a single thread. This tends to make your code straightforward to write, reason about, and debug. If you're not convinced, check out the example that handles about a dozen I/O interfaces in a single event loop. 11 | 12 | `Aiorospy` and `asyncio` might be for you if: 13 | - your rospy node wants to handle I/O from numerous sources in a complex manner 14 | - you're trying to implement a WebSocket or HTTP server in a single process alongside topics, services, and actions (if a multi-process WSGI/ASGI with IPC feels like overkill) 15 | - you've grown to hate maintaining complex rospy nodes because of callback hell, locking noise, and deadlocking 16 | - you're tired of having to debug complex rospy nodes where the core control flow jumps among countless threads 17 | - you want precdictability over what will happen next and in what order 18 | 19 | ## Requirements 20 | 21 | ### Python >= 3.6 22 | Asyncio does not exist in Python 2 and is only provisional in Python 3.5. If you're using Xenial, you can install it using the [deadsnakes](https://launchpad.net/~deadsnakes/+archive/ubuntu/ppa) ppa. 23 | 24 | ### [catkin_virtualenv](https://github.com/locusrobotics/catkin_virtualenv) 25 | Simplifies dependency management and makes using a different version of python pretty easy. Check `requirements.txt` to see what additional dependencies are used when this package is built. 26 | 27 | ## Examples 28 | 29 | Check out the [`asyncio_examples/scripts`](https://github.com/locusrobotics/aiorospy/tree/master/aiorospy_examples/scripts) folder for examples of topics, services, actions, and a composite node. 30 | 31 | Take note that when using` rospy` and `asyncio` together, the following boilerplate is recommended: 32 | 33 | ``` 34 | import aiorospy 35 | import asyncio 36 | import rospy 37 | 38 | rospy.init_node('node_name') 39 | 40 | loop = asyncio.get_event_loop() 41 | 42 | tasks = asyncio.gather( 43 | my_top_level_task(), 44 | # ... 45 | ) 46 | 47 | # Any uncaught exceptions will cause this task to get cancelled 48 | aiorospy.cancel_on_exception(tasks) 49 | # ROS shutdown will cause this task toget cancelled 50 | aiorospy.cancel_on_shutdown(tasks) 51 | 52 | try: 53 | loop.run_until_complete(tasks) 54 | except asyncio.CancelledError: 55 | pass 56 | ``` 57 | 58 | ## How it works 59 | There's no desire to reimplement rospy this late in its life, so this package wraps the various rospy interfaces instead. This means that there are still underlying threads that handle TCP I/O for topics and services. But unlike `rospy`, the API is not callbacks that run in separate threads, but rather `awaitables` that run in the main thread's event loop. This is accomplished by using thread-safe intermediaries such as `asyncio.call_soon_threadsafe` and `janus.Queue`. 60 | -------------------------------------------------------------------------------- /aiorospy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package aiorospy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.0 (2022-02-23) 6 | ------------------ 7 | * Prepare for noetic release with python3 (#39) 8 | * replace python_interpreter 3.6 with 3 9 | * Check python version with setuptools 10 | * Fix lint 11 | Co-authored-by: betaboon 12 | Co-authored-by: Paul Bovbel 13 | * Wait for a service longer (#37) 14 | * Don't wait for service in ensure 15 | * Don't pre-empt wait_for_service as often 16 | * ServiceProxy is not threadsafe (#36) 17 | * Limit the size of the action feedback queue by default (#35) 18 | * Contributors: Doug Smith, Paul Bovbel, abencz 19 | 20 | 0.4.0 (2023-02-22) 21 | ------------------ 22 | * 0.3.0 23 | * Update changelogs 24 | * Add exponential delay when retrying service after exception (#40) 25 | * Prepare for noetic release with python3 (#39) 26 | * replace python_interpreter 3.6 with 3 27 | * Check python version with setuptools 28 | * Fix lint 29 | Co-authored-by: betaboon 30 | Co-authored-by: Paul Bovbel 31 | * Wait for a service longer (#37) 32 | * Don't wait for service in ensure 33 | * Don't pre-empt wait_for_service as often 34 | * ServiceProxy is not threadsafe (#36) 35 | * Limit the size of the action feedback queue by default (#35) 36 | * Contributors: Alex Bencz, Doug Smith, Gary Servin, Paul Bovbel, abencz 37 | 38 | 0.5.0 (2023-09-25) 39 | ------------------ 40 | * 0.4.0 41 | * Update changelogs 42 | * 0.3.0 43 | * Update changelogs 44 | * Add exponential delay when retrying service after exception (#40) 45 | * Prepare for noetic release with python3 (#39) 46 | * replace python_interpreter 3.6 with 3 47 | * Check python version with setuptools 48 | * Fix lint 49 | Co-authored-by: betaboon 50 | Co-authored-by: Paul Bovbel 51 | * Wait for a service longer (#37) 52 | * Don't wait for service in ensure 53 | * Don't pre-empt wait_for_service as often 54 | * ServiceProxy is not threadsafe (#36) 55 | * Limit the size of the action feedback queue by default (#35) 56 | * Contributors: Alex Bencz, Doug Smith, Gary Servin, Paul Bovbel, abencz 57 | 58 | 0.10.0 (2025-06-06) 59 | ------------------- 60 | * Update packages dependencies to build on Noble (#48) 61 | * Update dependencies 62 | * Update cmake minimum version 63 | * Update dependecies 64 | * Update github actions 65 | * Match versions to the lowest common version 66 | * Contributors: Gary Servin 67 | 68 | 0.9.0 (2025-02-04) 69 | ------------------ 70 | 71 | 0.8.0 (2024-09-16) 72 | ------------------ 73 | * Don't use async_generator for python3.9+ (#47) 74 | * Fix async generator dependency (#46) 75 | * Fix naming of async generator dependency 76 | * Use contextlib if available 77 | * Contributors: Gary Servin, Michael Johnson 78 | 79 | 0.7.0 (2024-06-17) 80 | ------------------ 81 | * RST-9628 re-create service proxy after transport terminated (#45) 82 | * Contributors: Alex Bencz 83 | 84 | 0.6.0 (2024-02-02) 85 | ------------------ 86 | * 0.5.0 87 | * Update changelogs 88 | * 0.4.0 89 | * Update changelogs 90 | * 0.3.0 91 | * Update changelogs 92 | * Add exponential delay when retrying service after exception (#40) 93 | * Prepare for noetic release with python3 (#39) 94 | * replace python_interpreter 3.6 with 3 95 | * Check python version with setuptools 96 | * Fix lint 97 | Co-authored-by: betaboon 98 | Co-authored-by: Paul Bovbel 99 | * Wait for a service longer (#37) 100 | * Don't wait for service in ensure 101 | * Don't pre-empt wait_for_service as often 102 | * ServiceProxy is not threadsafe (#36) 103 | * Limit the size of the action feedback queue by default (#35) 104 | * Contributors: Alex Bencz, Doug Smith, Gary Servin, Paul Bovbel, abencz 105 | 106 | 0.2.0 (2020-10-02) 107 | ------------------ 108 | * Missing return (`#33 `_) 109 | * Don't instantiate queue outside of coroutine (`#32 `_) 110 | * Don't take or passthrough loop arguments unnecessarily (`#31 `_) 111 | * Lock requirements 112 | * python3.6 support (`#29 `_) 113 | * Make subprocess a context manager to ensure shutdown; expose timing vars from timer (`#28 `_) 114 | * Add Timer and subprocess helpers (`#27 `_) 115 | * Add Timer and subprocess helpers 116 | * Fix comments 117 | * Implement simple action server (`#26 `_) 118 | Implement proper simple action server, and fix a bunch of flaky tests. 119 | * run_in_executor doesn't take kwargs (`#25 `_) 120 | * Update to aiostream 0.3.3 (`#21 `_) 121 | fixes a bug during installation in some environments, see https://github.com/vxgmichel/aiostream/pull/42 122 | * Fix exception monitor handling of concurrent.futures.CancelledError (`#20 `_) 123 | * Contributors: Andreas Bresser, Paul Bovbel, betaboon 124 | 125 | 0.1.0 (2019-07-12) 126 | ------------------ 127 | * Cleanup (`#18 `_) 128 | * Also stop action client to prevent memory leaks 129 | * Fix virtualenv for examples; drop requirement on std_srv fork 130 | * Usability fixes (`#17 `_) 131 | log_during async helper to log periodically while waiting for an awaitable 132 | Periodic logging to async-blocking methods in services and actions 133 | Automatically clean up actions that are improperly terminated 134 | * Implement simple_actions demo; fix bug in ExecutionMonitor (`#16 `_) 135 | * Update internal components and examples (`#14 `_) 136 | * Re-implement actions, services 137 | * Add tests 138 | * Update examples 139 | * Allow ensure_goal to be cancelled properly (`#13 `_) 140 | * Fix missing await (`#12 `_) 141 | * get event loop not running loop (`#11 `_) 142 | * Sprinkle some extra docs 143 | * Async Actions (`#7 `_) 144 | Actions and subscriber rewrite 145 | * return state and result (`#6 `_) 146 | * return state and result 147 | * Split off aiorospy_examples (`#5 `_) 148 | * Split off an aiorospy_examples package to avoid pinning python version 149 | * Restore LICENSE and README 150 | * Move dependencies; use venv's default python 151 | * Contributors: Andrew Blakey, Kaitlin Gallagher, Paul Bovbel 152 | -------------------------------------------------------------------------------- /aiorospy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | project(aiorospy) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | ) 7 | 8 | catkin_package() 9 | 10 | catkin_python_setup() 11 | 12 | install(FILES requirements.txt 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 14 | 15 | if(CATKIN_ENABLE_TESTING) 16 | find_package(catkin REQUIRED COMPONENTS catkin_virtualenv roslint rostest) 17 | 18 | catkin_generate_virtualenv( 19 | INPUT_REQUIREMENTS requirements.in 20 | PYTHON_INTERPRETER python3 21 | ) 22 | set(python_test_scripts 23 | tests/test_action_client.py 24 | tests/test_action_server.py 25 | tests/test_service_proxy.py 26 | tests/test_service.py 27 | tests/test_subscriber.py 28 | ) 29 | 30 | roslint_python() 31 | roslint_python(${python_test_scripts}) 32 | roslint_add_test() 33 | 34 | catkin_install_python(PROGRAMS ${python_test_scripts} 35 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 36 | 37 | add_rostest(tests/test_aiorospy.test 38 | DEPENDENCIES ${PROJECT_NAME}_generate_virtualenv 39 | ) 40 | endif() 41 | -------------------------------------------------------------------------------- /aiorospy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aiorospy 4 | 0.10.0 5 | Asyncio wrapper around rospy I/O interfaces. 6 | 7 | Andrew Blakey 8 | Paul Bovbel 9 | 10 | Andrew Blakey 11 | Paul Bovbel 12 | 13 | MIT 14 | 15 | catkin 16 | python3-setuptools 17 | 18 | actionlib 19 | rospy 20 | 21 | catkin_virtualenv 22 | python3 23 | roslint 24 | rostest 25 | 26 | 27 | requirements.txt 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /aiorospy/requirements.in: -------------------------------------------------------------------------------- 1 | aiounittest 2 | janus 3 | async_generator 4 | -------------------------------------------------------------------------------- /aiorospy/requirements.txt: -------------------------------------------------------------------------------- 1 | aiounittest==1.4.3 # via -r requirements.in 2 | async-generator==1.10 # via -r requirements.in 3 | janus==1.0.0 # via -r requirements.in 4 | typing-extensions==4.12.2 # via janus 5 | wrapt==1.17.2 # via aiounittest 6 | -------------------------------------------------------------------------------- /aiorospy/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | import sys 6 | 7 | if sys.version_info < (3, 5): 8 | sys.exit('Sorry, Python < 3.5 is not supported') 9 | 10 | setup_args = generate_distutils_setup( 11 | packages=[ 12 | 'aiorospy' 13 | ], 14 | package_dir={'': 'src'}, 15 | python_requires='>3.5.0') 16 | 17 | setup(**setup_args) 18 | -------------------------------------------------------------------------------- /aiorospy/src/aiorospy/__init__.py: -------------------------------------------------------------------------------- 1 | from .action import AsyncActionClient, AsyncActionServer 2 | from .helpers import ExceptionMonitor, cancel_on_exception, cancel_on_shutdown, log_during 3 | from .service import AsyncService, AsyncServiceProxy 4 | from .topic import AsyncPublisher, AsyncSubscriber 5 | 6 | __all__ = ('AsyncSubscriber', 'AsyncService', 'AsyncServiceProxy', 'AsyncActionClient', 'AsyncActionServer') 7 | -------------------------------------------------------------------------------- /aiorospy/src/aiorospy/action.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | import logging 3 | from functools import partial 4 | 5 | import janus 6 | import rospy 7 | from actionlib import ActionClient, ActionServer, CommState, GoalStatus 8 | 9 | from .helpers import ExceptionMonitor, deflector_shield, log_during 10 | from .topic import AsyncSubscriber 11 | 12 | logger = logging.getLogger(__name__) 13 | 14 | 15 | class _AsyncGoalHandle: 16 | 17 | def __init__(self, name, exception_monitor, loop, feedback_queue_size): 18 | """ This class should not be user-constructed """ 19 | self._loop = loop 20 | self.status = None 21 | self.result = None 22 | 23 | self._name = name 24 | self._exception_monitor = exception_monitor 25 | self._feedback_queue = janus.Queue(maxsize=feedback_queue_size) 26 | self._old_statuses = set() 27 | 28 | self._done_event = asyncio.Event() 29 | self._status_cond = asyncio.Condition() 30 | 31 | async def feedback(self, log_period=None): 32 | """ Async generator providing feedback from the goal. The generator terminates when the goal 33 | is done. 34 | """ 35 | terminal_status = asyncio.ensure_future(self._done_event.wait()) 36 | try: 37 | while True: 38 | try: 39 | new_feedback = asyncio.ensure_future(self._feedback_queue.async_q.get()) 40 | done, pending = await asyncio.wait( 41 | {terminal_status, new_feedback}, 42 | return_when=asyncio.FIRST_COMPLETED, 43 | timeout=log_period) 44 | 45 | if new_feedback in done: 46 | yield new_feedback.result() 47 | 48 | elif terminal_status in done: 49 | return 50 | 51 | else: 52 | logger.info(f"Waiting for feedback on {self.goal_id} from {self._name}") 53 | finally: 54 | new_feedback.cancel() 55 | await deflector_shield(new_feedback) 56 | finally: 57 | terminal_status.cancel() 58 | await deflector_shield(terminal_status) 59 | 60 | async def reach_status(self, status, log_period=None): 61 | """ Await until the goal reaches a particular status. """ 62 | while True: 63 | async with self._status_cond: 64 | if status in self._old_statuses: 65 | return 66 | elif self._done_event.is_set(): 67 | raise RuntimeError(f"Action is done, will never reach status {GoalStatus.to_string(status)}") 68 | else: 69 | await log_during( 70 | self._status_cond.wait(), 71 | f"Waiting for {self.goal_id} on {self._name} to reach {GoalStatus.to_string(status)}", 72 | log_period) 73 | 74 | async def wait(self, log_period=None): 75 | """ Await until the goal terminates. """ 76 | return await log_during( 77 | self._done_event.wait(), 78 | f"Waiting for {self.goal_id} on {self._name} to complete", 79 | log_period 80 | ) 81 | 82 | def done(self): 83 | """ Specifies if the goal is terminated. """ 84 | return self._done_event.is_set() 85 | 86 | def cancel(self): 87 | """ Cancel the goal. """ 88 | # This gets injected by AsyncActionClient after init 89 | raise NotImplementedError() 90 | 91 | def cancelled(self): 92 | """ Specifies if the goal has been cancelled. """ 93 | return self.status in {GoalStatus.PREEMPTED, GoalStatus.PREEMPTING, GoalStatus.RECALLED, GoalStatus.RECALLING} 94 | 95 | def _transition_cb(self, goal_handle): 96 | try: 97 | future = asyncio.run_coroutine_threadsafe(self._process_transition( 98 | # We must use these accessors here instead of passing through goal_handle to avoid hitting deadlock 99 | status=goal_handle.get_goal_status(), 100 | comm_state=goal_handle.get_comm_state(), 101 | text=goal_handle.get_goal_status_text(), 102 | result=goal_handle.get_result() 103 | ), loop=self._loop) 104 | except RuntimeError: 105 | # Don't raise errors if a transition comes after the event loop is shutdown 106 | if not self._loop.is_closed(): 107 | raise 108 | 109 | def _feedback_cb(self, goal_handle, feedback): 110 | while True: 111 | try: 112 | self._feedback_queue.sync_q.put(feedback, block=False) 113 | break 114 | except janus.SyncQueueFull: 115 | try: 116 | self._feedback_queue.sync_q.get(block=False) 117 | except janus.SyncQueueEmpty: 118 | pass 119 | 120 | async def _process_transition(self, status, comm_state, text, result): 121 | async with self._status_cond: 122 | self.status = status 123 | self.comm_state = comm_state 124 | self.text = text 125 | 126 | logger.debug(f"Event in goal {self.goal_id}: status {GoalStatus.to_string(status)} result {result}") 127 | 128 | if self.status not in self._old_statuses: 129 | self._old_statuses.add(self.status) 130 | # (pbovbel) hack, if you accept a goal too quickly, we never see PENDING status 131 | # this is probably an issue elsewhere, and a DAG of action states would be great to have. 132 | if self.status != GoalStatus.PENDING and GoalStatus.PENDING not in self._old_statuses: 133 | self._old_statuses.add(GoalStatus.PENDING) 134 | 135 | if self.comm_state == CommState.DONE: 136 | self.result = result 137 | self._done_event.set() 138 | 139 | self._status_cond.notify_all() 140 | 141 | 142 | class AsyncActionClient: 143 | """ Async wrapper around the action client API. """ 144 | 145 | def __init__(self, name, action_spec, feedback_queue_size=10): 146 | self._loop = asyncio.get_event_loop() 147 | self.name = name 148 | self.action_spec = action_spec 149 | self._exception_monitor = ExceptionMonitor() 150 | self._feedback_queue_size = feedback_queue_size 151 | self._started_event = asyncio.Event() 152 | 153 | async def start(self): 154 | """ Start the action client. """ 155 | self._client = ActionClient(self.name, self.action_spec) 156 | try: 157 | self._started_event.set() 158 | await self._exception_monitor.start() 159 | finally: 160 | # TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142 161 | self._started_event.clear() 162 | self._client.stop() 163 | self._client = None 164 | 165 | async def _started(self, log_period=None): 166 | await log_during(self._started_event.wait(), f"Waiting for {self.name} client to be started...", period=5.0) 167 | 168 | async def wait_for_server(self, log_period=None): 169 | """ Wait for the action server to connect to this client. """ 170 | await self._started(log_period=log_period) 171 | await log_during(self._wait_for_server(), f"Waiting for {self.name} server...", period=log_period) 172 | 173 | async def _wait_for_server(self): 174 | while True: 175 | # Use a small timeout so that the execution can be cancelled if necessary 176 | connected = await self._loop.run_in_executor(None, self._client.wait_for_server, rospy.Duration(0.1)) 177 | if connected: 178 | return connected 179 | 180 | async def send_goal(self, goal): 181 | """ Send a goal to an action server. As in rospy, if you have not made sure the server is up and listening to 182 | the client, the goal will be swallowed. 183 | """ 184 | await self._started(log_period=5.0) 185 | async_handle = _AsyncGoalHandle( 186 | name=self.name, 187 | exception_monitor=self._exception_monitor, 188 | loop=self._loop, 189 | feedback_queue_size=self._feedback_queue_size 190 | ) 191 | sync_handle = self._client.send_goal( 192 | goal, 193 | transition_cb=async_handle._transition_cb, 194 | feedback_cb=async_handle._feedback_cb, 195 | ) 196 | async_handle.goal_id = sync_handle.comm_state_machine.action_goal.goal_id.id 197 | async_handle.cancel = sync_handle.cancel 198 | 199 | return async_handle 200 | 201 | async def ensure_goal(self, goal, resend_timeout): 202 | """ Send a goal to an action server. If the goal is not processed by the action server within resend_timeout, 203 | resend the goal. 204 | """ 205 | while True: 206 | await self.wait_for_server(log_period=5.0) 207 | handle = await self.send_goal(goal) 208 | try: 209 | await asyncio.wait_for(handle.reach_status(GoalStatus.PENDING), timeout=resend_timeout) 210 | except asyncio.TimeoutError: 211 | logger.warn(f"Action goal for {self.name} was not processed within timeout, resending") 212 | handle.cancel() 213 | continue 214 | except asyncio.CancelledError: 215 | handle.cancel() 216 | raise 217 | return handle 218 | 219 | 220 | class AsyncActionServer: 221 | """ Async wrapper around the action server API. """ 222 | 223 | def __init__(self, name, action_spec, coro, simple=False): 224 | """ Initialize an action server. Incoming goals will be processed via the speficied coroutine. """ 225 | self.name = name 226 | self.action_spec = action_spec 227 | self.simple = simple 228 | self.tasks = {} 229 | 230 | self._loop = asyncio.get_event_loop() 231 | self._coro = coro 232 | 233 | self._exception_monitor = ExceptionMonitor() 234 | 235 | async def start(self): 236 | """ Start the action server. """ 237 | self._server = ActionServer( 238 | self.name, self.action_spec, auto_start=False, 239 | # Make sure to run callbacks on the main thread 240 | goal_cb=partial(self._loop.call_soon_threadsafe, self._goal_cb), 241 | cancel_cb=partial(self._loop.call_soon_threadsafe, self._cancel_cb), 242 | ) 243 | try: 244 | self._server.start() 245 | await self._exception_monitor.start() 246 | finally: 247 | # TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142 248 | self._server.stop() 249 | self._server = None 250 | 251 | async def cancel(self, goal_handle): 252 | """ Cancel a particular goal's handler task. """ 253 | task = self._cancel_cb(goal_handle) 254 | if task: 255 | await deflector_shield(task) 256 | 257 | async def cancel_all(self): 258 | for task in self.tasks.values(): 259 | if not task.cancelled(): 260 | task.cancel() 261 | await deflector_shield(task) 262 | 263 | def _goal_cb(self, goal_handle): 264 | """ Process incoming goals by spinning off a new asynchronous task to handle the callback. 265 | """ 266 | goal_id = goal_handle.get_goal_id().id 267 | task = asyncio.ensure_future(self._wrapper_coro( 268 | goal_id=goal_id, 269 | goal_handle=goal_handle, 270 | preempt_tasks={**self.tasks} if self.simple else {}, 271 | )) 272 | task.add_done_callback(partial(self._task_done_callback, goal_handle=goal_handle)) 273 | self._exception_monitor.register_task(task) 274 | self.tasks[goal_id] = task 275 | 276 | async def _wrapper_coro(self, goal_id, goal_handle, preempt_tasks={}): 277 | """ Wrap the user-provided coroutine to allow the simple action mode to preempt any previously submitted goals. 278 | """ 279 | if preempt_tasks: 280 | logger.debug(f"Before goal {goal_id}, preempting {' '.join(self.tasks.keys())}") 281 | 282 | for other_task in preempt_tasks.values(): 283 | if not other_task.cancelled(): 284 | other_task.cancel() 285 | try: 286 | await deflector_shield(other_task) 287 | except asyncio.CancelledError: 288 | goal_handle.set_canceled(f"Goal {goal_id} was preempted before starting") 289 | raise 290 | 291 | logger.debug(f"Starting callback for goal {goal_id}") 292 | await self._coro(goal_handle) 293 | 294 | def _cancel_cb(self, goal_handle): 295 | """ Process incoming cancellations by finding the matching task and cancelling it. 296 | """ 297 | goal_id = goal_handle.get_goal_id().id 298 | try: 299 | task = self.tasks[goal_id] 300 | task.cancel() 301 | return task 302 | except KeyError: 303 | logger.debug(f"Received cancellation for untracked goal_id {goal_id}") 304 | return None 305 | 306 | PENDING_STATUS = {GoalStatus.PENDING, GoalStatus.RECALLING} 307 | ACTIVE_STATUS = {GoalStatus.ACTIVE, GoalStatus.PREEMPTING} 308 | NON_TERMINAL_STATUS = PENDING_STATUS | ACTIVE_STATUS 309 | 310 | def _task_done_callback(self, task, goal_handle): 311 | """ Process finished tasks and translate the result/exception into actionlib signals. 312 | """ 313 | goal_id = goal_handle.get_goal_id().id 314 | status = goal_handle.get_goal_status().status 315 | try: 316 | exc = task.exception() 317 | except asyncio.CancelledError: 318 | if status in self.NON_TERMINAL_STATUS: 319 | message = f"Task handler was cancelled, goal {goal_id} cancelled automatically" 320 | goal_handle.set_canceled(text=message) 321 | logger.warning( 322 | f"{message}. Please call set_canceled in the action server coroutine when CancelledError is raised") 323 | else: 324 | rejected_message = f"Goal {goal_id} rejected" 325 | aborted_message = f"Goal {goal_id} aborted" 326 | 327 | if exc is not None: 328 | reason = f"uncaught exception in actionserver handler: {exc}" 329 | if status in self.PENDING_STATUS: 330 | goal_handle.set_rejected(result=None, text=f"{rejected_message}, {reason}") 331 | elif status in self.ACTIVE_STATUS: 332 | goal_handle.set_aborted(result=None, text=f"{aborted_message}, {reason}") 333 | 334 | else: 335 | reason = f"never completed server-side" 336 | if status in self.PENDING_STATUS: 337 | goal_handle.set_rejected(result=None, text=f"{rejected_message}, {reason}") 338 | logger.warning(f"{rejected_message}, {reason}") 339 | elif status in self.ACTIVE_STATUS: 340 | goal_handle.set_aborted(result=None, text=f"{aborted_message}, {reason}") 341 | logger.warning(f"{aborted_message}, {reason}") 342 | 343 | del self.tasks[goal_id] 344 | -------------------------------------------------------------------------------- /aiorospy/src/aiorospy/helpers.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | import concurrent.futures 3 | import functools 4 | import logging 5 | import subprocess 6 | import time 7 | import sys 8 | 9 | import janus 10 | import rospy 11 | 12 | if sys.version_info >= (3, 9): 13 | from contextlib import asynccontextmanager 14 | else: 15 | from async_generator import asynccontextmanager 16 | 17 | logger = logging.getLogger(__name__) 18 | 19 | 20 | def cancel_on_shutdown(task): 21 | loop = asyncio.get_event_loop() 22 | rospy.on_shutdown(lambda: loop.call_soon_threadsafe(task.cancel)) 23 | 24 | 25 | def cancel_on_exception(task): 26 | def handler(loop, context): 27 | loop.default_exception_handler(context) 28 | if not task.cancelled(): 29 | task.cancel() 30 | 31 | asyncio.get_event_loop().set_exception_handler(handler) 32 | 33 | 34 | class ExceptionMonitor: 35 | """ Monitor exceptions in background tasks so they don't get ignored. 36 | """ 37 | 38 | def __init__(self): 39 | self._pending_tasks = set() 40 | 41 | async def start(self): 42 | """ Monitor registered background tasks, and raise their exceptions. 43 | """ 44 | self._exception_q = janus.Queue() 45 | try: 46 | while True: 47 | exc = await self._exception_q.async_q.get() 48 | raise exc 49 | 50 | except asyncio.CancelledError: 51 | # Cancel any tasks that will no longer be monitored 52 | while self._pending_tasks: 53 | task = self._pending_tasks.pop() 54 | if not task.cancelled(): 55 | task.cancel() 56 | 57 | # We can't await a concurrent.future task, make sure it comes from asyncio 58 | if asyncio.isfuture(task): 59 | await task 60 | raise 61 | 62 | finally: 63 | try: 64 | exc = self._exception_q.async_q.get_nowait() 65 | raise exc 66 | except asyncio.QueueEmpty: 67 | pass 68 | 69 | def register_task(self, task): 70 | """ Register a task with the exception monitor. If the exception monitor is shutdown, all registered 71 | tasks will be cancelled. Supports asyncio and concurrent.futures tasks. 72 | """ 73 | task.add_done_callback(self._task_done_callback) 74 | self._pending_tasks.add(task) 75 | 76 | def _task_done_callback(self, task): 77 | """ When a task monitored by this ExceptionMonitor finishes, we want to check if there are any uncaught 78 | exceptions. Cancellations are normal and should be supressed, but everything else should be passed up to 79 | the monitor queue. """ 80 | try: 81 | self._pending_tasks.remove(task) 82 | except KeyError: 83 | pass 84 | 85 | try: 86 | exc = task.exception() 87 | except asyncio.CancelledError: 88 | # asyncio.Future.exception will raise CancelledError 89 | pass 90 | else: 91 | # concurrent.futures.Future.exception will return CancelledError 92 | if exc is None or isinstance(exc, concurrent.futures.CancelledError): 93 | pass 94 | else: 95 | self._exception_q.sync_q.put(exc) 96 | 97 | 98 | def iscoroutinefunction_or_partial(fxn): 99 | """ Before python3.8, asyncio.iscoroutine is unable to examine coroutines wrapped via partial. 100 | See https://stackoverflow.com/a/52422903/1198131 101 | """ 102 | while isinstance(fxn, functools.partial): 103 | fxn = fxn.func 104 | return asyncio.iscoroutinefunction(fxn) 105 | 106 | 107 | async def do_while(awaitable, period, do, *args, **kwargs): 108 | """ Convience function to periodically 'do' a callable while an awaitable is in progress. """ 109 | if period is not None: 110 | task = asyncio.ensure_future(awaitable) 111 | while True: 112 | try: 113 | result = await asyncio.wait_for( 114 | asyncio.shield(task), 115 | timeout=period) 116 | return result 117 | except asyncio.TimeoutError: 118 | if iscoroutinefunction_or_partial(do): 119 | await do(*args, **kwargs) 120 | else: 121 | do(*args, **kwargs) 122 | except asyncio.CancelledError: 123 | task.cancel() 124 | await task 125 | raise 126 | else: 127 | return await awaitable 128 | 129 | 130 | async def log_during(awaitable, msg, period, sink=logger.info): 131 | """ Convenience function to repeatedly log a line, while some task has not completed. """ 132 | return await do_while(awaitable, period, sink, msg) 133 | 134 | 135 | class ChildCancelled(asyncio.CancelledError): 136 | pass 137 | 138 | 139 | async def detect_cancel(task): 140 | """ asyncio makes it very hard to distinguish an inner cancel from an outer cancel. 141 | See this thread https://stackoverflow.com/a/55424838/1198131. 142 | """ 143 | cont = asyncio.get_event_loop().create_future() 144 | 145 | def on_done(_): 146 | if task.cancelled(): 147 | cont.set_exception(ChildCancelled()) 148 | elif task.exception() is not None: 149 | cont.set_exception(task.exception()) 150 | else: 151 | cont.set_result(task.result()) 152 | 153 | task.add_done_callback(on_done) 154 | return await cont 155 | 156 | 157 | async def deflector_shield(task): 158 | """ Wrap a task with deflector_shield if you want to await its completion from a coroutine, but not get cancelled 159 | yourself if the wrapped task is cancelled. 160 | """ 161 | try: 162 | return await detect_cancel(asyncio.shield(task)) 163 | except ChildCancelled: 164 | return None # supress propagating an 'inner' cancel 165 | 166 | 167 | @asynccontextmanager 168 | async def subprocess_run(command, sudo=False, capture_output=False, terminate_timeout=5.0, *args, **kwargs): 169 | """ Higher level wrapper for asyncio.subprocess_exec, to run a command asynchronously. 170 | :param command: Command to be executed in a list. e.g. ['ls', '-l'] 171 | :param sudo: Appends sudo before the command. 172 | :param check: Check if the command exited with a 0 returncode. 173 | :param wait: Wait for the command to complete. If false, returns a handle to the process. 174 | :param capture_output: Pipe stdout and stderr to the process handle. 175 | """ 176 | process = await subprocess_start(command, sudo, capture_output, *args, **kwargs) 177 | try: 178 | yield process 179 | finally: 180 | await subprocess_end(process, terminate_timeout) 181 | 182 | 183 | async def subprocess_start(command, sudo=False, capture_output=False, *args, **kwargs): 184 | if sudo: 185 | command = ['sudo', '-S'] + command 186 | 187 | logger.debug(' '.join(command)) 188 | 189 | if capture_output: 190 | kwargs['stdout'] = asyncio.subprocess.PIPE 191 | kwargs['stderr'] = asyncio.subprocess.PIPE 192 | 193 | process = await asyncio.create_subprocess_exec( 194 | *command, *args, **kwargs) 195 | 196 | return process 197 | 198 | 199 | async def subprocess_end(process, terminate_timeout=5.0): 200 | if process.returncode is None: 201 | process.terminate() 202 | try: 203 | await asyncio.wait_for(process.wait(), timeout=terminate_timeout) 204 | except asyncio.TimeoutError: 205 | logger.warn(f"Process did not terminate, escalating to kill: {command}") 206 | process.kill() 207 | await process.wait() 208 | 209 | 210 | async def subprocess_complete(process, check=False): 211 | # This will not be accessible after process.communicate() completes 212 | args = process._transport._proc.args 213 | 214 | stdout, stderr = await process.communicate() 215 | 216 | if check and process.returncode != 0: 217 | raise subprocess.CalledProcessError(returncode=process.returncode, 218 | cmd=args, 219 | output=stdout, 220 | stderr=stderr) 221 | 222 | return subprocess.CompletedProcess(args=args, 223 | returncode=process.returncode, 224 | stdout=stdout, 225 | stderr=stderr) 226 | 227 | 228 | class Timer: 229 | """ Run something periodically, suspending the coroutine inbetween. 230 | ``` 231 | timer = Timer(period=period) 232 | while True: 233 | async with timer: 234 | something() 235 | ``` 236 | """ 237 | def __init__(self, period=1.0): 238 | self.period = period 239 | self.last = self.now = time.time() 240 | 241 | async def __aenter__(self): 242 | delta = self.now + self.period - time.time() 243 | if delta > 0: 244 | await asyncio.sleep(delta) 245 | 246 | self.now = time.time() 247 | 248 | async def __aexit__(self, exc_type, exc, tb): 249 | self.last = self.now 250 | 251 | 252 | class ExponentialSleep: 253 | def __init__(self, min_period, max_period): 254 | self._min_period = min_period 255 | self._max_period = max_period 256 | self._n = 0 257 | 258 | @property 259 | def next_sleep_time(self): 260 | return min(self._max_period, (2 ** self._n) * self._min_period) 261 | 262 | async def __call__(self): 263 | await asyncio.sleep(self.next_sleep_time) 264 | self._n += 1 265 | -------------------------------------------------------------------------------- /aiorospy/src/aiorospy/service.py: -------------------------------------------------------------------------------- 1 | 2 | import asyncio 3 | import functools 4 | import logging 5 | import sys 6 | 7 | import rospy 8 | 9 | from .helpers import ExceptionMonitor, ExponentialSleep, log_during 10 | 11 | logger = logging.getLogger(__name__) 12 | 13 | 14 | class AsyncServiceProxy: 15 | 16 | def __init__(self, name, service_class): 17 | self.name = name 18 | self.service_class = service_class 19 | self._loop = asyncio.get_event_loop() 20 | self._lock = asyncio.Lock() 21 | self._srv_proxy = rospy.ServiceProxy(name, service_class) 22 | 23 | async def wait_for_service(self, log_period): 24 | """ Wait for a ROS service to be available. """ 25 | await log_during(self._wait_for_service(), f"Waiting for service {self.name}...", log_period) 26 | 27 | async def _wait_for_service(self): 28 | while True: 29 | try: 30 | # ~Use a small timeout so the execution can be cancelled if necessary~ 31 | # Use a large timeout, otherwise wait_for_service will never succeed in bad networking environments. 32 | return await self._loop.run_in_executor(None, self._srv_proxy.wait_for_service, 10.0) 33 | except rospy.ROSException: 34 | continue 35 | 36 | async def send(self, *args, **kwargs): 37 | """ Send a request to a ROS service. """ 38 | log_period = kwargs.pop('log_period', None) 39 | service_call = functools.partial(self._srv_proxy.call, *args, **kwargs) 40 | # Non-persistent ServiceProxy is not thread safe 41 | # https://github.com/ros/ros_comm/blob/noetic-devel/clients/rospy/src/rospy/impl/tcpros_service.py#L534 42 | async with self._lock: 43 | try: 44 | return await log_during(self._loop.run_in_executor(None, service_call), 45 | f"Trying to call service {self.name}...", log_period) 46 | except rospy.exceptions.TransportTerminated: 47 | # service proxy transport was disconnected and needs to be re-created 48 | self._srv_proxy = rospy.ServiceProxy(self.name, self.service_class) 49 | raise 50 | 51 | async def ensure(self, *args, **kwargs): 52 | """ Send a request to a ROS service, retrying if comms failure is detected. """ 53 | log_period = kwargs.pop('log_period', None) 54 | max_retry_delay = kwargs.pop('max_retry_delay', 30) 55 | sleep = ExponentialSleep(1, max_retry_delay) 56 | 57 | while True: 58 | await self.wait_for_service(log_period) 59 | try: 60 | return await self.send(*args, **kwargs) 61 | except (rospy.ServiceException, rospy.exceptions.ROSException, rospy.exceptions.ROSInternalException) as e: 62 | logger.exception(f"Caught exception {e}, retrying service call after {sleep.next_sleep_time}s...") 63 | await sleep() 64 | continue 65 | except AttributeError as e: 66 | logger.exception(f"Caught AttributeError {e}, recreating service proxy " 67 | f"and retrying service call after {sleep.next_sleep_time}s...") 68 | await sleep() 69 | self._srv_proxy = rospy.ServiceProxy(self.name, self.service_class) 70 | continue 71 | 72 | 73 | class AsyncService: 74 | 75 | def __init__(self, name, service_class, coro): 76 | self._loop = asyncio.get_event_loop() 77 | self.name = name 78 | self.service_class = service_class 79 | self._coro = coro 80 | self._exception_monitor = ExceptionMonitor() 81 | 82 | async def start(self): 83 | """ Start the ROS service server """ 84 | self._srv = rospy.Service( 85 | self.name, self.service_class, self._handler, 86 | # We don't need rospy's internal exception handler, which just logs the errors. 87 | error_handler=lambda e, exc_type, exc_value, tb: None 88 | ) 89 | 90 | try: 91 | await self._exception_monitor.start() 92 | finally: 93 | self._srv.shutdown() 94 | 95 | def _handler(self, msg): 96 | future = asyncio.run_coroutine_threadsafe(self._coro(msg), loop=self._loop) 97 | self._exception_monitor.register_task(future) 98 | 99 | # Blocks until the future has a result. 100 | return future.result() 101 | -------------------------------------------------------------------------------- /aiorospy/src/aiorospy/topic.py: -------------------------------------------------------------------------------- 1 | import asyncio 2 | from functools import partial 3 | 4 | import janus 5 | import rospy 6 | 7 | 8 | class AsyncSubscriber: 9 | def __init__(self, name, data_class, queue_size=None): 10 | """ Create an asynchronous subscriber. """ 11 | self.name = name 12 | self._data_class = data_class 13 | self._queue_size = queue_size 14 | 15 | async def subscribe(self): 16 | """ Generator to pull messages from a subscription. """ 17 | queue = janus.Queue(maxsize=self._queue_size if self._queue_size is not None else 0) 18 | self._subscriber = rospy.Subscriber( 19 | self.name, 20 | self._data_class, 21 | queue_size=self._queue_size, 22 | callback=partial(self._callback, queue=queue)) 23 | try: 24 | while not rospy.is_shutdown(): 25 | yield await queue.async_q.get() 26 | finally: 27 | self._subscriber.unregister() 28 | 29 | def _callback(self, msg, queue): 30 | while True: 31 | try: 32 | queue.sync_q.put_nowait(msg) 33 | break 34 | except janus.SyncQueueFull: 35 | # Drop a single message from the queue 36 | try: 37 | _ = queue.sync_q.get() 38 | except janus.SyncQueueEmpty: 39 | pass 40 | 41 | 42 | class AsyncPublisher(rospy.Publisher): 43 | pass 44 | -------------------------------------------------------------------------------- /aiorospy/tests/test_action_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import sys 4 | import unittest 5 | from threading import Event 6 | 7 | import aiounittest 8 | import rospy 9 | import rostest 10 | from actionlib import ActionServer, GoalStatus 11 | from actionlib.msg import TestAction, TestFeedback, TestGoal, TestResult 12 | from aiorospy import AsyncActionClient 13 | from aiorospy.helpers import deflector_shield 14 | 15 | 16 | class TestActionClient(aiounittest.AsyncTestCase): 17 | 18 | @classmethod 19 | def setUpClass(cls): 20 | rospy.init_node("test_action_client", anonymous=True, disable_signals=True) 21 | 22 | def create_server(self, ns, goal_cb=None, cancel_cb=None, auto_start=True): 23 | action_server = ActionServer(ns, TestAction, goal_cb=goal_cb, cancel_cb=cancel_cb, auto_start=False) 24 | if auto_start: 25 | action_server.start() 26 | return action_server 27 | 28 | async def test_success_result(self): 29 | expected_result = TestResult(0) 30 | feedback_up_to = 3 31 | 32 | def goal_cb(goal_handle): 33 | goal_handle.set_accepted() 34 | for idx in range(feedback_up_to): 35 | goal_handle.publish_feedback(TestFeedback(idx)) 36 | goal_handle.set_succeeded(result=expected_result) 37 | 38 | server = self.create_server("test_success_result", goal_cb) 39 | client = AsyncActionClient(server.ns, TestAction) 40 | client_task = asyncio.ensure_future(client.start()) 41 | 42 | await client.wait_for_server() 43 | goal_handle = await client.send_goal(TestGoal(1)) 44 | 45 | idx = 0 46 | async for feedback in goal_handle.feedback(log_period=1.0): 47 | print(f"feedback {idx}") 48 | self.assertEqual(feedback, TestFeedback(idx)) 49 | idx += 1 50 | 51 | await goal_handle.wait() 52 | self.assertEqual(goal_handle.status, GoalStatus.SUCCEEDED) 53 | self.assertEqual(expected_result, goal_handle.result) 54 | 55 | client_task.cancel() 56 | await deflector_shield(client_task) 57 | 58 | async def test_wait_for_result(self): 59 | received_accepted = Event() 60 | 61 | def goal_cb(goal_handle): 62 | goal_handle.set_accepted() 63 | received_accepted.wait() 64 | goal_handle.set_succeeded(result=TestResult(0)) 65 | 66 | server = self.create_server("test_wait_for_result", goal_cb) 67 | client = AsyncActionClient(server.ns, TestAction) 68 | client_task = asyncio.ensure_future(client.start()) 69 | 70 | await client.wait_for_server() 71 | goal_handle = await client.send_goal(TestGoal(1)) 72 | await goal_handle.reach_status(GoalStatus.ACTIVE, log_period=1.0) 73 | 74 | received_accepted.set() 75 | 76 | with self.assertRaises(RuntimeError): 77 | await goal_handle.reach_status(GoalStatus.REJECTED, log_period=1.0) 78 | 79 | await goal_handle.reach_status(GoalStatus.SUCCEEDED, log_period=1.0) 80 | 81 | with self.assertRaises(RuntimeError): 82 | await goal_handle.reach_status(GoalStatus.REJECTED, log_period=1.0) 83 | 84 | client_task.cancel() 85 | await deflector_shield(client_task) 86 | 87 | async def test_cancel(self): 88 | def goal_cb(goal_handle): 89 | goal_handle.set_accepted() 90 | 91 | def cancel_cb(goal_handle): 92 | goal_handle.set_canceled() 93 | 94 | server = self.create_server("test_cancel", goal_cb, cancel_cb) 95 | client = AsyncActionClient(server.ns, TestAction) 96 | client_task = asyncio.ensure_future(client.start()) 97 | 98 | await client.wait_for_server() 99 | goal_handle = await client.send_goal(TestGoal()) 100 | await goal_handle.reach_status(GoalStatus.ACTIVE, log_period=1.0) 101 | 102 | goal_handle.cancel() 103 | await goal_handle.reach_status(GoalStatus.PREEMPTED, log_period=1.0) 104 | 105 | client_task.cancel() 106 | await deflector_shield(client_task) 107 | 108 | async def test_ensure(self): 109 | def goal_cb(goal_handle): 110 | goal_handle.set_accepted() 111 | 112 | server = self.create_server("test_ensure", goal_cb, auto_start=False) 113 | client = AsyncActionClient(server.ns, TestAction) 114 | client_task = asyncio.ensure_future(client.start()) 115 | 116 | with self.assertRaises(asyncio.TimeoutError): 117 | await asyncio.wait_for(client.ensure_goal(TestGoal(), resend_timeout=0.1), timeout=1) 118 | 119 | server.start() 120 | 121 | goal_handle = await client.ensure_goal(TestGoal(), resend_timeout=0.1) 122 | await goal_handle.reach_status(GoalStatus.ACTIVE, log_period=1.0) 123 | 124 | client_task.cancel() 125 | await deflector_shield(client_task) 126 | 127 | 128 | if __name__ == '__main__': 129 | rostest.rosrun('aiorospy', 'test_action_client', TestActionClient, sys.argv) 130 | -------------------------------------------------------------------------------- /aiorospy/tests/test_action_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import sys 4 | import unittest 5 | 6 | import aiounittest 7 | import janus 8 | import rospy 9 | import rostest 10 | from actionlib import ActionClient as SyncActionClient 11 | from actionlib import CommState, GoalStatus 12 | from actionlib.msg import TestAction, TestGoal, TestResult 13 | from aiorospy import AsyncActionServer 14 | from aiorospy.helpers import deflector_shield 15 | 16 | 17 | class TestActionServer(aiounittest.AsyncTestCase): 18 | 19 | @classmethod 20 | def setUpClass(cls): 21 | rospy.init_node("test_action_server", anonymous=True, disable_signals=True) 22 | 23 | async def wait_for_status(self, goal_handle, status): 24 | while goal_handle.get_goal_status() != status: 25 | await asyncio.sleep(0.1) 26 | 27 | async def wait_for_result(self, goal_handle): 28 | while goal_handle.get_comm_state() != CommState.DONE: 29 | await asyncio.sleep(0.1) 30 | 31 | async def test_goal_succeeded(self): 32 | magic_value = 5 33 | 34 | async def goal_coro(goal_handle): 35 | goal_handle.set_accepted() 36 | goal_handle.set_succeeded(result=TestResult(goal_handle.get_goal().goal)) 37 | 38 | client = SyncActionClient("test_goal_succeeded", TestAction) 39 | server = AsyncActionServer(client.ns, TestAction, coro=goal_coro) 40 | server_task = asyncio.ensure_future(server.start()) 41 | 42 | await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server) 43 | goal_handle = client.send_goal(TestGoal(magic_value)) 44 | 45 | await self.wait_for_result(goal_handle) 46 | 47 | self.assertEqual(goal_handle.get_goal_status(), GoalStatus.SUCCEEDED) 48 | self.assertEqual(goal_handle.get_result().result, magic_value) 49 | 50 | server_task.cancel() 51 | await deflector_shield(server_task) 52 | 53 | async def test_goal_canceled_from_client(self): 54 | async def goal_coro(goal_handle): 55 | try: 56 | goal_handle.set_accepted() 57 | await asyncio.sleep(1000000) 58 | except asyncio.CancelledError: 59 | goal_handle.set_canceled() 60 | raise 61 | goal_handle.set_succeeded() 62 | 63 | client = SyncActionClient("test_goal_canceled_from_client", TestAction) 64 | server = AsyncActionServer(client.ns, TestAction, coro=goal_coro) 65 | server_task = asyncio.ensure_future(server.start()) 66 | 67 | await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server) 68 | goal_handle = client.send_goal(TestGoal()) 69 | 70 | await self.wait_for_status(goal_handle, GoalStatus.ACTIVE) 71 | 72 | goal_handle.cancel() 73 | 74 | await self.wait_for_status(goal_handle, GoalStatus.PREEMPTED) 75 | self.assertEquals(goal_handle.get_goal_status(), GoalStatus.PREEMPTED) 76 | 77 | server_task.cancel() 78 | await deflector_shield(server_task) 79 | 80 | async def test_goal_canceled_from_server(self): 81 | queue = janus.Queue() 82 | 83 | async def goal_coro(goal_handle): 84 | queue.sync_q.put_nowait(goal_handle) 85 | try: 86 | goal_handle.set_accepted() 87 | await asyncio.sleep(1000000) 88 | except asyncio.CancelledError: 89 | goal_handle.set_canceled() 90 | raise 91 | goal_handle.set_succeeded() 92 | 93 | client = SyncActionClient("test_goal_canceled_from_server", TestAction) 94 | server = AsyncActionServer(client.ns, TestAction, coro=goal_coro) 95 | server_task = asyncio.ensure_future(server.start()) 96 | 97 | await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server) 98 | goal_handle = client.send_goal(TestGoal()) 99 | 100 | await self.wait_for_status(goal_handle, GoalStatus.ACTIVE) 101 | 102 | server_goal_handle = await queue.async_q.get() 103 | await server.cancel(server_goal_handle) 104 | 105 | await self.wait_for_status(goal_handle, GoalStatus.PREEMPTED) 106 | self.assertEquals(goal_handle.get_goal_status(), GoalStatus.PREEMPTED) 107 | 108 | server_task.cancel() 109 | await deflector_shield(server_task) 110 | 111 | async def test_goal_exception(self): 112 | async def goal_coro(goal_handle): 113 | goal_handle.set_accepted() 114 | raise RuntimeError() 115 | 116 | client = SyncActionClient("test_goal_aborted", TestAction) 117 | server = AsyncActionServer(client.ns, TestAction, coro=goal_coro) 118 | server_task = asyncio.ensure_future(server.start()) 119 | 120 | await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server) 121 | goal_handle = client.send_goal(TestGoal()) 122 | 123 | await self.wait_for_status(goal_handle, GoalStatus.ABORTED) 124 | self.assertEquals(goal_handle.get_goal_status(), GoalStatus.ABORTED) 125 | 126 | with self.assertRaises(RuntimeError): 127 | await deflector_shield(server_task) 128 | 129 | async def test_server_simple(self): 130 | event = asyncio.Event() 131 | 132 | async def goal_coro(goal_handle): 133 | delay = goal_handle.get_goal().goal 134 | try: 135 | if event.is_set(): 136 | raise RuntimeError(f"Event wasn't cleared by another goal, bail!") 137 | event.set() 138 | goal_handle.set_accepted() 139 | 140 | await asyncio.sleep(delay) 141 | 142 | except asyncio.CancelledError: 143 | event.clear() 144 | goal_handle.set_canceled() 145 | raise 146 | 147 | event.clear() 148 | goal_handle.set_succeeded(result=TestResult(delay)) 149 | 150 | client = SyncActionClient("test_server_simple", TestAction) 151 | server = AsyncActionServer(client.ns, TestAction, coro=goal_coro, simple=True) 152 | server_task = asyncio.ensure_future(server.start()) 153 | 154 | await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server) 155 | 156 | handles = [] 157 | for i in range(10): 158 | handle = client.send_goal(TestGoal(1000000)) 159 | await self.wait_for_status(handle, GoalStatus.ACTIVE) 160 | handles.append(handle) 161 | 162 | last_handle = client.send_goal(TestGoal(0)) 163 | await self.wait_for_status(last_handle, GoalStatus.SUCCEEDED) 164 | 165 | for handle in handles: 166 | self.assertEqual(handle.get_goal_status(), GoalStatus.PREEMPTED) 167 | self.assertEqual(last_handle.get_goal_status(), GoalStatus.SUCCEEDED) 168 | 169 | server_task.cancel() 170 | await deflector_shield(server_task) 171 | 172 | 173 | if __name__ == '__main__': 174 | rostest.rosrun('aiorospy', 'test_action_server', TestActionServer, sys.argv) 175 | -------------------------------------------------------------------------------- /aiorospy/tests/test_aiorospy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /aiorospy/tests/test_service.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import sys 4 | import unittest 5 | 6 | import aiounittest 7 | import rospy 8 | import rostest 9 | from aiorospy import AsyncService 10 | from aiorospy.helpers import deflector_shield 11 | from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse 12 | 13 | 14 | class TestServiceProxy(aiounittest.AsyncTestCase): 15 | 16 | @classmethod 17 | def setUpClass(cls): 18 | rospy.init_node("test_node", anonymous=True, disable_signals=True) 19 | 20 | def setUp(self): 21 | self.client = rospy.ServiceProxy("test_service", SetBool) 22 | 23 | async def test_service_normal(self): 24 | loop = asyncio.get_event_loop() 25 | 26 | async def callback(req): 27 | return SetBoolResponse(success=req.data) 28 | 29 | self.server = AsyncService("test_service", SetBool, callback) 30 | server_task = asyncio.ensure_future(self.server.start()) 31 | 32 | await loop.run_in_executor(None, self.client.wait_for_service) 33 | response = await loop.run_in_executor(None, self.client.call, True) 34 | self.assertEquals(True, response.success) 35 | 36 | server_task.cancel() 37 | await deflector_shield(server_task) 38 | 39 | async def test_service_exception(self): 40 | loop = asyncio.get_event_loop() 41 | 42 | async def callback(req): 43 | raise RuntimeError() 44 | 45 | self.server = AsyncService("test_service", SetBool, callback) 46 | server_task = asyncio.ensure_future(self.server.start()) 47 | 48 | await loop.run_in_executor(None, self.client.wait_for_service) 49 | 50 | with self.assertRaises(rospy.ServiceException): 51 | response = await loop.run_in_executor(None, self.client.call, True) 52 | 53 | with self.assertRaises(RuntimeError): 54 | await deflector_shield(server_task) 55 | 56 | 57 | if __name__ == '__main__': 58 | rostest.rosrun('aiorospy', 'test_service_proxy', TestServiceProxy, sys.argv) 59 | -------------------------------------------------------------------------------- /aiorospy/tests/test_service_proxy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import sys 4 | import unittest 5 | 6 | import aiounittest 7 | import rospy 8 | import rostest 9 | from aiorospy import AsyncServiceProxy 10 | from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse 11 | 12 | 13 | class TestServiceProxy(aiounittest.AsyncTestCase): 14 | 15 | @classmethod 16 | def setUpClass(cls): 17 | rospy.init_node("test_node", anonymous=True, disable_signals=True) 18 | 19 | def setUp(self): 20 | self.server = rospy.Service("test_service", SetBool, self.callback) 21 | 22 | def callback(self, req): 23 | return SetBoolResponse(success=req.data) 24 | 25 | async def test_service_proxy(self): 26 | client = AsyncServiceProxy("test_service", SetBool) 27 | response = await client.ensure(False) 28 | self.assertEquals(False, response.success) 29 | response = await client.ensure(data=True) 30 | self.assertEquals(True, response.success) 31 | 32 | 33 | if __name__ == '__main__': 34 | rostest.rosrun('aiorospy', 'test_service_proxy', TestServiceProxy, sys.argv) 35 | -------------------------------------------------------------------------------- /aiorospy/tests/test_subscriber.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import sys 4 | import unittest 5 | 6 | import aiounittest 7 | import rospy 8 | import rostest 9 | from aiorospy import AsyncSubscriber 10 | from aiorospy.helpers import deflector_shield 11 | from std_msgs.msg import Int16 12 | 13 | 14 | class TestSubscriber(aiounittest.AsyncTestCase): 15 | 16 | @classmethod 17 | def setUpClass(cls): 18 | rospy.init_node("test_subscriber", anonymous=True, disable_signals=True) 19 | 20 | async def test_subscriber(self): 21 | message_quantity = 5 22 | to_send = [Int16(idx) for idx in range(message_quantity)] 23 | 24 | async def publish(): 25 | pub = rospy.Publisher("test_subscriber", Int16, queue_size=message_quantity) 26 | while pub.get_num_connections() <= 0: 27 | await asyncio.sleep(0.1) 28 | 29 | for msg in to_send: 30 | pub.publish(msg) 31 | 32 | pub_task = asyncio.ensure_future(publish()) 33 | 34 | received = [] 35 | 36 | sub = AsyncSubscriber("test_subscriber", Int16) 37 | 38 | async for msg in sub.subscribe(): 39 | received.append(msg) 40 | if len(received) == len(to_send): 41 | break 42 | 43 | self.assertEqual(to_send, received) 44 | 45 | pub_task.cancel() 46 | await deflector_shield(pub_task) 47 | 48 | @unittest.skip("Test is flaky, hard to be deterministic across three buffers.") 49 | async def test_subscriber_small_queue(self): 50 | queue_size = 1 51 | message_quantity = 10 52 | 53 | to_send = [Int16(idx) for idx in range(message_quantity)] 54 | 55 | async def publish(): 56 | pub = rospy.Publisher("test_subscriber_small_queue", Int16, queue_size=len(to_send)) 57 | while pub.get_num_connections() <= 0: 58 | await asyncio.sleep(1) 59 | 60 | for msg in to_send: 61 | pub.publish(msg) 62 | 63 | pub_task = asyncio.ensure_future(publish()) 64 | 65 | received = [] 66 | 67 | async def subscribe(): 68 | sub = AsyncSubscriber("test_subscriber_small_queue", Int16, queue_size=queue_size) 69 | async for msg in sub.subscribe(): 70 | received.append(msg) 71 | 72 | with self.assertRaises(asyncio.TimeoutError): 73 | await asyncio.wait_for(subscribe(), timeout=1.0) 74 | 75 | self.assertTrue(len(received) < message_quantity) 76 | 77 | pub_task.cancel() 78 | await deflector_shield(pub_task) 79 | 80 | 81 | if __name__ == '__main__': 82 | rostest.rosrun('aiorospy', 'test_subscriber', TestSubscriber, sys.argv) 83 | -------------------------------------------------------------------------------- /aiorospy_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package aiorospy_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.0 (2022-02-23) 6 | ------------------ 7 | * Prepare for noetic release with python3 (#39) 8 | * replace python_interpreter 3.6 with 3 9 | * Check python version with setuptools 10 | * Fix lint 11 | Co-authored-by: betaboon 12 | Co-authored-by: Paul Bovbel 13 | * Contributors: Doug Smith 14 | 15 | 0.4.0 (2023-02-22) 16 | ------------------ 17 | * 0.3.0 18 | * Update changelogs 19 | * Prepare for noetic release with python3 (#39) 20 | * replace python_interpreter 3.6 with 3 21 | * Check python version with setuptools 22 | * Fix lint 23 | Co-authored-by: betaboon 24 | Co-authored-by: Paul Bovbel 25 | * Contributors: Doug Smith, Gary Servin 26 | 27 | 0.5.0 (2023-09-25) 28 | ------------------ 29 | * 0.4.0 30 | * Update changelogs 31 | * 0.3.0 32 | * Update changelogs 33 | * Prepare for noetic release with python3 (#39) 34 | * replace python_interpreter 3.6 with 3 35 | * Check python version with setuptools 36 | * Fix lint 37 | Co-authored-by: betaboon 38 | Co-authored-by: Paul Bovbel 39 | * Contributors: Doug Smith, Gary Servin 40 | 41 | 0.10.0 (2025-06-06) 42 | ------------------- 43 | * Update packages dependencies to build on Noble (#48) 44 | * Update dependencies 45 | * Update cmake minimum version 46 | * Update dependecies 47 | * Update github actions 48 | * Match versions to the lowest common version 49 | * Contributors: Gary Servin 50 | 51 | 0.9.0 (2025-02-04) 52 | ------------------ 53 | 54 | 0.8.0 (2024-09-16) 55 | ------------------ 56 | 57 | 0.7.0 (2024-06-17) 58 | ------------------ 59 | 60 | 0.6.0 (2024-02-02) 61 | ------------------ 62 | * 0.5.0 63 | * Update changelogs 64 | * 0.4.0 65 | * Update changelogs 66 | * 0.3.0 67 | * Update changelogs 68 | * Prepare for noetic release with python3 (#39) 69 | * replace python_interpreter 3.6 with 3 70 | * Check python version with setuptools 71 | * Fix lint 72 | Co-authored-by: betaboon 73 | Co-authored-by: Paul Bovbel 74 | * Contributors: Doug Smith, Gary Servin 75 | 76 | 0.2.0 (2020-10-02) 77 | ------------------ 78 | * Lock requirements 79 | * python3.6 support (`#29 `_) 80 | * Implement simple action server (`#26 `_) 81 | Implement proper simple action server, and fix a bunch of flaky tests. 82 | * Remove locus_msgs 83 | * Contributors: Paul Bovbel, betaboon 84 | 85 | 0.1.0 (2019-07-12) 86 | ------------------ 87 | * Cleanup (`#18 `_) 88 | * Also stop action client to prevent memory leaks 89 | * Fix virtualenv for examples; drop requirement on std_srv fork 90 | * Idiomatic bool random 91 | * Usability fixes (`#17 `_) 92 | log_during async helper to log periodically while waiting for an awaitable 93 | Periodic logging to async-blocking methods in services and actions 94 | Automatically clean up actions that are improperly terminated 95 | * Implement simple_actions demo; fix bug in ExecutionMonitor (`#16 `_) 96 | * Update internal components and examples (`#14 `_) 97 | * Re-implement actions, services 98 | * Add tests 99 | * Update examples 100 | * Async Actions (`#7 `_) 101 | Actions and subscriber rewrite 102 | * return state and result (`#6 `_) 103 | * return state and result 104 | * Split off aiorospy_examples (`#5 `_) 105 | * Split off an aiorospy_examples package to avoid pinning python version 106 | * Restore LICENSE and README 107 | * Move dependencies; use venv's default python 108 | * Contributors: Kaitlin Gallagher, Paul Bovbel 109 | -------------------------------------------------------------------------------- /aiorospy_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | project(aiorospy_examples) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | catkin_virtualenv 6 | ) 7 | 8 | catkin_package() 9 | 10 | catkin_generate_virtualenv( 11 | INPUT_REQUIREMENTS requirements.in 12 | PYTHON_INTERPRETER python3 13 | ) 14 | 15 | install(FILES requirements.txt 16 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 17 | 18 | set(python_scripts 19 | scripts/actions 20 | scripts/aiorospy_telephone 21 | scripts/services 22 | scripts/simple_actions 23 | scripts/topics 24 | ) 25 | 26 | catkin_install_python( 27 | PROGRAMS ${python_scripts} 28 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 29 | ) 30 | 31 | if(CATKIN_ENABLE_TESTING) 32 | find_package(catkin REQUIRED COMPONENTS roslint) 33 | roslint_python(${python_scripts}) 34 | roslint_add_test() 35 | endif() 36 | -------------------------------------------------------------------------------- /aiorospy_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aiorospy_examples 4 | 0.10.0 5 | Example scripts using aiorospy. 6 | 7 | Andrew Blakey 8 | Paul Bovbel 9 | 10 | Andrew Blakey 11 | Paul Bovbel 12 | MIT 13 | 14 | catkin 15 | 16 | catkin_virtualenv 17 | 18 | python3 19 | 20 | aiorospy 21 | 22 | rospy_tutorials 23 | std_msgs 24 | std_srvs 25 | 26 | roslint 27 | 28 | 29 | requirements.txt 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /aiorospy_examples/requirements.in: -------------------------------------------------------------------------------- 1 | aiohttp 2 | -------------------------------------------------------------------------------- /aiorospy_examples/requirements.txt: -------------------------------------------------------------------------------- 1 | aiohappyeyeballs==2.4.4 # via aiohttp 2 | aiohttp==3.10.11 # via -r requirements.in 3 | aiosignal==1.3.1 # via aiohttp 4 | async-timeout==5.0.1 # via aiohttp 5 | attrs==25.1.0 # via aiohttp 6 | frozenlist==1.5.0 # via aiohttp, aiosignal 7 | idna==3.10 # via yarl 8 | multidict==6.1.0 # via aiohttp, yarl 9 | propcache==0.2.0 # via aiohttp, yarl 10 | typing-extensions==4.12.2 # via multidict 11 | yarl==1.15.2 # via aiohttp 12 | -------------------------------------------------------------------------------- /aiorospy_examples/scripts/actions: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import random 4 | 5 | import aiorospy 6 | import rospy 7 | from actionlib.msg import TestAction, TestGoal, TestResult 8 | 9 | 10 | class ActionDemo: 11 | 12 | def __init__(self): 13 | self.server = aiorospy.AsyncActionServer('async_action', TestAction, self.handle_action) 14 | self.client = aiorospy.AsyncActionClient('async_action', TestAction) 15 | 16 | async def handle_action(self, goal_handle): 17 | goal_id = goal_handle.get_goal_id().id.split('-')[1] 18 | try: 19 | delay = goal_handle.get_goal().goal / 1000 20 | print(f"Server: Doing task {goal_id} for {delay}s") 21 | goal_handle.set_accepted() 22 | await asyncio.sleep(delay) 23 | goal_handle.set_succeeded() 24 | print(f"Server: Done task {goal_id}!") 25 | 26 | except asyncio.CancelledError: 27 | print(f"Server: Interrupted during task {goal_id}") 28 | goal_handle.set_canceled() 29 | raise 30 | 31 | async def goal_loop(self): 32 | while True: 33 | number = random.randint(1, 1000) 34 | print(f"Client: Asking server to work for {number/1000}s") 35 | goal_handle = await self.client.ensure_goal(TestGoal(goal=number), resend_timeout=1.0) 36 | try: 37 | await asyncio.wait_for(goal_handle.wait(), timeout=0.5) 38 | except asyncio.TimeoutError: 39 | print("Client: Cancelling after 0.5s...") 40 | goal_handle.cancel() 41 | await goal_handle.wait() 42 | print("---") 43 | 44 | 45 | if __name__ == '__main__': 46 | rospy.init_node('actions') 47 | 48 | loop = asyncio.get_event_loop() 49 | loop.set_debug(True) 50 | 51 | demo = ActionDemo() 52 | 53 | tasks = asyncio.gather( 54 | demo.server.start(), 55 | demo.client.start(), 56 | demo.goal_loop() 57 | ) 58 | 59 | aiorospy.cancel_on_exception(tasks) 60 | aiorospy.cancel_on_shutdown(tasks) 61 | 62 | try: 63 | loop.run_until_complete(tasks) 64 | except asyncio.CancelledError: 65 | pass 66 | -------------------------------------------------------------------------------- /aiorospy_examples/scripts/aiorospy_telephone: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import logging 4 | 5 | import aiorospy 6 | import rospy 7 | from actionlib.msg import TestAction, TestGoal, TestResult 8 | from rospy_tutorials.srv import (AddTwoInts, AddTwoIntsRequest, 9 | AddTwoIntsResponse) 10 | from std_msgs.msg import UInt64 11 | 12 | from aiohttp import ClientSession, web 13 | 14 | # Quiet down warnings a bit so we can see the result more cleanly. Comment out to see what kinds of warnings we get. 15 | logging.getLogger('aiohttp.access').setLevel(logging.WARNING) 16 | 17 | 18 | class AiorospyTelephone: 19 | ''' 20 | The Aiorospy Telephone is an exercise in single-thread concurrency to help illustrate the power of this model. 21 | During the lifecycle of a single request we have numerous pending tasks/coroutines for stateful I/O transactions. 22 | 23 | These include: 24 | - HTTP request 25 | - WebSocket connection 26 | - ROS Action 27 | - ROS Publisher/Subscriber 28 | - ROS Service call 29 | 30 | You'll notice that while there are underlying threads for some of the concurrent tasks, the entire application 31 | flow is handled in a single thread in the event loop with numerous places where deadlocking would occur without 32 | concurrency. Part of the beauty of this model is that you can read and predict the control flow in a linear manner. 33 | And all this is done without a single lock. 34 | 35 | Note that we jump between string and integer data to avoid having to create additional ROS message types for this 36 | example. 37 | 38 | Also note that we don't do much cleanup so a bunch of ROS stuff complains on shutdown. Typically one wouldn't have 39 | nodes connecting to themselves. 40 | ''' 41 | 42 | def __init__(self): 43 | self.publisher = rospy.Publisher('hello_pub', UInt64, queue_size=1) 44 | self.subscriber = aiorospy.AsyncSubscriber('hello_pub', UInt64) 45 | 46 | self.action_server = aiorospy.AsyncActionServer('hello_action', TestAction, self.handle_action) 47 | self.action_client = aiorospy.AsyncActionClient('hello_action', TestAction) 48 | 49 | self.service = aiorospy.AsyncService('hello_srv', AddTwoInts, self.handle_rosservice_call) 50 | self.service_proxy = aiorospy.AsyncServiceProxy('hello_srv', AddTwoInts) 51 | 52 | self.http_server = web.Application() 53 | self.http_server.add_routes([ 54 | web.get('/ws', self.handle_websocket_connection), 55 | web.get('/hello', self.handle_http_request) 56 | ]) 57 | 58 | self.count = 0 59 | 60 | async def start(self): 61 | runner = web.AppRunner(self.http_server) 62 | await runner.setup() 63 | 64 | site = web.TCPSite(runner) 65 | 66 | return await asyncio.gather( 67 | self.action_client.start(), 68 | self.action_server.start(), 69 | self.service.start(), 70 | site.start(), 71 | self.send_goal() 72 | ) 73 | 74 | async def send_goal(self): 75 | while True: 76 | print('Step 1: Create and dispatch an actionlib goal and await its completion.') 77 | goal_handle = await self.action_client.ensure_goal(TestGoal(self.count), resend_timeout=5.0) 78 | await goal_handle.wait() 79 | 80 | print('Step 10: print the actionlib goal result. Wow. That all sure was unnecessary.') 81 | print(f'The count is: {goal_handle.result.result}') 82 | 83 | await asyncio.sleep(2) 84 | self.count += 1 85 | 86 | async def handle_action(self, goal_handle): 87 | goal_handle.set_accepted() 88 | 89 | print('Step 2: Handle the actionlib goal by making a ROS service call and await its completion.') 90 | req = AddTwoIntsRequest(a=int(goal_handle.goal.goal.goal)) 91 | res = await self.service_proxy.ensure(req) 92 | 93 | print('Step 9: Set the actionlib goal as succeeded, passing the service call response content as a result.') 94 | goal_handle.set_succeeded(TestResult(res.sum)) 95 | 96 | async def handle_rosservice_call(self, req): 97 | async with ClientSession() as session: 98 | print('Step 3: Handle the ROS service call by making an HTTP request.') 99 | async with session.get(f'http://localhost:8080/hello?count={req.a}') as response: 100 | 101 | print('Step 8: Await the HTTP response then respond to the ROS service call with the contents.') 102 | count = await response.text() 103 | await session.close() 104 | return AddTwoIntsResponse(int(count)) 105 | 106 | async def handle_http_request(self, request): 107 | print('Step 4: Handle the HTTP request by establishing a WebSocket connection and sending a message.') 108 | value = request.rel_url.query['count'] 109 | async with ClientSession() as session: 110 | async with session.ws_connect('http://localhost:8080/ws') as ws: 111 | await ws.send_str(str(value)) 112 | 113 | print('Step 6: Wait for a ROS message to be published on the ROS topic.') 114 | async for msg in self.subscriber.subscribe(): 115 | break 116 | await session.close() 117 | 118 | print('Step 7: respond to the HTTP request with the contents of the ROS message.') 119 | return web.Response(text=str(msg.data)) 120 | 121 | async def handle_websocket_connection(self, request): 122 | print('Step 5: Handle the WebSocket connection and message by publishing the data on the ROS topic.') 123 | ws = web.WebSocketResponse() 124 | await ws.prepare(request) 125 | async for msg in ws: 126 | while self.publisher.get_num_connections() <= 0: 127 | await asyncio.sleep(0.1) 128 | self.publisher.publish(int(msg.data)) 129 | 130 | return ws 131 | 132 | 133 | if __name__ == '__main__': 134 | rospy.init_node('aiorospy_telephone', log_level=rospy.ERROR) 135 | 136 | loop = asyncio.get_event_loop() 137 | loop.set_debug(True) 138 | 139 | telephone = AiorospyTelephone() 140 | 141 | task = loop.create_task(telephone.start()) 142 | 143 | aiorospy.cancel_on_exception(task) 144 | aiorospy.cancel_on_shutdown(task) 145 | 146 | try: 147 | loop.run_until_complete(task) 148 | except asyncio.CancelledError: 149 | pass 150 | -------------------------------------------------------------------------------- /aiorospy_examples/scripts/services: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import random 4 | 5 | import aiorospy 6 | import rospy 7 | from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse 8 | 9 | 10 | async def handle_service(req): 11 | return SetBoolResponse(success=not req.data) 12 | 13 | 14 | async def request_loop(client): 15 | while True: 16 | data = random.choice((True, False)) 17 | res = await client.ensure(SetBoolRequest(data=data)) 18 | print(f'I sent {data}, I got {res.success}') 19 | await asyncio.sleep(1) 20 | 21 | 22 | if __name__ == '__main__': 23 | rospy.init_node('services') 24 | 25 | loop = asyncio.get_event_loop() 26 | loop.set_debug(True) 27 | 28 | server = aiorospy.AsyncService('service', SetBool, handle_service) 29 | client = aiorospy.AsyncServiceProxy(server.name, SetBool) 30 | 31 | tasks = asyncio.gather( 32 | server.start(), 33 | request_loop(client) 34 | ) 35 | 36 | aiorospy.cancel_on_exception(tasks) 37 | aiorospy.cancel_on_shutdown(tasks) 38 | 39 | try: 40 | loop.run_until_complete(tasks) 41 | except asyncio.CancelledError: 42 | pass 43 | -------------------------------------------------------------------------------- /aiorospy_examples/scripts/simple_actions: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | import random 4 | 5 | import aiorospy 6 | import rospy 7 | from actionlib.msg import TestAction, TestGoal, TestResult 8 | 9 | 10 | class SimpleActionDemo: 11 | 12 | def __init__(self): 13 | self.server = aiorospy.AsyncActionServer('async_simple_action', TestAction, self.handle_action, simple=True) 14 | self.client = aiorospy.AsyncActionClient('async_simple_action', TestAction) 15 | 16 | async def handle_action(self, goal_handle): 17 | goal_id = goal_handle.get_goal_id().id.split('-')[1] 18 | 19 | try: 20 | delay = goal_handle.get_goal().goal / 1000 21 | print(f"Server: Doing task {goal_id} for {delay}s") 22 | goal_handle.set_accepted() 23 | await asyncio.sleep(delay) 24 | goal_handle.set_succeeded() 25 | print(f"Server: Done task {goal_id}!") 26 | 27 | except asyncio.CancelledError: 28 | print(f"Server: Interrupted during task {goal_id}") 29 | goal_handle.set_canceled() 30 | raise 31 | 32 | async def goal_loop(self): 33 | while True: 34 | number = random.randint(1, 1000) 35 | print(f"Client: Asking server to work for {number/1000}s") 36 | goal_handle = await self.client.ensure_goal(TestGoal(goal=number), resend_timeout=1.0) 37 | try: 38 | await asyncio.wait_for(goal_handle.wait(), timeout=0.5) 39 | except asyncio.TimeoutError: 40 | print("Client: Giving up after 0.5s...") 41 | print("---") 42 | 43 | 44 | if __name__ == '__main__': 45 | rospy.init_node('simple_actions') 46 | 47 | loop = asyncio.get_event_loop() 48 | loop.set_debug(True) 49 | 50 | demo = SimpleActionDemo() 51 | 52 | tasks = asyncio.gather( 53 | demo.server.start(), 54 | demo.client.start(), 55 | demo.goal_loop() 56 | ) 57 | 58 | aiorospy.cancel_on_exception(tasks) 59 | aiorospy.cancel_on_shutdown(tasks) 60 | 61 | try: 62 | loop.run_until_complete(tasks) 63 | except asyncio.CancelledError: 64 | pass 65 | -------------------------------------------------------------------------------- /aiorospy_examples/scripts/topics: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import asyncio 3 | 4 | import aiorospy 5 | import rospy 6 | from std_msgs.msg import String 7 | 8 | 9 | async def talker(): 10 | pub = rospy.Publisher('chatter', String, queue_size=1) 11 | while True: 12 | hello_str = "hello world %s" % rospy.get_time() 13 | pub.publish(hello_str) 14 | await asyncio.sleep(1) 15 | 16 | 17 | async def listener(): 18 | sub = aiorospy.AsyncSubscriber('chatter', String) 19 | async for message in sub.subscribe(): 20 | print(f'I heard: {message.data}') 21 | 22 | 23 | if __name__ == '__main__': 24 | rospy.init_node('topics') 25 | 26 | loop = asyncio.get_event_loop() 27 | loop.set_debug(True) 28 | 29 | tasks = asyncio.gather( 30 | talker(), 31 | listener() 32 | ) 33 | 34 | aiorospy.cancel_on_exception(tasks) 35 | aiorospy.cancel_on_shutdown(tasks) 36 | 37 | try: 38 | loop.run_until_complete(tasks) 39 | except asyncio.CancelledError: 40 | pass 41 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [pep8] 2 | max-line-length = 120 3 | --------------------------------------------------------------------------------