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 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()
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)