add avoiding second kick logic in main dsd and add dribble

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -5,6 +5,7 @@
 from std_msgs.msg import Bool
 
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
+from bitbots_blackboard.body_blackboard import BodyBlackboard
 from bitbots_msgs.msg import TTS, HeadMode, RobotControlState
 
 THeadMode: TypeAlias = Literal[  # type: ignore[valid-type]
@@ -19,6 +20,7 @@
 
 class MiscCapsule(AbstractBlackboardCapsule):
     """Capsule for miscellaneous functions."""
+    blackboard: BodyBlackboard
 
     def __init__(self, node, blackboard):
         super().__init__(node, blackboard)
@@ -46,6 +48,10 @@ def __init__(self, node, blackboard):
 
         self.hcm_deactivate_pub = self._node.create_publisher(Bool, "hcm_deactivate", 1)
 
+        self.ball_movement_detection_start_ball_position = None
+
+        self.kickoff_or_throwin_kick = False
+
     #####################
     # ## Tracking Part ##
     #####################

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

@@ -32,6 +32,7 @@ def __init__(self, node, blackboard):
             self.team_data[i] = TeamData()
         self.times_to_ball = dict()
         self.own_time_to_ball = 9999.0
+        self.last_time_team_mate_kicked = None
 
         # Mapping
         self.roles_mapping = {

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

@@ -22,7 +22,7 @@ def __init__(self, blackboard, dsd, parameters):
         self.blocking = parameters.get("blocking", True)
         self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"])
         # Offset so we kick the ball with one foot instead of the center between the feet
-        self.side_offset = parameters.get("side_offset", 0.08)
+        self.side_offset = parameters.get("side_offset", 0.0)
 
     def perform(self, reevaluate=False):
         pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset)

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

@@ -0,0 +1,17 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+
+
+class StoreBallMovementDetectionStartPosition(AbstractActionElement):
+    """
+    Stores the starting position for ball movement detection.
+    """
+
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters: dict):
+        super().__init__(blackboard, dsd, parameters)
+
+    def perform(self, reevaluate=False):
+        self.blackboard.misc.ball_movement_detection_start_ball_position = self.blackboard.world_model.get_ball_position_xy()
+        self.pop()

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

@@ -0,0 +1,38 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+
+
+class SecondBallTouchAllowed(AbstractDecisionElement):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.after_kick_min_ball_movement = self.blackboard.config["after_kick_min_ball_movement"]
+        self.other_took_the_first_kick = False
+
+    def perform(self, reevaluate=False):
+        """
+        Determines whether we are confident regarding the ball's position.
+        :param reevaluate:
+        :return:
+        """
+        # TODO check if less than three robots are in play
+        if self.other_took_the_first_kick:
+            return "YES"
+        elif self. blackboard.misc.kickoff_or_throwin_kick == None or not self.blackboard.misc.kickoff_or_throwin_kick:
+            return "YES"
+        elif not self.blackboard.gamestate.has_kick():
+            return "YES"
+        elif self.blackboard.gamestate.is_teammate_kicking():
+            self.other_took_the_first_kick = True
+            return "YES"
+        else:
+            ball_pos = self.blackboard.world_model.get_ball_position_xy()
+            if self.blackboard.misc.ball_movement_detection_start_ball_position is not None and (abs(ball_pos[0] - self.blackboard.misc.ball_movement_detection_start_ball_position[0]) > self.after_kick_min_ball_movement or abs(
+                ball_pos[1] - self.blackboard.misc.ball_movement_detection_start_ball_position[1]) > self.after_kick_min_ball_movement):
+                return "NO"
+            else:
+                return "YES"
+
+    def get_reevaluate(self):
+        return True

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

@@ -40,9 +40,9 @@ $DoOnce
 $AvoidBall
     NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle
         YES --> $BallKickArea
-            NEAR --> $FootSelection
-                LEFT --> #PerformKickLeft
-                RIGHT --> #PerformKickRight
+            NEAR --> $DoOnce
+                NOT_DONE --> @StoreBallMovementDetectionStartPosition
+                DONE --> #Dribble
             FAR --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map
         NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist
     YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh
@@ -111,26 +111,28 @@ $DoOnce
 @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand
 
 #NormalBehavior
-$BallSeen
-    NO --> $ConfigRole
-        GOALIE --> #RolePositionWithPause
-        ELSE --> #SearchBall
-    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
+$SecondBallTouchAllowed
+    NO --> @SetNoKickOffOrThowInVariable, @LookAtFieldFeatures, @ChangeAction + action:passive, @AvoidBallActive, @GoToPassPreparePosition // TODO: Implement SetNoKickOffOrThowInVariable
+    YES --> $BallSeen
+        NO --> $ConfigRole
+            GOALIE --> #RolePositionWithPause
+            ELSE --> #SearchBall
+        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
 
 #PlayingBehavior
-$SecondaryStateDecider
+$SecondaryStateDecider  // TODO add Decision for Throw in and change Variable if in Throw in State and rewrite this? TO fit better with the new game controller, 
     PENALTYSHOOT --> #PenaltyShootoutBehavior
     TIMEOUT --> #StandAndLook
     ELSE --> $SecondaryStateTeamDecider
@@ -147,23 +149,26 @@ $IsPenalized
         ELSE --> #GetWalkreadyAndLocalize
     NO --> $GameStateDecider
         INITIAL --> #Init
-        READY --> $AnyGoalScoreRecently + time:50
-            YES --> #PositioningReady
-            NO --> $DoOnce
-                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
-                DONE --> #PositioningReady
-        SET --> $WhistleDetected
-            DETECTED --> $SecondaryStateTeamDecider
-                OUR --> #PlayingBehavior
-                OTHER --> #PositioningReady
-                ELSE --> #PlayingBehavior
-            NOT_DETECTED --> $SecondaryStateDecider
-                PENALTYSHOOT --> $SecondaryStateTeamDecider
-                    OUR --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @Stand // we need to also see the goalie
-                    OTHER --> $BallSeen
-                        YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball
-                        NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
-                ELSE --> #StandAndLook
+        READY --> $DoOnce
+            NOT_DONE --> @SetNoKickOffOrThowInVariable // TODO:Implement SetNoKickOffOrThowInVariable
+    YES --> $BallSeen
+            DONE --> $AnyGoalScoreRecently + time:50
+                YES --> #PositioningReady
+                NO --> $DoOnce
+                    NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
+                    DONE --> #PositioningReady
+            SET --> $WhistleDetected
+                DETECTED --> $SecondaryStateTeamDecider
+                    OUR --> #PlayingBehavior
+                    OTHER --> #PositioningReady
+                    ELSE --> #PlayingBehavior
+                NOT_DETECTED --> $SecondaryStateDecider
+                    PENALTYSHOOT --> $SecondaryStateTeamDecider
+                        OUR --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @Stand // we need to also see the goalie
+                        OTHER --> $BallSeen
+                            YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball
+                            NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
+                    ELSE --> #StandAndLook
         FINISHED --> $CurrentScore
             AHEAD --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand
             ELSE --> #Init

src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -61,10 +61,10 @@ body_behavior:
     # An area in which the ball can be kicked
     # defined by min/max x/y values in meters which represent ball positions relative to base_footprint
     # http://www.ros.org/reps/rep-0103.html#axis-orientation
-    kick_x_enter: 0.23
-    kick_x_leave: 0.25
-    kick_y_enter: 0.12
-    kick_y_leave: 0.12
+    kick_x_enter: 0.4
+    kick_x_leave: 0.45
+    kick_y_enter: 0.15
+    kick_y_leave: 0.15
 
     # defines the radius around the goal (in form of a box)
     # in this area, the goalie will react to the ball.
@@ -128,10 +128,13 @@ body_behavior:
     # distance from center point, that the ball must be during an opponent kickoff to think that it moved
     kickoff_min_ball_movement: 0.5
 
+    # distance from other ball position, that the ball must be traveled after a kick to think that it moved
+    after_kick_min_ball_movement: 0.5
+
     # dribble action
-    dribble_max_speed_x: 0.05
-    dribble_max_speed_y: 0.05
-    dribble_p: 0.8
+    dribble_max_speed_x: 0.5
+    dribble_max_speed_y: 0.4
+    dribble_p: 3.0
 
     # dribble decision
     dribble_orient_threshold: 0.5