running ros on asyncio loop without active polling

[ksuszka]

There is a small issue with the following code:

async def ros_loop():
    while rclpy.ok():
        rclpy.spin_once(node, timeout_sec=0)
        await asyncio.sleep(1e-4)

it uses some measurable CPU time and introduces additional latency. You need to choose sleep period which balances CPU/latency.

Here is an attempt to solve this issue by introducing an extension to the ROS executor which makes the executor to wait for a ROS event on a separate thread but delegates handling of the event to asyncio loop. Code is based on rclpy executors code (https://github.com/ros2/rclpy/blob/87fbec0d6bbfda968d14689c977e9a6bdaa48886/rclpy/rclpy/__init__.py#L318, https://github.com/ros2/rclpy/blob/87fbec0d6bbfda968d14689c977e9a6bdaa48886/rclpy/rclpy/executors.py#L902):

import asyncio
from typing import Optional

import rclpy
from rclpy.context import Context
from rclpy.executors import ConditionReachedException
from rclpy.executors import ShutdownException
from rclpy.executors import SingleThreadedExecutor
from rclpy.executors import TimeoutException
from rclpy.node import Node

from std_msgs.msg import String

class AsyncioLoopExecutor(SingleThreadedExecutor):

    def __init__(self, *, context: Optional[Context] = None) -> None:
        super().__init__(context=context)

    async def spin_once_async(self) -> None:
        loop = asyncio.get_event_loop()
        def handle(handler):
            handler()
            if handler.exception() is not None:
                raise handler.exception()
            handler.result()  # raise any exceptions

        def spin_once_thread(loop):
            try:
                handler, entity, node = self.wait_for_ready_callbacks()
            except ShutdownException:
                pass
            except TimeoutException:
                pass
            except ConditionReachedException:
                pass
            else:
                loop.call_soon_threadsafe(handle, handler)
        await asyncio.to_thread(spin_once_thread, loop)

class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello World: %d' % self.i
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1

async def ros_loop(node):
    executor = AsyncioLoopExecutor()
    try:
        executor.add_node(node)
        while executor.context.ok():
            await executor.spin_once_async()
    finally:
        executor.remove_node(node)

async def asyncio_sample_work_loop():
    while True:
        print("asyncio work")
        await asyncio.sleep(1)

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    try:
        future = asyncio.wait([ros_loop(minimal_publisher), asyncio_sample_work_loop()], return_when=asyncio.FIRST_EXCEPTION)
        done, _pending = asyncio.get_event_loop().run_until_complete(future)
        for task in done:
            task.result()  # raises exceptions if any        
    except KeyboardInterrupt:
        pass
    finally:
        # Destroy the node explicitly
        # (optional - otherwise it will be done automatically
        # when the garbage collector destroys the node object)
        minimal_publisher.destroy_node()
        rclpy.try_shutdown()    


if __name__ == '__main__':
    main()

[rickwalder]

This is an awesome contrib (and honestly needed in ros), but i have one question - does this line
await asyncio.to_thread(spin_once_thread, loop)
mean that cb's can execute out of order? eg

1. this yields
2. the ros loop runs again, and actually gets something to handle and handles it
3. this gets something to handle, and handles it out of order

It boils down to: if multiple things wait_for_ready_callbacks, are they guaranteed to be woken up in the same order that they called in?

[ksuszka]

In the current implementation there is only a single call to wait_for_ready_callbacks at a given time. The loop in ros_loop ensures that each call to executor.spin_once_async() is made after the previous one has ended. So at a given time there is only a single parallel thread taking from the ROS queue work to execute, which is started at the beginning of the asyncio.to_thread call and which is "finished" by the time the await keyword in ros_loop method allows further excution.