diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py index 436f060f9..d7c636104 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py +++ b/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() diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py index 62d4cd368..0e338bf12 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py +++ b/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,6 +8,19 @@ 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.""" @@ -14,3 +28,9 @@ class AbstractBlackboardCapsule: 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() diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py index 98d459fe6..c44d479f1 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py +++ b/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""" diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py index caa4589c2..6578b7c50 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py @@ -10,7 +10,6 @@ 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 @@ -18,7 +17,7 @@ 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 diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py index 640ff0f3e..c529e74a6 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py +++ b/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 diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py index 6e9c4f1c7..e56d7bbcc 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py +++ b/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) diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py index 22b1e4dcf..20680dddb 100755 --- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py +++ b/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() diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 99b3c5afa..a5f790aec 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/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) diff --git a/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/filter.py b/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/filter.py index 4be05f09e..e58626f9e 100644 --- a/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/filter.py +++ b/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()