Behavior optimizations (#692)

Author: jaagut

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
 
@@ -22,7 +22,7 @@ def __init__(self, node: Node):
         self.step_running = False
         self.node = node
 
-        self.tf_buffer = Buffer(node, Duration(seconds=30))
+        self.tf_buffer = Buffer(Duration(seconds=30), node)
 
         blackboard = BodyBlackboard(node, self.tf_buffer)
         self.dsd = DSD(blackboard, "debug/dsd/body_behavior", node)  # TODO: use config
@@ -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_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -22,7 +22,7 @@ def __init__(self) -> None:
         super().__init__("bitbots_path_planning")
 
         # We need to create a tf buffer
-        self.tf_buffer = Buffer(self, Duration(seconds=self.config.tf_buffer_duration))
+        self.tf_buffer = Buffer(Duration(seconds=self.config.tf_buffer_duration), self)
 
         self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer)
         self.controller = Controller(node=self, buffer=self.tf_buffer)

bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py

@@ -60,7 +60,7 @@ def __init__(self):
 
         self.set_state_defaults()
 
-        self.tf_buffer = Buffer(self.node)
+        self.tf_buffer = Buffer(node=self.node)
 
         self.run_spin_in_thread()
         self.try_to_establish_connection()

bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py

@@ -32,7 +32,7 @@ def __init__(self) -> None:
         """
         super().__init__("ball_filter")
         self.logger = self.get_logger()
-        self.tf_buffer = Buffer(self, Duration(seconds=2))
+        self.tf_buffer = Buffer(Duration(seconds=2), self)
         # Setup dynamic reconfigure config
         self.param_listener = parameters.ParamListener(self)
 

bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/filter.py

@@ -20,7 +20,7 @@ class RobotFilter(Node):
     def __init__(self):
         super().__init__("bitbots_robot_filter")
 
-        self.tf_buffer = Buffer(self, Duration(seconds=10.0))
+        self.tf_buffer = Buffer(Duration(seconds=10.0), self)
 
         self.robots: list[tuple[sv3dm.Robot, Time]] = list()
         self.team: dict[int, TeamData] = dict()