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()
# 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) gameState.gameState = int_key elif key == 'p': # penalize / unpenalize gameState.penalized = not gameState.penalized elif key in [chr(ord('a') + x) for x in range(10)]: gameState.secondaryState = ord(key) - ord('a') elif key == 'm': gameState.secondaryStateMode = (gameState.secondaryStateMode + 1) % 3 elif key == 't': if gameState.secondaryStateTeam == ourTeamID: gameState.secondaryStateTeam = ourTeamID + 1 else: gameState.secondaryStateTeam = ourTeamID sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A")
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)