diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py index 7aa51388bd..9e1f93b91d 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py +++ b/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 ## ##################### diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py index c44d479f18..4fe4483a94 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py +++ b/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 ): diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/change_action.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/change_action.py index 4f80dc6fab..1d288344fb 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/change_action.py +++ b/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): diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py index 4ec541313c..129d6e7884 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py +++ b/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) diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/set_no_kick_off_or_throw_in_variable.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/set_no_kick_off_or_throw_in_variable.py new file mode 100644 index 0000000000..dd432c4849 --- /dev/null +++ b/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() diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/store_movement_detection_ball_position.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/store_movement_detection_ball_position.py new file mode 100644 index 0000000000..691c049720 --- /dev/null +++ b/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() diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/sceond_ball_touch_allowed.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/sceond_ball_touch_allowed.py new file mode 100644 index 0000000000..2fb8ebf37a --- /dev/null +++ b/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 diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index b44d630353..98bdd4d47a 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/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 diff --git a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 649cb77c7e..8c3181b952 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/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 diff --git a/src/bitbots_msgs/msg/Strategy.msg b/src/bitbots_msgs/msg/Strategy.msg index b83702eb76..674a0ae5ee 100644 --- a/src/bitbots_msgs/msg/Strategy.msg +++ b/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 diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/robocup_protocol_converter.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/robocup_protocol_converter.py index b3ca448c78..daa404cbb0 100644 --- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/robocup_protocol_converter.py +++ b/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), diff --git a/src/bitbots_team_communication/bitbots_team_communication/scripts/show_team_comm.py b/src/bitbots_team_communication/bitbots_team_communication/scripts/show_team_comm.py index 14e137f6b4..c156fb3007 100755 --- a/src/bitbots_team_communication/bitbots_team_communication/scripts/show_team_comm.py +++ b/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", diff --git a/src/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py b/src/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py index 80da523f25..b72cd34052 100755 --- a/src/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py +++ b/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):