diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py index 1a2235e9f..2dcb5a048 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py +++ b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py @@ -84,6 +84,12 @@ def get_seconds_since_unpenalized(self) -> float: def get_is_penalized(self) -> bool: return self.gamestate.penalized + + def get_penalized_team_mates(self) -> int: + return self.gamestate.team_mates_with_penalty + + def get_penalized_rivals(self) -> int: + return self.gamestate.rivals_with_penalty def received_gamestate(self) -> bool: return self.last_update != 0.0 diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/number_penalized_players.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/number_penalized_players.py new file mode 100644 index 000000000..e6b04cba9 --- /dev/null +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/number_penalized_players.py @@ -0,0 +1,66 @@ +from bitbots_blackboard.body_blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement +from game_controller_hsl_interfaces.msg import GameState + + +class NumberPenalizedTeamMates(AbstractDecisionElement): + blackboard: BodyBlackboard + + def __init__(self, blackboard, dsd, parameters): + super().__init__(blackboard, dsd, parameters) + + def perform(self, reevaluate=False): + """ + Return number of penalized team mates + :param reevaluate: + :return: + """ + game_state_penalized_team_mates = self.blackboard.gamestate.get_penalized_team_mates() + + if game_state_penalized_team_mates == 4: + return "FOUR" + elif game_state_penalized_team_mates == 3: + return "THREE" + elif game_state_penalized_team_mates == 2: + return "TWO" + elif game_state_penalized_team_mates == 1: + return "ONE" + else: + return "ZERO" + + def get_reevaluate(self): + """ + Game state can change during the game + """ + return True + +class NumberPenalizedRivals(AbstractDecisionElement): + blackboard: BodyBlackboard + + def __init__(self, blackboard, dsd, parameters): + super().__init__(blackboard, dsd, parameters) + + def perform(self, reevaluate=False): + """ + Return number of penalized rivals + :param reevaluate: + :return: + """ + game_state_penalized_rivals = self.blackboard.gamestate.get_penalized_rivals() + + if game_state_penalized_rivals == 4: + return "FOUR" + elif game_state_penalized_rivals == 3: + return "THREE" + elif game_state_penalized_rivals == 2: + return "TWO" + elif game_state_penalized_rivals == 1: + return "ONE" + else: + return "ZERO" + + def get_reevaluate(self): + """ + Game state can change during the game + """ + 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 60f838420..789a14102 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 @@ -108,6 +108,27 @@ $DoOnce SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition +#PlacingWithoutTeamCom +$DoOnce + NOT_DONE --> @ForgetBall, @LookAtFieldFeatures + DONE --> $ConfigRole + GOALIE --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @GoToBlockPosition + ELSE --> $BallSeen + NO --> $SecondaryStateTeamDecider + OUR --> #SearchBall + OTHER --> @AvoidBallActive, @LookAtFieldFeatures, @WalkInPlace + duration:2, @GoToRelativePosition + x:1 + y:0 + t:0, @Stand + //TODO + YES --> $NumberPenalizedTeamMates + ZERO + ONE + TWO + THREE + FOUR + YES --> $RankToBallNoGoalieWithoutTeamCom + 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 + #Init @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand @@ -130,15 +151,43 @@ $BallSeen SECOND --> #SupporterRole THIRD --> #DefensePositioning +#NormalBehaviorWithoutTeamCom +$BallSeen + NO --> $ConfigRole + GOALIE --> #RolePositionWithPause + ELSE --> #SearchBall + YES --> $KickOffTimeUp + NO_NORMAL --> #StandAndLook + NO_FREEKICK --> #PlacingWithoutTeamCom + YES --> $ConfigRole + GOALIE --> #GoalieBehavior + ELSE --> $CountActiveRobotsWithoutGoalie + ONE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + SECOND --> #DefensePositioning + ELSE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + SECOND --> #SupporterRole + THIRD --> #DefensePositioning + #PlayingBehavior -$SecondaryStateDecider - PENALTYSHOOT --> #PenaltyShootoutBehavior - TIMEOUT --> #StandAndLook - ELSE --> $SecondaryStateTeamDecider - OUR --> #NormalBehavior - OTHER --> #Placing - NORMAL --> #NormalBehavior - OVERTIME --> #NormalBehavior +$TeamComLimitReached + YES --> $SecondaryStateDecider + PENALTYSHOOT --> #PenaltyShootoutBehavior + TIMEOUT --> #StandAndLook + ELSE --> $SecondaryStateTeamDecider + OUR --> #NormalBehaviorWithoutTeamCom + OTHER --> #PlacingWithoutTeamCom + NORMAL --> #NormalBehaviorWithoutTeamCom + OVERTIME --> #NormalBehaviorWithoutTeamCom + NO --> $SecondaryStateDecider + PENALTYSHOOT --> #PenaltyShootoutBehavior + TIMEOUT --> #StandAndLook + ELSE --> $SecondaryStateTeamDecider + OUR --> #NormalBehavior + OTHER --> #Placing + NORMAL --> #NormalBehavior + OVERTIME --> #NormalBehavior -->BodyBehavior $IsPenalized diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py index d52c6e7a1..1eda840f3 100755 --- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py +++ b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py @@ -52,6 +52,7 @@ def __init__(self): self.rate: int = self.node.get_parameter("rate").value self.lifetime: int = self.node.get_parameter("lifetime").value self.avg_walking_speed: float = self.node.get_parameter("avg_walking_speed").value + self.rate_is_reduced: bool = False self.topics = get_parameter_dict(self.node, "topics") self.map_frame: str = self.node.get_parameter("map_frame").value @@ -66,7 +67,7 @@ def __init__(self): self.run_spin_in_thread() self.try_to_establish_connection() - self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup()) + self.timer = self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup()) self.receive_forever() def spin(self): @@ -263,6 +264,11 @@ def handle_message(self, string_message: bytes): self.team_data_publisher.publish(team_data) def send_message(self): + + if not self.rate_is_reduced: + if self.gamestate is not None and self.gamestate.secs_remaining > 180 and (self.gamestate.message_budget / self.gamestate.secs_remaining) < 11.2: + self.reduce_rate() + if not self.is_robot_allowed_to_send_message(): self.logger.debug("Robot is not allowed to send message") return @@ -301,7 +307,17 @@ def should_message_be_discarded(self, message: Proto.Message) -> bool: return is_own_message or is_message_from_oposite_team def is_robot_allowed_to_send_message(self) -> bool: - return self.gamestate is not None and not self.gamestate.penalized + #a penalized robot doesn't need to publish + if self.gamestate is not None and not self.gamestate.penalized: + return False + #if we are close to our message budget, we dont want to continue publishing + if self.gamestate is not None and (self.gamestate.message_budget > 40): + return False + #we dont want to publish messages if only one robot is in a team. (this may never occure since this is the max team size, not the number of active players) + if self.gamestate is not None and self.gamestate.players_per_team == 1: + return False + + return True def get_current_time(self) -> Time: return self.node.get_clock().now() @@ -313,6 +329,13 @@ def extract_orientation_yaw_angle(self, quaternion: Quaternion): def convert_to_euler(self, quaternion: Quaternion): return transforms3d.euler.quat2euler([quaternion.w, quaternion.x, quaternion.y, quaternion.z]) + + def reduce_rate(self): + self.rate = 1 + self.timer.cancel() + self.timer = self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup()) + self.rate_is_reduced = True + def main():