def perform(self):
new_gs = GameState()
if self.keep_playing:
new_gs.gameState = 3
else:
new_gs.gameState = self.gs_index
new_gs.allowedToMove = True
rospy.loginfo('CURRENT GAMESTATE is {}'.format(self.gs_index))
self.gamestate_pub.publish(new_gs)
self.gs_index = (self.gs_index + 1) % 5
def __init__(self, node: Node):
self.team_id = self.node.get_parameter(
'team_id').get_parameter_value().double_value
self.gamestate = GameState()
self.last_update = 0
self.unpenalized_time = 0
self.last_goal_from_us_time = -86400
self.last_goal_time = -86400
self.free_kick_kickoff_team = None
def receive_once(self):
""" Receives a package and interprets it.
Calls :func:`on_new_gamestate`
Sends an answer to the GC """
try:
data, peer = self.socket.recvfrom(GameState.sizeof())
# Throws a ConstError if it doesn't work
parsed_state = GameState.parse(data)
# Assign the new package after it parsed successful to the state
self.state = parsed_state
self.time = time.time()
# Call the handler for the package
self.on_new_gamestate(self.state)
# Answer the GameController
self.answer_to_gamecontroller(peer)
except AssertionError as ae:
rospy.logerr(ae.message)
except socket.timeout:
rospy.loginfo_throttle(
5.0, "No GameController message received (socket timeout)")
except ConstError:
rospy.logwarn("Parse Error: Probably using an old protocol!")
finally:
if self.get_time_since_last_package(
) > self.game_controller_lost_time:
self.time += 5 # Resend message every five seconds
rospy.logwarn_throttle(
5.0,
'No game controller messages received, allowing robot to move'
)
msg = GameStateMsg()
msg.allowedToMove = True
msg.gameState = 3 # PLAYING
self.state_publisher.publish(msg)
def publisher_main():
#initiieren des publishers
rospy.init_node('publisher')
print('started publisher node')
pub = rospy.Publisher('ball_relative', BallRelative, queue_size=10)
pubRobo = rospy.Publisher('amcl_pose',
PoseWithCovarianceStamped,
queue_size=10)
pubTeam = rospy.Publisher('obstacles_relative',
ObstaclesRelative,
queue_size=10)
pubStrategy = rospy.Publisher('strategy', Strategy, queue_size=10)
pubGame = rospy.Publisher('gamestate', GameState, queue_size=10)
pubState = rospy.Publisher('robot_state', RobotControlState, queue_size=10)
pubTarget = rospy.Publisher('move_base_simple/goal', Pose2D, queue_size=10)
rate = rospy.Rate(10)
timeCounter = 30
roboActionCounter = 30
firsthalf = True
durationHalfGame = 60
# Coordinates from pathMaker ========================================================================================
# robo1 with pathmaker
robo1 = getCoordinates("robo4.yaml")
robo1Length = len(robo1)
robo1Counter = 1
# teammates with pathmaker
teammate1 = getCoordinates('TeamClubMate1.yaml')
team1Length = len(teammate1) #anzahl eintraege
team1Counter = 1
teammate2 = getCoordinates('TeamClubMate2.yaml')
team2Length = len(teammate2)
team2Counter = 1
# opponents with pathmaker
opponent1 = getCoordinates('SuperScaryOpponent.yaml')
op1Length = len(opponent1)
op1Counter = 1
# opponents with pathmaker
undef = getCoordinates('undef.yaml')
undefLength = len(opponent1)
undefCounter = 1
# ball with pathmaker
ball = getCoordinates('HeartBall.yaml')
ballLength = len(ball)
ballCounter = 1
#teammate1[0 % length ].get('x') # fuer 0 ein counter, dann entsteht loop
#teammate1[1].get('x') # an der ersten Stelle x-wert
while not rospy.is_shutdown():
# Ball with pathmaker
msgBall = BallRelative()
msgBall.header.stamp = rospy.Time.now()
msgBall.header.frame_id = "base_link"
msgBall.ball_relative.y = ball[ballCounter % ballLength].get('x')
msgBall.ball_relative.x = ball[ballCounter % ballLength].get('y')
msgBall.confidence = 1.0
pub.publish(msgBall)
ballCounter += 1
# Robo1 with pathmaker
msgRobo = PoseWithCovarianceStamped()
msgRobo.header.stamp = rospy.Time.now()
msgRobo.pose.pose.position.x = robo1[int(robo1Counter) %
robo1Length].get('x')
msgRobo.pose.pose.position.y = robo1[int(robo1Counter) %
robo1Length].get('y')
# Angle of robot in quaternions
angle = robo1[int(robo1Counter) % robo1Length].get('ang')
quaternion = tf.transformations.quaternion_from_euler(
0, 0, float(angle))
msgRobo.pose.pose.orientation.x = quaternion[0]
msgRobo.pose.pose.orientation.y = quaternion[1]
msgRobo.pose.pose.orientation.z = quaternion[2]
msgRobo.pose.pose.orientation.w = quaternion[3]
pubRobo.publish(msgRobo)
# Role of Robo1, gets information from pathMaker
msgStrategy = Strategy()
msgRoleString = robo1[int(robo1Counter) % robo1Length].get('action')
msgStrategy.role = actionDecoder.get(
msgRoleString
) #actiondecoder gleicht den string ab mit dictonary und gibt int zurueck
# Action of Robo1, changes after short time (roboActionCounter)
if roboActionCounter == 0:
msgStrategy.action = 3 # TRYING_TO_SCORE
else:
msgStrategy.action = 2 # GOING_TO_BALL
pubStrategy.publish(msgStrategy)
roboActionCounter -= 1
roboActionCounter = max(roboActionCounter, 0)
robo1Counter += 1
# Teammates with pathmaker, contains list of teammates
msgTeam = ObstaclesRelative()
msgTeam1 = ObstacleRelative()
msgTeam1.color = 2 # magenta
msgTeam1.position.x = teammate1[int(team1Counter) %
team1Length].get('x')
msgTeam1.position.y = teammate1[int(team1Counter) %
team1Length].get('y')
msgTeam2 = ObstacleRelative()
msgTeam2.color = 2 # magenta
msgTeam2.position.x = teammate2[int(team2Counter) %
team2Length].get('x')
msgTeam2.position.y = teammate2[int(team2Counter) %
team2Length].get('y')
# Opponents with pathmaker, contains list of opponents
msgOp = ObstaclesRelative()
msgUndef = ObstacleRelative()
msgUndef.color = 1 # undef
msgUndef.position.x = undef[int(undefCounter) % undefLength].get('x')
msgUndef.position.y = undef[int(undefCounter) % undefLength].get('y')
msgOp1 = ObstacleRelative()
msgOp1.color = 3 # cyan
msgOp1.position.x = opponent1[int(op1Counter) % op1Length].get('x')
msgOp1.position.y = opponent1[int(op1Counter) % op1Length].get('y')
# Publish all obstacles
msgTeam.obstacles = [msgTeam1, msgTeam2, msgOp1, msgUndef]
pubTeam.publish(msgTeam)
team1Counter += 1
team2Counter += 1
op1Counter += 1
undefCounter += 1
# GameState msgs ===========================================================================================
# Penalty: Seconds till unpenalized and boolean
msgGame = GameState()
msgBall.header.stamp = rospy.Time.now()
msgGame.secondsTillUnpenalized = timeCounter
# Penalty boolean
msgGame.penalized = timeCounter > 0
# Sets halftime and rest secs
msgGame.firstHalf = firsthalf
msgGame.secondsRemaining = durationHalfGame
# Sets Score
msgGame.ownScore = 7
msgGame.rivalScore = 1
# team colors
msgGame.teamColor = 1 # magenta
pubGame.publish(msgGame)
timeCounter -= 1
timeCounter = max(timeCounter, 0)
durationHalfGame -= 1
if durationHalfGame == 0:
durationHalfGame = 60
firsthalf = False
# Sets hardware state
msgState = RobotControlState()
msgState.state = 10
pubState.publish(msgState)
# Target
msgTarget = Pose2D()
if firsthalf:
msgTarget.x = 3.5
msgTarget.y = 2.0
else:
msgTarget.x = 2.0
msgTarget.y = 1.0
pubTarget.publish(msgTarget)
rate.sleep()
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('sim_gamestate')
namespaces = ['amy/', 'rory/', 'jack/', 'donna/', '']
publishers = [
rospy.Publisher(n + 'gamestate',
GameStateMsg,
queue_size=1,
latch=True) for n in namespaces
]
gameState = GameStateMsg()
gameState.header.stamp = rospy.Time.now()
# init secondary state team to our teamID
gameState.secondaryStateTeam = rospy.get_param(
"team_id", 8) # 8 is the default TeamID in the behavior
ourTeamID = gameState.secondaryStateTeam
try:
print(msg)
while True:
key = get_key()
if key == '\x03':
break
elif key in ['0', '1', '2', '3', '4']:
int_key = int(key)
def on_new_gamestate(self, state):
""" Is called with the new game state after receiving a package
Needs to be implemented or set
:param state: Game State
"""
if state.teams[0].team_number == self.team:
own_team = state.teams[0]
rival_team = state.teams[1]
elif state.teams[1].team_number == self.team:
own_team = state.teams[1]
rival_team = state.teams[0]
else:
rospy.logerr('Team {} not playing, only {} and {}'.format(
self.team, state.teams[0].team_number,
state.teams[1].team_number))
return
try:
me = own_team.players[self.player - 1]
except IndexError:
rospy.logerr('Robot {} not playing'.format(self.player))
return
msg = GameStateMsg()
msg.header.stamp = rospy.Time.now()
msg.gameState = state.game_state.intvalue
msg.secondaryState = state.secondary_state.intvalue
msg.firstHalf = state.first_half
msg.ownScore = own_team.score
msg.rivalScore = rival_team.score
msg.secondsRemaining = state.seconds_remaining
msg.secondary_seconds_remaining = state.secondary_seconds_remaining
msg.hasKickOff = state.kick_of_team == self.team
msg.penalized = me.penalty != 0
msg.secondsTillUnpenalized = me.secs_till_unpenalized
if me.penalty != 0:
msg.allowedToMove = False
elif state.game_state in ('STATE_INITIAL', 'STATE_SET'):
msg.allowedToMove = False
elif state.game_state == 'STATE_READY':
msg.allowedToMove = True
elif state.game_state == 'STATE_PLAYING':
if state.kick_of_team >= 128:
# Drop ball
msg.allowedToMove = True
elif state.secondary_state in ('STATE_DIRECT_FREEKICK',
'STATE_INDIRECT_FREEKICK',
'STATE_PENALTYKICK',
'STATE_CORNER_KICK',
'STATE_GOAL_KICK',
'STATE_THROW_IN'):
if state.secondary_state_info[1] in (0, 2):
msg.allowedToMove = False
else:
msg.allowedToMove = True
msg.secondaryStateTeam = state.secondary_state_info[0]
elif state.secondary_state == 'STATE_PENALTYSHOOT':
# we have penalty kick
if state.kick_of_team == self.team:
msg.allowedToMove = True
else:
msg.allowedToMove = False
elif state.kick_of_team == self.team:
msg.allowedToMove = True
else:
# Other team has kickoff
if msg.secondary_seconds_remaining != 0:
msg.allowedToMove = False
else:
# We have waited the kickoff time
msg.allowedToMove = True
msg.teamColor = own_team.team_color.intvalue
msg.dropInTeam = state.drop_in_team
msg.dropInTime = state.drop_in_time
msg.penaltyShot = own_team.penalty_shot
msg.singleShots = own_team.single_shots
msg.coach_message = own_team.coach_message
self.state_publisher.publish(msg)
def __init__(self):
self.team_id = rospy.get_param("team_id", 8)
self.gamestate = GameState()
self.last_update = 0
self.unpenalized_since = 0
def __init__(self):
self.team_id = rospy.get_param("team_id", 8)
self.kick_off_valid_time = rosparam.get_param("Behaviour/Body/Common/kickOffValidTime")
self.drop_ball_valid_time = rosparam.get_param("Behaviour/Body/Common/dropBallValidTime")
self.gamestate = GameState()
self.last_update = 0
def on_new_gamestate(self, state):
""" Is called with the new game state after receiving a package.
The information is processed and published as a standard message to a ROS topic.
:param state: Game State
"""
if state.teams[0].team_number == self.team:
own_team = state.teams[0]
rival_team = state.teams[1]
elif state.teams[1].team_number == self.team:
own_team = state.teams[1]
rival_team = state.teams[0]
else:
rospy.logerr('Team {} not playing, only {} and {}'.format(
self.team, state.teams[0].team_number,
state.teams[1].team_number))
return
try:
me = own_team.players[self.player - 1]
except IndexError:
rospy.logerr('Robot {} not playing'.format(self.player))
return
msg = GameStateMsg()
msg.header.stamp = rospy.Time.now()
msg.gameState = state.game_state.intvalue
msg.secondaryState = state.secondary_state.intvalue
msg.secondaryStateMode = state.secondary_state_info[1]
msg.firstHalf = state.first_half
msg.ownScore = own_team.score
msg.rivalScore = rival_team.score
msg.secondsRemaining = state.seconds_remaining
msg.secondary_seconds_remaining = state.secondary_seconds_remaining
msg.hasKickOff = state.kick_of_team == self.team
msg.penalized = me.penalty != 0
msg.secondsTillUnpenalized = me.secs_till_unpenalized
msg.secondaryStateTeam = state.secondary_state_info[0]
msg.secondaryStateMode = state.secondary_state_info[1]
msg.teamColor = own_team.team_color.intvalue
msg.dropInTeam = state.drop_in_team
msg.dropInTime = state.drop_in_time
msg.penaltyShot = own_team.penalty_shot
msg.singleShots = own_team.single_shots
msg.coach_message = own_team.coach_message
self.state_publisher.publish(msg)
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('sim_gamestate')
state_publisher = rospy.Publisher('gamestate', GameStateMsg, queue_size=1)
gameState = GameStateMsg()
gameState.header.stamp = rospy.Time.now()
try:
print(msg)
while True:
key = getKey()
if key == ' ':
gameState.allowedToMove = not gameState.allowedToMove
elif key == '\x03':
break
elif key in ['0', '1', '2', '3', '4']:
int_key = int(key)
gameState.gameState = int_key
elif key == 'p': # penalize/unpenalize
gameState.penalized = not gameState.penalized