diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py index 693a92ad0..127462850 100644 --- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py +++ b/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)) @@ -194,3 +196,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 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() 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()