11from typing import Optional
22
33from bitbots_utils .utils import get_parameters_from_other_node
4- from game_controller_hl_interfaces .msg import GameState
4+ from game_controller_hsl_interfaces .msg import GameState
5+ from std_msgs .msg import Bool
56
67from bitbots_blackboard .capsules import AbstractBlackboardCapsule
78
@@ -12,33 +13,44 @@ class GameStatusCapsule(AbstractBlackboardCapsule):
1213 def __init__ (self , node , blackboard = None ):
1314 super ().__init__ (node , blackboard )
1415 self .team_id : int = get_parameters_from_other_node (self ._node , "parameter_blackboard" , ["team_id" ])["team_id" ]
16+ self .own_id : int = get_parameters_from_other_node (self ._node , "parameter_blackboard" , ["bot_id" ])["bot_id" ]
1517 self .gamestate = GameState ()
1618 self .last_update : float = 0.0
1719 self .unpenalized_time : float = 0.0
1820 self .last_goal_from_us_time = - 86400.0
1921 self .last_goal_time = - 86400.0
2022 self .free_kick_kickoff_team : Optional [bool ] = None
23+ self .game_controller_stop : bool = False
24+ # publish stopped msg for hcm
25+ self .stop_pub = node .create_publisher (Bool , "game_controller/stop_msg" , 1 )
2126
22- def get_gamestate (self ) -> int :
23- return self .gamestate .game_state
27+ def get_game_state (self ) -> int :
28+ # Init, ready, set, playing, finished
29+ return self .gamestate .main_state
2430
25- def get_secondary_state (self ) -> int :
26- return self .gamestate .secondary_state
31+ def get_game_phase (self ) -> int :
32+ # Timeout, Normal, Extratime, Penaltyshoot
33+ return self .gamestate .game_phase
2734
28- def get_secondary_state_mode (self ) -> int :
29- return self .gamestate .secondary_state_mode
35+ def get_set_play (self ) -> int :
36+ # None, Direct Freekick, Indirect Freekick, Penalty, Throw in, Goalkick, Cornerkick,
37+ return self .gamestate .set_play
3038
3139 def get_secondary_team (self ) -> int :
32- return self .gamestate .secondary_state_team
40+ # Team ID, wer in set Play den Ball hat
41+ return self .gamestate .kicking_team
3342
3443 def has_kickoff (self ) -> bool :
35- return self .gamestate .has_kick_off
44+ # vegelcih mit eigener Teamnummer
45+ return self .gamestate .kicking_team == self .team_id
46+
47+ def is_stopped (self ) -> bool :
48+ return self .gamestate .stopped
3649
3750 def has_penalty_kick (self ) -> bool :
3851 return (
39- self .gamestate .secondary_state == GameState .STATE_PENALTYKICK
40- or self .gamestate .secondary_state == GameState .STATE_PENALTYSHOOT
41- ) and self .gamestate ._secondary_state_team == self .team_id
52+ self .gamestate .set_play == GameState .SET_PLAY_PENALTY_KICK and self .gamestate .kicking_team == self .team_id
53+ )
4254
4355 def get_our_goals (self ) -> int :
4456 return self .gamestate .own_score
@@ -55,26 +67,17 @@ def get_seconds_since_any_goal(self) -> float:
5567 def get_seconds_remaining (self ) -> float :
5668 # Time from the message minus time passed since receiving it
5769 return max (
58- self .gamestate .seconds_remaining - (self ._node .get_clock ().now ().nanoseconds / 1e9 - self .last_update ), 0.0
70+ self .gamestate .secs_remaining - (self ._node .get_clock ().now ().nanoseconds / 1e9 - self .last_update ), 0.0
5971 )
6072
6173 def get_secondary_seconds_remaining (self ) -> float :
6274 """Seconds remaining for things like kickoff"""
6375 # Time from the message minus time passed since receiving it
6476 return max (
65- self .gamestate .secondary_seconds_remaining
66- - (self ._node .get_clock ().now ().nanoseconds / 1e9 - self .last_update ),
77+ self .gamestate .secondary_time - (self ._node .get_clock ().now ().nanoseconds / 1e9 - self .last_update ),
6778 0.0 ,
6879 )
6980
70- def get_seconds_since_last_drop_ball (self ) -> Optional [float ]:
71- """Returns the seconds since the last drop in"""
72- if self .gamestate .drop_in_time == - 1 :
73- return None
74- else :
75- # Time from the message plus seconds passed since receiving it
76- return self .gamestate .drop_in_time + (self ._node .get_clock ().now ().nanoseconds / 1e9 - self .last_update )
77-
7881 def get_seconds_since_unpenalized (self ) -> float :
7982 return self ._node .get_clock ().now ().nanoseconds / 1e9 - self .unpenalized_time
8083
@@ -87,9 +90,6 @@ def received_gamestate(self) -> bool:
8790 def get_team_id (self ) -> int :
8891 return self .team_id
8992
90- def get_red_cards (self ) -> int :
91- return self .gamestate .team_mates_with_red_card
92-
9393 def gamestate_callback (self , gamestate_msg : GameState ) -> None :
9494 if self .gamestate .penalized and not gamestate_msg .penalized :
9595 self .unpenalized_time = self ._node .get_clock ().now ().nanoseconds / 1e9
@@ -101,21 +101,29 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
101101 if gamestate_msg .rival_score > self .gamestate .rival_score :
102102 self .last_goal_time = self ._node .get_clock ().now ().nanoseconds / 1e9
103103
104+ self .game_controller_stop = gamestate_msg .stopped
105+
106+ self .stop_pub .publish (Bool (data = self .game_controller_stop ))
107+
108+ """Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
104109 if (
105- gamestate_msg .secondary_state_mode == 2
106- and self .gamestate .secondary_state_mode != 2
107- and gamestate_msg .game_state == GameState .GAMESTATE_PLAYING
110+ gamestate_msg.main_state == GameState.STATE_SET
111+ and self.gamestate.setPlay != 2
112+ and gamestate_msg.state == GameState.STATE_PLAYING
108113 ):
109114 # secondary action is now executed but we will not see this in the new messages.
110115 # it will look like a normal kick off, but we need to remember that this is some sort of free kick
111116 # we set the kickoff value accordingly, then we will not be allowed to move if it is a kick for the others
112- self .free_kick_kickoff_team = gamestate_msg .secondary_state_team
117+ self.free_kick_kickoff_team = gamestate_msg.kicking_team
113118
114- if gamestate_msg .secondary_state_mode != 2 and gamestate_msg .secondary_seconds_remaining == 0 :
119+ if gamestate_msg.set_play != 2 and gamestate_msg.secondary_time == 0:
120+ self.free_kick_kickoff_team = gamestate_msg.kicking_team
121+
122+ if gamestate_msg.set_play != 2 and gamestate_msg.secondary_time == 0:
115123 self.free_kick_kickoff_team = None
116124
117125 if self.free_kick_kickoff_team is not None:
118126 gamestate_msg.has_kick_off = self.free_kick_kickoff_team == self.team_id
119-
127+ """
120128 self .last_update = self ._node .get_clock ().now ().nanoseconds / 1e9
121129 self .gamestate = gamestate_msg
0 commit comments