add avoiding second kick logic in main dsd and add dribble

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

@@ -46,6 +46,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: Optional[tuple[float, float]] = None
+
+        self.kickoff_or_throwin_kick: bool = 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 = {
@@ -55,6 +56,7 @@ def __init__(self, node, blackboard):
             Strategy.ACTION_SEARCHING,
             Strategy.ACTION_KICKING,
             Strategy.ACTION_LOCALIZING,
+            Strategy.ACTION_PASSIVE,
         }
 
         # The strategy which is communicated to the other robots
@@ -125,6 +127,7 @@ def team_rank_to_ball(
             if (
                 self.is_valid(data)
                 and (data.strategy.role != Strategy.ROLE_GOALIE or count_goalies)
+                and data.strategy.action != Strategy.ACTION_PASSIVE
                 and data.ball_absolute.covariance[0] < self.ball_max_covariance
                 and data.ball_absolute.covariance[7] < self.ball_max_covariance
             ):

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

@@ -27,6 +27,7 @@ def __init__(self, blackboard, dsd, parameters):
             "localizing": Strategy.ACTION_LOCALIZING,
             "trying_to_score": Strategy.ACTION_TRYING_TO_SCORE,
             "waiting": Strategy.ACTION_WAITING,
+            "passive": Strategy.ACTION_PASSIVE,
         }
 
     def perform(self, reevaluate=False):

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/set_no_kick_off_or_throw_in_variable.py

@@ -0,0 +1,20 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+
+
+class SetNoKickOffOrThrowInVariable(AbstractActionElement):
+    """
+    Sets the no_kick_off_or_throw_in variable.
+    """
+
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.do_not_reevaluate()
+        self.blackboard = blackboard
+
+        self.blackboard.misc.kickoff_or_throwin_kick = parameters.get("value", None)
+
+    def perform(self, reevaluate=False):
+        self.pop()

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

@@ -0,0 +1,34 @@
+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()
+
+
+class ForgetBallStartPosition(AbstractActionElement):
+    """
+    Forgets 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 = None
+        self.pop()

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

@@ -0,0 +1,63 @@
+import math
+
+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
+        self.latch = False
+
+    def perform(self, reevaluate=False):
+        """
+        Determines whether we are confident regarding the ball's position.
+        :param reevaluate:
+        :return:
+        """
+        ball_pos = self.blackboard.world_model.get_ball_position_xy()
+        kick_off_or_throwin_kick = (
+            self.blackboard.misc.kickoff_or_throwin_kick is None or self.blackboard.misc.kickoff_or_throwin_kick
+        )
+        active_team_players = self.blackboard.team_data.get_number_of_active_field_players(count_goalie=True)
+        is_kicking_team = self.blackboard.gamestate.has_kick()
+        self.other_took_the_first_kick = (
+            self.other_took_the_first_kick or self.blackboard.team_data.is_team_mate_kicking()
+        )
+        ball_start_position = self.blackboard.misc.ball_movement_detection_start_ball_position
+
+        self.publish_debug_data("other kicked", self.other_took_the_first_kick)
+        self.publish_debug_data("kick off or throw in kick", kick_off_or_throwin_kick)
+        self.publish_debug_data("active team players", active_team_players)
+        self.publish_debug_data("is kicking team", is_kicking_team)
+        self.publish_debug_data("ball position", ball_pos)
+        self.publish_debug_data("ball start position", ball_start_position)
+
+        if active_team_players < 2:
+            return "YES"
+        elif self.other_took_the_first_kick:
+            self.latch = False
+            return "YES"
+        elif self.latch:
+            return "NO"
+        elif not kick_off_or_throwin_kick:
+            return "YES"
+        elif not is_kicking_team:
+            return "YES"
+        else:
+            if (
+                ball_start_position is not None
+                and math.hypot(ball_pos[0] - ball_start_position[0], ball_pos[1] - ball_start_position[1])
+                > self.after_kick_min_ball_movement
+            ):
+                self.latch = True
+                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,33 +111,41 @@ $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 --> @SetNoKickOffOrThrowInVariable + value:false + r:false, @LookAtFieldFeatures + r:false, @ChangeAction + action:passive + r:false, @AvoidBallActive + r:false, @GoToPassPreparePosition + blocking:true // 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
+    THROWIN --> $DoOnce
+        NOT_DONE --> @SetNoKickOffOrThrowInVariable + value:true, @ForgetBallStartPosition + r:false
+        DONE --> $SecondaryStateTeamDecider
+            OUR --> #NormalBehavior
+            ELSE --> #Placing
     ELSE --> $SecondaryStateTeamDecider
         OUR --> #NormalBehavior
         ELSE --> #Placing
     NORMAL --> #NormalBehavior
     OVERTIME --> #NormalBehavior
+
 
 -->BodyBehavior
 $IsPenalized
@@ -147,11 +155,13 @@ $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
+        READY --> $DoOnce
+            NOT_DONE --> @SetNoKickOffOrThrowInVariable + value:true + r:false, @ForgetBallStartPosition + r:false
+            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

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.2
+
     # 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

src/bitbots_msgs/msg/Strategy.msg

@@ -20,6 +20,7 @@ uint8 ACTION_WAITING=4
 uint8 ACTION_KICKING=5
 uint8 ACTION_SEARCHING=6
 uint8 ACTION_LOCALIZING=7
+uint8 ACTION_PASSIVE=8
 uint8 action
 
 # Offensive strategy

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/robocup_protocol_converter.py

@@ -33,6 +33,7 @@ def __init__(self, own_team_color: TeamColor):
             (Proto.ACTION_KICKING, Strategy.ACTION_KICKING),
             (Proto.ACTION_SEARCHING, Strategy.ACTION_SEARCHING),
             (Proto.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING),
+            (Proto.ACTION_PASSIVE, Strategy.ACTION_PASSIVE),
         )
         self.side_mapping = (
             (Proto.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED),

src/bitbots_team_communication/bitbots_team_communication/scripts/show_team_comm.py

@@ -71,6 +71,7 @@ def __init__(self):
             Strategy.ACTION_POSITIONING: "Positioning",
             Strategy.ACTION_TRYING_TO_SCORE: "Trying to score",
             Strategy.ACTION_UNDEFINED: "Undefined",
+            Strategy.ACTION_PASSIVE: "Passive",
         }
         self.sides = {
             Strategy.SIDE_LEFT: "Left",

src/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py

@@ -140,6 +140,7 @@ class StrategyBox(QGroupBox):
         "Searching": Strategy.ACTION_SEARCHING,
         "Trying to Score": Strategy.ACTION_TRYING_TO_SCORE,
         "Waiting": Strategy.ACTION_WAITING,
+        "Passive": Strategy.ACTION_PASSIVE,
     }
 
     def __init__(self, parent=None):