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..753a7655c 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 @@ -22,6 +22,7 @@ def __init__(self, node, blackboard=None): self.free_kick_kickoff_team: bool | None = None self.game_controller_stop: bool = False self.last_timestep_whistle_detected: Time | None = None + self.team_com_limit_has_reached: bool = False # publish stopped msg for hcm self.stop_pub = node.create_publisher(Bool, "game_controller/stop_msg", 1) @@ -84,12 +85,21 @@ 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 def get_team_id(self) -> int: return self.team_id + + def get_team_com_limit_has_reached(self) -> bool: + return self.team_com_limit_has_reached def gamestate_callback(self, gamestate_msg: GameState) -> None: if self.gamestate.penalized and not gamestate_msg.penalized: @@ -106,6 +116,8 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None: self.stop_pub.publish(Bool(data=self.game_controller_stop)) + self.team_com_limit_has_reached = gamestate_msg.message_budget < 40 + """Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt if ( gamestate_msg.main_state == GameState.STATE_SET 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 c44d479f1..a945f52e9 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 @@ -68,6 +68,16 @@ def __init__(self, node, blackboard): self.data_timeout: float = float(self._node.get_parameter("team_data_timeout").value) self.ball_max_covariance: float = float(self._node.get_parameter("ball_max_covariance").value) + # --- Save informations for handling decisions after team com has switched off --- + # The last rank to ball result before team_com limit was reached + self.last_rank_to_ball_with_team_com: int = -1 + # Was goalie active before team_com limit was reached + self.was_goalie_active: bool = True + # Was goalie handling the ball before team_com limit was reached + self.was_goalie_handling_ball_with_team_com: bool = False + # The last number of active field players before team_com limit was reached + self.last_number_of_active_players: int = -1 + @cached_capsule_function def time(self) -> Time: """Returns the current time of the node, this is its own function so it can be cached during the decision loop.""" @@ -93,7 +103,9 @@ def is_goalie_handling_ball(self) -> bool: and data.strategy.role == Strategy.ROLE_GOALIE and data.strategy.action in [Strategy.ACTION_GOING_TO_BALL, Strategy.ACTION_KICKING] ): + self.was_goalie_handling_ball_with_team_com = True return True + self.was_goalie_handling_ball_with_team_com = False return False @cached_capsule_function @@ -138,7 +150,9 @@ def team_rank_to_ball( ) for rank, distance in enumerate(sorted(distances)): if own_ball_distance < distance: + self.last_rank_to_ball_with_team_com = rank + 1 return rank + 1 + self.last_rank_to_ball_with_team_com = len(distances) + 1 return len(distances) + 1 def set_action(self, action: int) -> None: @@ -151,6 +165,18 @@ def set_action(self, action: int) -> None: def get_action(self) -> tuple[int, float]: return self.strategy.action, self.action_update + + def get_was_goalie_handling_ball(self) -> bool: + return self.was_goalie_handling_ball_with_team_com + + def get_was_goalie_active(self) -> bool: + return self.was_goalie_active + + def get_last_rank_with_team_com(self) -> int: + return self.last_rank_to_ball_with_team_com + + def get_last_number_active_player(self) -> int: + return self.last_number_of_active_players def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None: """Set the role of this robot in the team @@ -195,6 +221,7 @@ def is_not_goalie(team_data: TeamData) -> bool: team_data_infos = filter(is_not_goalie, team_data_infos) # type: ignore[assignment] # Count valid team data infos (aka robots with valid team data) + self.last_number_of_active_players = sum(map(self.is_valid, team_data_infos)) return sum(map(self.is_valid, team_data_infos)) @cached_capsule_function @@ -209,6 +236,7 @@ def is_a_goalie(team_data: TeamData) -> bool: team_data_infos = filter(is_a_goalie, team_data_infos) # type: ignore[assignment] # Count valid team data infos (aka robots with valid team data) + self.was_goalie_active = sum(map(self.is_valid, team_data_infos)) == 1 return sum(map(self.is_valid, team_data_infos)) == 1 def get_own_time_to_ball(self) -> float: diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/closest_to_ball.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/closest_to_ball.py index c3b7766b6..426956464 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/closest_to_ball.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/closest_to_ball.py @@ -28,9 +28,12 @@ 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=True, use_time_to_ball=True) - self.publish_debug_data("time to ball", my_time_to_ball) + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + rank = self.blackboard.team_data.get_last_rank_with_team_com() + else: + 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=True, 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 "YES" @@ -47,9 +50,12 @@ 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) + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + rank = self.blackboard.team_data.get_last_rank_with_team_com() + else: + 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" diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/count_active_players_without_goalie.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/count_active_players_without_goalie.py index 1440883e7..7e4a5468e 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/count_active_players_without_goalie.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/count_active_players_without_goalie.py @@ -13,7 +13,10 @@ def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): - number_of_active_teammates = self.blackboard.team_data.get_number_of_active_field_players(False) + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + number_of_active_teammates = self.blackboard.team_data.get_last_number_active_player() + else: + number_of_active_teammates = self.blackboard.team_data.get_number_of_active_field_players(False) self.publish_debug_data("Number of active Teammates", number_of_active_teammates) if number_of_active_teammates == 0: return "ZERO" diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py index c0783318c..08e4a8f39 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_active.py @@ -13,10 +13,16 @@ def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): - if self.blackboard.team_data.get_is_goalie_active(): - return "YES" + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + if self.blackboard.team_data.get_was_goalie_active(): + return "YES" + else: + return "NO" else: - return "NO" + if self.blackboard.team_data.get_is_goalie_active(): + return "YES" + else: + return "NO" def get_reevaluate(self): return True diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_handling_ball.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_handling_ball.py index fee37dcd4..83b007348 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_handling_ball.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/goalie_handling_ball.py @@ -12,10 +12,16 @@ def perform(self, reevaluate=False): """ It is determined if the goalie is currently going towards the ball """ - if self.blackboard.team_data.is_goalie_handling_ball(): - return "YES" + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + if self.blackboard.team_data.get_was_goalie_handling_ball(): + return "YES" + else: + return "NO" else: - return "NO" + if self.blackboard.team_data.is_goalie_handling_ball(): + return "YES" + else: + return "NO" def get_reevaluate(self): return True 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/decisions/team_com_limit_reached.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/team_com_limit_reached.py new file mode 100644 index 000000000..ca9ca85c8 --- /dev/null +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/team_com_limit_reached.py @@ -0,0 +1,18 @@ +from bitbots_blackboard.body_blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement + + +class TeamComLimitReached(AbstractDecisionElement): + blackboard: BodyBlackboard + + def __init__(self, blackboard, dsd, parameters): + super().__init__(blackboard, dsd, parameters) + + def perform(self, reevaluate=False): + if self.blackboard.gamstate.get_team_com_limit_has_reached(): + return "YES" + else: + return "NO" + + 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 60f838420..4bd09e7f4 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 @@ -76,11 +76,13 @@ $GoalieActive NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToBlockPosition #SupporterRole -$PassStarted - YES --> $BallSeen - YES --> @TrackBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition - NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition - NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassPreparePosition +$TeamComLimitReached + YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassPreparePosition + NO --> $PassStarted + YES --> $BallSeen + YES --> @TrackBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition + NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition + NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassPreparePosition #PenaltyShootoutBehavior $SecondaryStateTeamDecider @@ -119,6 +121,34 @@ $BallSeen YES --> $KickOffTimeUp NO_NORMAL --> #StandAndLook NO_FREEKICK --> #Placing + YES --> $ConfigRole + GOALIE --> #GoalieBehavior + ELSE --> $TeamComLimitReached //this decision is only for better visualization + NO -- > $CountActiveRobotsWithoutGoalie + ONE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + SECOND --> #DefensePositioning + ELSE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + SECOND --> #SupporterRole + THIRD --> #DefensePositioning + YES --> $CountActiveRobotsWithoutGoalie + ONE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + SECOND --> #DefensePositioning + ELSE --> $RankToBallNoGoalie + FIRST --> #StrikerRole + 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 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..d6a2ce13b 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 @@ -50,8 +50,10 @@ def __init__(self): self.socket_communication = SocketCommunication(self.node, self.logger, self.team_id, self.player_id) self.rate: int = self.node.get_parameter("rate").value + self.reduced_rate: int = self.node.get_parameter("reduced_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 +68,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.create_timer(self.rate) self.receive_forever() def spin(self): @@ -263,6 +265,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 @@ -275,8 +282,11 @@ def is_still_valid(time: Optional[TimeMsg]) -> bool: message = self.protocol_converter.convert_to_message(self, msg, is_still_valid) proto_msg = message.SerializeToString() - self.logger.debug(f"Sending msg with size {len(proto_msg)} bytes") - self.socket_communication.send_message(proto_msg) + if(len(proto_msg) > 512): + self.logger.warning(f"Team_com msg not sended, because size {len(proto_msg)} bytes is above the maximum of 512 bytes") + else: + self.logger.debug(f"Sending msg with size {len(proto_msg)} bytes") + self.socket_communication.send_message(proto_msg) def create_empty_message(self, now: Time) -> Proto.Message: message = Proto.Message() @@ -301,7 +311,15 @@ 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 + #the budget smaller 40 as stop definition makes sure we have 10 msg per robot left in case of some delay in the communication with the game controller + if self.gamestate is not None and (self.gamestate.message_budget < 40): + return False + + return True def get_current_time(self) -> Time: return self.node.get_clock().now() @@ -313,7 +331,29 @@ 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.timer.cancel() + self.create_timer(self.reduced_rate) + self.rate_is_reduced = True + self.logger.warning("Team communication: message sending rate is reduced now") + + def create_timer(self, rate: int): + self.timer = self.node.create_timer(1 / rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup()) + + def should_reduce_rate(self): + # we are allowed to send 2.5 msg per second on average with each robot (12000 with 4 robots in a 1200 sekonds game) + # the msg_left_linear_rate is the amount of messages we would send if we send exactly with this 2.5 msg per sec per robot rate + # once our actual msg budget is below this linear rate we tend to send more msg then allowed and should reduce our sending rate + if self.game_started_recently(): + return False + + msg_left_linear_rate = (self.gamestate.first_half * 600 + self.gamestate.secs_remaining) * 4 * 2.5 + return msg_left_linear_rate > self.gamestate.message_budget + + def game_started_recently(self): + #true in the first 60 seconds of the game + return self.gamestate.first_half and self.gamestate.secs_remaining > 540 def main(): rclpy.init(args=None) diff --git a/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml b/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml index b4554bc6a..49fc7ae6f 100644 --- a/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml +++ b/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml @@ -18,6 +18,8 @@ team_comm: # Rate of published messages in Hz rate: 2 + # Rate of published messages in Hz, when rate is reduced + reduced_rate: 1 # average walking speed in [m/s] avg_walking_speed: 0.2