Use the events executor for most suitable python nodes (#693)

Author: Flova

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py

@@ -34,3 +34,19 @@ def __init__(self, node: Node, tf_buffer: tf2.BufferInterface):
         self.costmap = CostmapCapsule(self.node, self)
         self.pathfinding = PathfindingCapsule(self.node, self)
         self.team_data = TeamDataCapsule(self.node, self)
+
+        self.capsules = [
+            self.misc,
+            self.gamestate,
+            self.animation,
+            self.kick,
+            self.world_model,
+            self.costmap,
+            self.pathfinding,
+            self.team_data,
+        ]
+
+    def clear_cache(self):
+        """Clear the cache of all capsules."""
+        for capsule in self.capsules:
+            capsule.clear_cache()

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py

@@ -1,3 +1,4 @@
+from functools import wraps
 from typing import TYPE_CHECKING
 
 from bitbots_utils.utils import nobeartype
@@ -7,10 +8,29 @@
     from bitbots_blackboard.body_blackboard import BodyBlackboard
 
 
+def cached_capsule_function(method):
+    """Decorator to cache the result of a method."""
+    cache_key = method.__name__
+
+    @wraps(method)
+    def wrapper(self):
+        if cache_key not in self._cache:
+            self._cache[cache_key] = method(self)
+        return self._cache[cache_key]
+
+    return wrapper
+
+
 class AbstractBlackboardCapsule:
     """Abstract class for blackboard capsules."""
 
     @nobeartype
     def __init__(self, node: Node, blackboard: "BodyBlackboard"):
         self._node = node
         self._blackboard = blackboard
+
+        self._cache: dict[str, object] = {}
+
+    def clear_cache(self):
+        """Clear the cache of this capsule."""
+        self._cache.clear()

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -8,7 +8,7 @@
 from ros2_numpy import numpify
 from std_msgs.msg import Float32
 
-from bitbots_blackboard.capsules import AbstractBlackboardCapsule
+from bitbots_blackboard.capsules import AbstractBlackboardCapsule, cached_capsule_function
 from bitbots_msgs.msg import Strategy, TeamData
 
 
@@ -68,16 +68,22 @@ def __init__(self, node, blackboard):
         self.data_timeout: float = float(self._node.get_parameter("team_data_timeout").value)
         self.ball_max_covariance: float = float(self._node.get_parameter("ball_max_covariance").value)
 
+    @cached_capsule_function
+    def time(self) -> Time:
+        """Returns the current time of the node, this is its own function so it can be cached during the decision loop."""
+        return self._node.get_clock().now()
+
     def is_valid(self, data: TeamData) -> bool:
         """
         Checks if a team data message from a given robot is valid.
         Meaning it is not too old and the robot is not penalized.
         """
         return (
-            self._node.get_clock().now() - Time.from_msg(data.header.stamp) < Duration(seconds=self.data_timeout)
+            self.time() - Time.from_msg(data.header.stamp) < Duration(seconds=self.data_timeout)
             and data.state != TeamData.STATE_PENALIZED
         )
 
+    @cached_capsule_function
     def is_goalie_handling_ball(self) -> bool:
         """Returns true if the goalie is going to the ball."""
         data: TeamData
@@ -90,6 +96,7 @@ def is_goalie_handling_ball(self) -> bool:
                 return True
         return False
 
+    @cached_capsule_function
     def is_team_mate_kicking(self) -> bool:
         """Returns true if one of the players in the own team is kicking."""
         data: TeamData
@@ -190,6 +197,7 @@ def is_not_goalie(team_data: TeamData) -> bool:
         # Count valid team data infos (aka robots with valid team data)
         return sum(map(self.is_valid, team_data_infos))
 
+    @cached_capsule_function
     def get_is_goalie_active(self) -> bool:
         def is_a_goalie(team_data: TeamData) -> bool:
             return team_data.strategy.role == Strategy.ROLE_GOALIE
@@ -221,6 +229,7 @@ def teammate_ball_is_valid(self) -> bool:
         """Returns true if a teammate has seen the ball accurately enough"""
         return self.get_teammate_ball() is not None
 
+    @cached_capsule_function
     def get_teammate_ball(self) -> Optional[PointStamped]:
         """Returns the best ball from all teammates that satisfies a minimum ball precision"""
 

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -10,15 +10,14 @@
     TransformStamped,
 )
 from rclpy.clock import ClockType
-from rclpy.duration import Duration
 from rclpy.time import Time
 from ros2_numpy import msgify, numpify
 from std_msgs.msg import Header
 from std_srvs.srv import Trigger
 from tf2_geometry_msgs import Point, PointStamped
 from tf_transformations import euler_from_quaternion
 
-from bitbots_blackboard.capsules import AbstractBlackboardCapsule
+from bitbots_blackboard.capsules import AbstractBlackboardCapsule, cached_capsule_function
 
 
 class WorldModelTFError(Exception): ...
@@ -27,9 +26,6 @@ class WorldModelTFError(Exception): ...
 class WorldModelPositionTFError(WorldModelTFError): ...
 
 
-class WorldModelBallTFError(WorldModelTFError): ...
-
-
 class WorldModelCapsule(AbstractBlackboardCapsule):
     """Provides information about the world model."""
 
@@ -78,10 +74,12 @@ def __init__(self, node, blackboard):
     ### Ball ###
     ############
 
+    @cached_capsule_function
     def ball_seen_self(self) -> bool:
         """Returns true we are reasonably sure that we have seen the ball"""
         return all(np.diag(self._ball_covariance) < self.ball_max_covariance)
 
+    @cached_capsule_function
     def ball_has_been_seen(self) -> bool:
         """Returns true if we or a teammate are reasonably sure that we have seen the ball"""
         return self.ball_seen_self() or self._blackboard.team_data.teammate_ball_is_valid()
@@ -91,6 +89,7 @@ def get_ball_position_xy(self) -> tuple[float, float]:
         ball = self.get_best_ball_point_stamped()
         return ball.point.x, ball.point.y
 
+    @cached_capsule_function
     def get_best_ball_point_stamped(self) -> PointStamped:
         """
         Returns the best ball, either its own ball has been in the ball_lost_lost time
@@ -113,21 +112,25 @@ def get_best_ball_point_stamped(self) -> PointStamped:
         self.debug_publisher_which_ball.publish(Header(stamp=own_ball.header.stamp, frame_id="own_ball_map"))
         return own_ball
 
+    @cached_capsule_function
     def get_ball_position_uv(self) -> tuple[float, float]:
         """
         Returns the ball position relative to the robot in the base_footprint frame.
         U and V are returned, where positive U is forward, positive V is to the left.
         """
         ball = self.get_best_ball_point_stamped()
-        try:
-            ball_bfp = self._blackboard.tf_buffer.transform(
-                ball, self.base_footprint_frame, timeout=Duration(seconds=0.2)
-            ).point
-        except tf2.ExtrapolationException as e:
-            self._node.get_logger().warn(str(e))
-            self._node.get_logger().error("Severe transformation problem concerning the ball!")
-            raise WorldModelBallTFError("Could not transform ball to base_footprint frame") from e
-        return ball_bfp.x, ball_bfp.y
+        assert ball.header.frame_id == self.map_frame, "Ball needs to be in the map frame"
+        our_pose = self.get_current_position_transform()
+        assert our_pose.header.frame_id == self.map_frame, "Our pose needs to be in the map frame"
+        # Construct the homogeneous transformation matrix for the ball position
+        ball_transform = np.eye(4)
+        ball_transform[0, 3] = ball.point.x
+        ball_transform[1, 3] = ball.point.y
+        # Get the homogeneous transformation matrix for the robot's current position
+        our_pose_transform = numpify(our_pose.transform)
+        # Calculate the relative position of the ball to the robot
+        relative_transform = np.linalg.inv(our_pose_transform) @ ball_transform
+        return relative_transform[0, 3], relative_transform[1, 3]
 
     def get_ball_distance(self) -> float:
         """
@@ -171,9 +174,7 @@ def forget_ball(self) -> None:
         """
         Forget that we saw a ball
         """
-        result: Trigger.Response = self.reset_ball_filter.call(Trigger.Request())
-        if not result.success:
-            self._node.get_logger().error(f"Ball filter reset failed with: '{result.message}'")
+        self.reset_ball_filter.call_async(Trigger.Request())
 
     ########
     # Goal #
@@ -218,6 +219,7 @@ def get_map_based_opp_goal_right_post_uv(self) -> tuple[float, float]:
     # Pose #
     ########
 
+    @cached_capsule_function
     def get_current_position(self) -> tuple[float, float, float]:
         """
         Returns the current position on the field as determined by the localization.
@@ -228,6 +230,7 @@ def get_current_position(self) -> tuple[float, float, float]:
         theta = euler_from_quaternion(numpify(transform.transform.rotation))[2]
         return transform.transform.translation.x, transform.transform.translation.y, theta
 
+    @cached_capsule_function
     def get_current_position_pose_stamped(self) -> PoseStamped:
         """
         Returns the current position as determined by the localization as a PoseStamped
@@ -241,6 +244,7 @@ def get_current_position_pose_stamped(self) -> PoseStamped:
             ),
         )
 
+    @cached_capsule_function
     def get_current_position_transform(self) -> TransformStamped:
         """
         Returns the current position as determined by the localization as a TransformStamped

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -8,7 +8,7 @@
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from soccer_vision_3d_msgs.msg import RobotArray
 
@@ -85,6 +85,7 @@ def loop(self):
             self.counter = (self.counter + 1) % blackboard.config["time_to_ball_divider"]
             if self.counter == 0:
                 blackboard.pathfinding.calculate_time_to_ball()
+            blackboard.clear_cache()
         except Exception as e:
             import traceback
 
@@ -97,12 +98,11 @@ def main(args=None):
     node = Node("body_behavior", automatically_declare_parameters_from_overrides=True)
     body_dsd = BodyDSD(node)
     node.create_timer(1 / 60.0, body_dsd.loop, callback_group=MutuallyExclusiveCallbackGroup(), clock=node.get_clock())
-    # Number of executor threads is the number of MutuallyExclusiveCallbackGroups + 2 threads needed by the tf listener and executor
-    multi_executor = MultiThreadedExecutor(num_threads=12)
-    multi_executor.add_node(node)
 
+    executor = EventsExecutor()
+    executor.add_node(node)
     try:
-        multi_executor.spin()
+        executor.spin()
     except KeyboardInterrupt:
         pass
 

bitbots_misc/bitbots_teleop/bitbots_teleop/joy_node.py

@@ -5,6 +5,7 @@
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.duration import Duration
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import Joy
 
@@ -240,6 +241,12 @@ def joy_cb(self, msg: Joy):
 def main():
     rclpy.init(args=None)
     node = JoyNode()
-    rclpy.spin(node)
+
+    executor = EventsExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+
     node.destroy_node()
-    rclpy.shutdown()

bitbots_misc/bitbots_tts/bitbots_tts/tts.py

@@ -10,7 +10,7 @@
 from ament_index_python import get_package_prefix
 from rcl_interfaces.msg import Parameter, SetParametersResult
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from rclpy.publisher import Publisher
 
@@ -121,7 +121,7 @@ def speak_cb(self, msg: Audio) -> None:
 def main(args=None):
     rclpy.init(args=args)
     node = Speaker()
-    executor = MultiThreadedExecutor(num_threads=2)
+    executor = EventsExecutor()
     executor.add_node(node)
     try:
         executor.spin()

bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py

@@ -12,7 +12,7 @@
 from dynamic_stack_decider.dsd import DSD
 from rcl_interfaces.msg import Parameter as ParameterMsg
 from rclpy.duration import Duration
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.serialization import deserialize_message
@@ -54,9 +54,9 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
         )
 
         # Create own executor for Python part
-        multi_executor = MultiThreadedExecutor(num_threads=10)
-        multi_executor.add_node(self.node)
-        self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True)
+        executor = EventsExecutor()
+        executor.add_node(self.node)
+        self.spin_thread = threading.Thread(target=executor.spin, args=(), daemon=True)
         self.spin_thread.start()
 
         # Otherwise messages will get lost, bc the init is not finished

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_handler.py

@@ -6,7 +6,7 @@
 from game_controller_hl_interfaces.msg import GameState
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import Imu
 
@@ -65,12 +65,12 @@ def main(args=None):
     node.create_timer(1 / 25.0, dsd.update, callback_group=MutuallyExclusiveCallbackGroup())
 
     # Create executor
-    multi_executor = MultiThreadedExecutor(4)
-    multi_executor.add_node(node)
+    executor = EventsExecutor()
+    executor.add_node(node)
 
     # Spin the executor
     try:
-        multi_executor.spin()
+        executor.spin()
     except KeyboardInterrupt:
         pass
 

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -4,6 +4,7 @@
 from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist
 from nav_msgs.msg import Path
 from rclpy.duration import Duration
+from rclpy.experimental.events_executor import EventsExecutor
 from std_msgs.msg import Bool, Empty
 from visualization_msgs.msg import MarkerArray
 
@@ -74,7 +75,11 @@ def main(args=None):
     rclpy.init(args=args)
     node = PathPlanning()
 
-    rclpy.spin(node)
+    executor = EventsExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
 
     node.destroy_node()
-    rclpy.shutdown()