Async/await and events executor in animation server (#704)

Author: jaagut

bitbots_misc/bitbots_utils/bitbots_utils/utils.py

@@ -1,5 +1,6 @@
 import os
-from typing import Any
+from threading import Thread
+from typing import Any, Callable
 
 import rclpy
 import yaml
@@ -11,6 +12,7 @@
 from rcl_interfaces.srv import GetParameters, SetParameters
 from rclpy.node import Node
 from rclpy.parameter import PARAMETER_SEPARATOR_STRING, Parameter, parameter_value_to_python
+from rclpy.task import Future
 
 # Create a new @nobeartype decorator disabling type-checking.
 nobeartype = beartype(conf=BeartypeConf(strategy=BeartypeStrategy.O0))
@@ -202,3 +204,43 @@ def parse_parameter_dict(*, namespace: str, parameter_dict: dict) -> list[Parame
             parameter = Parameter(name=full_param_name, value=param_value)
             parameters.append(parameter.to_parameter_msg())
     return parameters
+
+
+async def async_wait_for(node: Node, rel_time: float):
+    """
+    ROS 2 does not provide an async sleep function, so we implement our own using a timer.
+    This function will wait for the specified relative time in seconds.
+
+    :param node: The ROS2 node to create the timer on.
+    :param rel_time: The relative time in seconds to wait.
+    :return: None
+    """
+    future = Future()
+    rel_time = max(rel_time, 0.0)
+
+    def done_waiting():
+        future.set_result(None)
+
+    timer = node.create_timer(rel_time, done_waiting, clock=node.get_clock())
+    await future
+    timer.cancel()
+    node.destroy_timer(timer)
+
+
+async def async_run_thread(func: Callable[[], Any]) -> None:
+    """
+    Allows the usage of blocking functions in an async context.
+
+    Spawns a thread to run the function and returns a Future that will be set when the function is done.
+    """
+    future = Future()
+
+    def thread_func():
+        try:
+            future.set_result(func())
+        except Exception as e:
+            future.set_exception(e)
+
+    thread = Thread(target=thread_func)
+    thread.start()
+    await future

bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py

@@ -7,11 +7,11 @@
 import numpy as np
 import rclpy
 from bitbots_utils.transforms import quat2fused
+from bitbots_utils.utils import async_wait_for
 from rclpy.action import ActionServer, GoalResponse
 from rclpy.action.server import ServerGoalHandle
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.duration import Duration
-from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
+from rclpy.executors import ExternalShutdownException
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import Imu, JointState
 from simpleeval import simple_eval
@@ -65,11 +65,9 @@ def __init__(self):
                 )
                 traceback.print_exc()
 
-        callback_group = ReentrantCallbackGroup()
-
         # Subscribers
-        self.create_subscription(JointState, "joint_states", self.update_current_pose, 1, callback_group=callback_group)
-        self.create_subscription(Imu, "imu/data", self.imu_callback, 1, callback_group=callback_group)
+        self.create_subscription(JointState, "joint_states", self.update_current_pose, 1)
+        self.create_subscription(Imu, "imu/data", self.imu_callback, 1)
 
         # Service clients
         self.hcm_animation_mode = self.create_client(SetBool, "play_animation_mode")
@@ -79,7 +77,7 @@ def __init__(self):
 
         # Action server for playing animations
         self._action_server = ActionServer(
-            self, PlayAnimation, "animation", self.execute_cb, callback_group=callback_group, goal_callback=self.goal_cb
+            self, PlayAnimation, "animation", self.execute_cb, goal_callback=self.goal_cb
         )
 
         # Service to temporarily add an animation to the cache
@@ -125,7 +123,7 @@ def goal_cb(self, request: PlayAnimation.Goal) -> GoalResponse:
         # Everything is fine we are good to go
         return GoalResponse.ACCEPT
 
-    def execute_cb(self, goal: ServerGoalHandle) -> PlayAnimation.Result:
+    async def execute_cb(self, goal: ServerGoalHandle) -> PlayAnimation.Result:
         """This is called, when the action should be executed."""
 
         # Convert goal id uuid to hashable tuple (custom UUID type)
@@ -165,16 +163,16 @@ def finish(successful: bool) -> PlayAnimation.Result:
 
             # Send request to make the HCM to go into animation play mode
             num_tries = 0
-            while rclpy.ok() and (not self.hcm_animation_mode.call(SetBool.Request(data=True)).success):  # type: ignore[attr-defined]
+            while rclpy.ok() and (not (await self.hcm_animation_mode.call_async(SetBool.Request(data=True))).success):  # type: ignore[attr-defined]
                 if num_tries >= 10:
                     self.get_logger().error("Failed to request HCM to go into animation play mode")
                     return finish(successful=False)
                 num_tries += 1
-                self.get_clock().sleep_for(Duration(seconds=0.1))
+                await async_wait_for(self, 0.1)
 
         # Make sure we have our current joint states
         while rclpy.ok() and self.current_joint_states is None:
-            self.get_clock().sleep_for(Duration(seconds=0.1))
+            await async_wait_for(self, 0.1)
 
         # Create splines
         animator = SplineAnimator(
@@ -263,7 +261,9 @@ def finish(successful: bool) -> PlayAnimation.Result:
                 if pose is None or (request.bounds and once and t > animator.get_keyframe_times()[request.end - 1]):
                     # Animation is finished, tell it to the hcm (except if it is from the hcm)
                     if not request.hcm:
-                        hcm_result: SetBool.Response = self.hcm_animation_mode.call(SetBool.Request(data=False))
+                        hcm_result: SetBool.Response = await self.hcm_animation_mode.call_async(
+                            SetBool.Request(data=False)
+                        )
                         if not hcm_result.success:
                             self.get_logger().error(f"Failed to finish animation on HCM. Reason: {hcm_result.message}")
 
@@ -283,7 +283,10 @@ def finish(successful: bool) -> PlayAnimation.Result:
 
                 once = True
 
-                self.get_clock().sleep_until(last_time + Duration(seconds=0.02))
+                execution_duration = (self.get_clock().now() - last_time).nanoseconds / 1e9
+
+                await async_wait_for(self, execution_duration + 0.02)  # Wait for the next iteration
+
             except (ExternalShutdownException, KeyboardInterrupt):
                 sys.exit(0)
         return finish(successful=False)
@@ -339,7 +342,7 @@ def add_animation(self, request: AddAnimation.Request, response: AddAnimation.Re
 def main(args=None):
     rclpy.init(args=args)
     node = AnimationNode()
-    ex = MultiThreadedExecutor(num_threads=10)
+    ex = EventsExecutor()
     ex.add_node(node)
     try:
         ex.spin()

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py

@@ -26,5 +26,5 @@ def perform(self, reevaluate=False):
         if not self.blackboard.visualization_active and not self.blackboard.simulation_active:
             req = SetBool.Request()
             req.data = False
-            self.blackboard.motor_switch_service.call(req)
+            self.blackboard.motor_switch_service.call_async(req)
         return self.pop()