Better defence, goalie and supporter positions

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py

@@ -8,6 +8,7 @@
 from bitbots_blackboard.capsules.kick_capsule import KickCapsule
 from bitbots_blackboard.capsules.misc_capsule import MiscCapsule
 from bitbots_blackboard.capsules.pathfinding_capsule import PathfindingCapsule
+from bitbots_blackboard.capsules.positioning_capsule import PositioningCapsule
 from bitbots_blackboard.capsules.team_data_capsule import TeamDataCapsule
 from bitbots_blackboard.capsules.world_model_capsule import WorldModelCapsule
 
@@ -34,6 +35,7 @@ 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.positioning = PositioningCapsule(self.node, self)
 
         self.capsules = [
             self.misc,
@@ -44,6 +46,7 @@ def __init__(self, node: Node, tf_buffer: tf2.BufferInterface):
             self.costmap,
             self.pathfinding,
             self.team_data,
+            self.positioning,
         ]
 
     def clear_cache(self):

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -98,6 +98,9 @@ def received_gamestate(self) -> bool:
     def get_team_id(self) -> int:
         return self.team_id
 
+    def get_own_id(self) -> int:
+        return self.own_id
+
     def gamestate_callback(self, gamestate_msg: GameState) -> None:
         if self.gamestate.penalized and not gamestate_msg.penalized:
             self.unpenalized_time = self._node.get_clock().now().nanoseconds / 1e9

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -1,3 +1,4 @@
+import math
 from typing import Literal, Optional
 
 import numpy as np
@@ -183,6 +184,26 @@ def get_active_teammate_poses(self, count_goalies: bool = False) -> list[Pose]:
                 poses.append(data.robot_position.pose)
         return poses
 
+    def quaternion_to_yaw(self, q) -> float:
+        """Extract yaw (theta) from a quaternion."""
+        return math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+    def get_robot_poses(self) -> dict[int, list[float]]:
+        """Returns a mapping of jersey_number -> [x, y, theta] for all active robots."""
+        robot_poses = {}
+        data: TeamData
+        for data in self.team_data.values():
+            if self.is_valid(data):
+                pose = data.robot_position.pose
+                robot_poses[data.robot_id] = [
+                    pose.position.x,
+                    pose.position.y,
+                    self.quaternion_to_yaw(pose.orientation),
+                ]
+        # include own data
+        robot_poses[self._blackboard.gamestate.get_own_id()] = list(self._blackboard.world_model.get_current_position())
+        return robot_poses
+
     def get_number_of_active_field_players(self, count_goalie: bool = False) -> int:
         def is_not_goalie(team_data: TeamData) -> bool:
             return team_data.strategy.role != Strategy.ROLE_GOALIE

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_block_position.py

@@ -6,6 +6,26 @@
 from dynamic_stack_decider.abstract_action_element import AbstractActionElement
 from tf2_geometry_msgs import PoseStamped
 
+class GoToGoaliePosition(AbstractActionElement):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+
+    def perform(self, reevaluate=False):
+
+        pose_msg = PoseStamped()
+        pose_msg.header.stamp = self.blackboard.node.get_clock().now().to_msg()
+        pose_msg.header.frame_id = self.blackboard.map_frame
+
+        optimal_positioning = self.blackboard.positioning.get_formation_assignment()
+        own_position = optimal_positioning[self.blackboard.gamestate.get_own_id()]
+        pose = own_position["goal_pose"]
+        pose_msg.pose.position.x = pose[0]
+        pose_msg.pose.position.y = pose[1]
+        pose_msg.pose.orientation.w = pose[2]
+
+        self.blackboard.pathfinding.publish(pose_msg)
 
 class GoToBlockPosition(AbstractActionElement):
     blackboard: BodyBlackboard

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_defense_position.py

@@ -83,8 +83,11 @@ def perform(self, reevaluate=False):
             pose_msg.pose.orientation = Quaternion(x=x, y=y, z=z, w=w)
         else:
             # center point between ball and own goal
-            pose_msg.pose.position.x = (goal_position[0] + ball_position[0]) / 2
-            pose_msg.pose.position.y = ball_position[1] / 2 + self.y_offset
-            pose_msg.pose.orientation.w = 1.0
+            optimal_positioning = self.blackboard.positioning.get_formation_assignment()
+            own_position = optimal_positioning[self.blackboard.gamestate.get_own_id()]
+            pose = own_position["goal_pose"]
+            pose_msg.pose.position.x = pose[0]
+            pose_msg.pose.position.y = pose[1]
+            pose_msg.pose.orientation.w = pose[2]
 
         self.blackboard.pathfinding.publish(pose_msg)

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_pass_position.py

@@ -40,9 +40,14 @@ def perform(self, reevaluate=False):
         pose_msg = PoseStamped()
         pose_msg.header.stamp = self.blackboard.node.get_clock().now().to_msg()
         pose_msg.header.frame_id = self.blackboard.map_frame
-        pose_msg.pose.position.x = float(goal_x)
-        pose_msg.pose.position.y = float(goal_y)
-        pose_msg.pose.orientation = quat_from_yaw(goal_yaw)
+
+        optimal_positioning = self.blackboard.positioning.get_formation_assignment()
+        own_position = optimal_positioning[self.blackboard.gamestate.get_own_id()]
+        pose = own_position["goal_pose"]
+        pose_msg.pose.position.x = pose[0]
+        pose_msg.pose.position.y = pose[1]
+        pose_msg.pose.orientation.w = pose[2]
+
         self.blackboard.pathfinding.publish(pose_msg)
 
         if not self.blocking:

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/closest_to_ball.py

@@ -33,34 +33,42 @@ def perform(self, reevaluate=False):
         self.publish_debug_data("time to ball", my_time_to_ball)
         self.publish_debug_data("Rank to ball", rank)
         if rank == 1:
-            return "YES"
+            return "NO"  # only for testing remeber to REMOVE!
         return "NO"
 
     def get_reevaluate(self):
         return True
 
 
-class RankToBallNoGoalie(AbstractDecisionElement):
+class RankToBallWithGoalie(AbstractDecisionElement):
     blackboard: BodyBlackboard
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
 
     def perform(self, reevaluate=False):
-        my_time_to_ball = self.blackboard.team_data.get_own_time_to_ball()
-        rank = self.blackboard.team_data.team_rank_to_ball(my_time_to_ball, count_goalies=False, use_time_to_ball=True)
-        self.publish_debug_data("time to ball", my_time_to_ball)
-        self.publish_debug_data("Rank to ball", rank)
-        if rank == 1:
-            return "FIRST"
-        elif rank == 2:
-            return "SECOND"
-        elif rank == 3:
-            return "THIRD"
+        optimal_positioning = self.blackboard.positioning.get_formation_assignment()
+        own_position = optimal_positioning[self.blackboard.gamestate.get_own_id()]
+        role = own_position["role"]
+        self.publish_debug_data("Role from positioning", role)
+        if role == "striker":
+            return "STRIKER"
+        elif role == "goalie":
+            return "GOALIE"
+        elif role == "defender_0":
+            return "DEFENDER_ZERO"
+        elif role == "defender_1":
+            return "DEFENDER1"
+        elif role == "defender_2":
+            return "DEFENDER2"
+        elif role == "defender_3":
+            return "DEFENDER3"
+        elif role == "supporter":
+            return "SUPPORTER"
         else:
             # emergency fall back if something goes wrong
-            self.blackboard.node.get_logger().warning("Rank to ball had some issues")
-            return "FIRST"
+            self.blackboard.node.get_logger().warning("Rank to ball had some issues. Role" + role)
+            return "STRIKER"
 
     def get_reevaluate(self):
         return True

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -62,11 +62,6 @@ $GoalieHandlingBall
     YES --> @ChangeAction + action:positioning, @AvoidBallActive, @LookAtFieldFeatures, @GoToDefensePosition //quick fix for playing with two robots during GO
     NO --> #KickWithAvoidance
 
-#DefensePositioning
-$GoalieActive
-    YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
-    NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToBlockPosition
-
 #SupporterRole
 $PassStarted
     YES --> $BallSeen
@@ -94,10 +89,12 @@ $DoOnce
             NO --> $SecondaryStateTeamDecider
                 OUR --> #SearchBall
                 ELSE --> @AvoidBallActive, @LookAtFieldFeatures, @WalkInPlace + duration:2, @GoToRelativePosition + x:1 + y:0 + t:0, @Stand
-            YES --> $RankToBallNoGoalie
-                FIRST --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_first
-                SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second
-                THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
+            YES --> $RankToBallWithGoalie
+                STRIKER --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_first
+                DEFENDER0 --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second
+                SUPPORTER --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
+                GOALIE --> @GoToGoaliePosition
+                ELSE --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second
 
 #Init
 @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand
@@ -110,16 +107,12 @@ $BallSeen
     YES --> $KickOffTimeUp
         NO_NORMAL --> #StandAndLook
         NO_FREEKICK --> #Placing
-        YES --> $ConfigRole
-            GOALIE --> #GoalieBehavior
-            ELSE --> $CountActiveRobotsWithoutGoalie
-                ONE --> $RankToBallNoGoalie
-                    FIRST --> #StrikerRole
-                    SECOND --> #DefensePositioning
-                ELSE --> $RankToBallNoGoalie
-                    FIRST --> #StrikerRole
-                    SECOND --> #SupporterRole
-                    THIRD --> #DefensePositioning
+        YES --> $RankToBallWithGoalie
+            STRIKER --> #StrikerRole
+            GOALIE --> @GoToGoaliePosition
+            DEFENDER_ZERO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
+            SUPPORTER --> #SupporterRole
+            ELSE --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition //defender 1 bis 4 integrieren?
 
 #PlayingBehavior
 $SecondaryStateDecider

src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py

@@ -61,8 +61,8 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path:
     sensor_topics = [
         ("joint_states", "sensor_msgs/msg/JointState"),
         ("imu/data", "sensor_msgs/msg/Imu"),
-        ("camera/image_proc", "sensor_msgs/msg/Image"),
-        ("camera/camera_info", "sensor_msgs/msg/CameraInfo"),
+        ("zed/zed_node/rgb/image_rect_color", "sensor_msgs/msg/Image"),
+        ("zed/zed_node/rgb/camera_info", "sensor_msgs/msg/CameraInfo"),
     ]
 
     for topic_suffix, msg_type in sensor_topics: