From dbc229ad7fbdfbc7acc902c8f0a11a1e2a87e361 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 17 Jun 2025 20:19:17 +0200 Subject: [PATCH 1/6] Add async utilities --- .../bitbots_utils/bitbots_utils/utils.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py index 693a92ad0..efd8a8ed9 100644 --- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py @@ -1,4 +1,5 @@ import os +from threading import Thread from typing import Any import rclpy @@ -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)) @@ -194,3 +196,36 @@ 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): + """ + ROS2 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) -> 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() + thread = Thread(target=lambda: future.set_result(func())) + thread.start() + await future From da1de8c8ac966adabb75da2c6949b6c74f8a5397 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 17 Jun 2025 20:20:18 +0200 Subject: [PATCH 2/6] Use events executor, which is single threaded. We therefore need to use async/await to avoid deadlocks. --- .../animation_node.py | 33 ++++++++++--------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py index d9c473ae7..01bc19f0b 100755 --- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py +++ b/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() From 5613b558a20a51f98eb7c9bc364ab4d1d2eea562 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 17 Jun 2025 20:20:47 +0200 Subject: [PATCH 3/6] Make service call that does not use the result async --- .../bitbots_hcm/hcm_dsd/actions/change_motor_power.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py index cdbbe7bab..a8b331600 100644 --- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py +++ b/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() From 55e591df3383a8b82238d28ee9d50bbd2a987e77 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 18 Jun 2025 17:02:04 +0200 Subject: [PATCH 4/6] Fix async exception handling --- bitbots_misc/bitbots_utils/bitbots_utils/utils.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py index efd8a8ed9..9766f509c 100644 --- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py @@ -226,6 +226,13 @@ async def async_run_thread(func: callable) -> None: Spawns a thread to run the function and returns a Future that will be set when the function is done. """ future = Future() - thread = Thread(target=lambda: future.set_result(func())) + + 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 From 66b99394effd1e283a6383be0ef2c5c72733a57f Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 19 Jun 2025 13:24:13 +0200 Subject: [PATCH 5/6] Fix type hint --- bitbots_misc/bitbots_utils/bitbots_utils/utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py index 9766f509c..fa780b7ba 100644 --- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py @@ -1,6 +1,6 @@ import os from threading import Thread -from typing import Any +from typing import Any, Callable import rclpy import yaml @@ -219,7 +219,7 @@ def done_waiting(): node.destroy_timer(timer) -async def async_run_thread(func: callable) -> None: +async def async_run_thread(func: Callable[[], Any]) -> None: """ Allows the usage of blocking functions in an async context. From b8cbb34c98a510727cd7b48af9ee6c61e488507e Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 24 Jun 2025 11:46:12 +0200 Subject: [PATCH 6/6] Update bitbots_misc/bitbots_utils/bitbots_utils/utils.py Co-authored-by: Jan Gutsche <34797331+jaagut@users.noreply.github.com> --- bitbots_misc/bitbots_utils/bitbots_utils/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py index fa780b7ba..127462850 100644 --- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py @@ -200,7 +200,7 @@ def parse_parameter_dict(*, namespace: str, parameter_dict: dict) -> list[Parame async def async_wait_for(node: Node, rel_time: float): """ - ROS2 does not provide an async sleep function, so we implement our own using a timer. + 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.