ChenXi/FCP_gym
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init gym

Browse Source
This commit is contained in:
ChenXi
2025-11-19 08:08:22 -05:00
commit eaaa5519bd
256 changed files with 46657 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

306
communication/Radio.py Normal file
View File

@@ -0,0 +1,306 @@
from typing import List
from world.commons.Other_Robot import Other_Robot
from world.World import World
import numpy as np
class Radio():
'''
map limits are hardcoded:
teammates/opponents positions (x,y) in ([-16,16],[-11,11])
ball position (x,y) in ([-15,15],[-10,10])
known server limitations:
claimed: all ascii from 0x20 to 0x7E except ' ', '(', ')'
bugs:
- ' or " clip the message
- '\' at the end or near another '\'
- ';' at beginning of message
'''
# map limits are hardcoded:
# lines, columns, half lines index, half cols index, (lines-1)/x_span, (cols-1)/y_span, combinations, combinations*2states,
TP = 321,221,160,110,10, 10,70941,141882 # teammate position
OP = 201,111,100,55, 6.25,5, 22311,44622 # opponent position
BP = 301,201,150,100,10, 10,60501 # ball position
SYMB = "!#$%&*+,-./0123456789:<=>?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[]^_`abcdefghijklmnopqrstuvwxyz{|}~;"
SLEN = len(SYMB)
SYMB_TO_IDX = {ord(s):i for i,s in enumerate(SYMB)}
def __init__(self, world : World, commit_announcement) -> None:
self.world = world
self.commit_announcement = commit_announcement
r = world.robot
t = world.teammates
o = world.opponents
self.groups = ( # player team/unum, group has ball?, self in group?
[(t[9],t[10],o[6],o[7],o[8],o[9],o[10]), True ], # 2 teammates, 5 opponents, ball
[(t[0],t[1], t[2],t[3],t[4],t[5],t[6] ), False], # 7 teammates
[(t[7],t[8], o[0],o[1],o[2],o[3],o[4],o[5]), False] # 2 teammates, 6 opponents
)
for g in self.groups: # add 'self in group?'
g.append(any(i.is_self for i in g[0]))
def get_player_combination(self, pos, is_unknown, is_down, info):
''' Returns combination (0-based) and number of possible combinations '''
if is_unknown:
return info[7]+1, info[7]+2 # return unknown combination
x,y = pos[:2]
if x < -17 or x > 17 or y < -12 or y > 12:
return info[7], info[7]+2 # return out of bounds combination (if it exceeds 1m in any axis)
# convert to int to avoid overflow later
l = int(np.clip( round(info[4]*x+info[2]), 0, info[0]-1 )) # absorb out of bounds positions (up to 1m in each axis)
c = int(np.clip( round(info[5]*y+info[3]), 0, info[1]-1 ))
return (l*info[1]+c)+(info[6] if is_down else 0), info[7]+2 # return valid combination
def get_ball_combination(self, x, y):
''' Returns combination (0-based) and number of possible combinations '''
# if ball is out of bounds, we force it in
l = int(np.clip( round(Radio.BP[4]*x+Radio.BP[2]), 0, Radio.BP[0]-1 ))
c = int(np.clip( round(Radio.BP[5]*y+Radio.BP[3]), 0, Radio.BP[1]-1 ))
return l*Radio.BP[1]+c, Radio.BP[6] # return valid combination
def get_ball_position(self,comb):
l = comb // Radio.BP[1]
c = comb % Radio.BP[1]
return np.array([l/Radio.BP[4]-15, c/Radio.BP[5]-10, 0.042]) # assume ball is on ground
def get_player_position(self,comb, info):
if comb == info[7]: return -1 # player is out of bounds
if comb == info[7]+1: return -2 # player is in unknown location
is_down = comb >= info[6]
if is_down:
comb -= info[6]
l = comb // info[1]
c = comb % info[1]
return l/info[4]-16, c/info[5]-11, is_down
def check_broadcast_requirements(self):
'''
Check if broadcast group is valid
Returns
-------
ready : bool
True if all requirements are met
Sequence: g0,g1,g2, ig0,ig1,ig2, iig0,iig1,iig2 (whole cycle: 0.36s)
igx means 'incomplete group', where <=1 element can be MIA recently
iigx means 'very incomplete group', where <=2 elements can be MIA recently
Rationale: prevent incomplete messages from monopolizing the broadcast space
However:
- 1st round: when 0 group members are missing, that group will update 3 times every 0.36s
- 2nd round: when 1 group member is recently missing, that group will update 2 times every 0.36s
- 3rd round: when 2 group members are recently missing, that group will update 1 time every 0.36s
- when >2 group members are recently missing, that group will not be updated
Players that have never been seen or heard are not considered for the 'recently missing'.
If there is only 1 group member since the beginning, the respective group can be updated, except in the 1st round.
In this way, the 1st round cannot be monopolized by clueless agents, which is important during games with 22 players.
'''
w = self.world
r = w.robot
ago40ms = w.time_local_ms - 40
ago370ms = w.time_local_ms - 370 # maximum delay (up to 2 MIAs) is 360ms because radio has a delay of 20ms (otherwise max delay would be 340ms)
group : List[Other_Robot]
idx9 = int((w.time_server * 25)+0.1) % 9 # sequence of 9 phases
max_MIA = idx9 // 3 # maximum number of MIA players (based on server time)
group_idx = idx9 % 3 # group number (based on server time)
group, has_ball, is_self_included = self.groups[group_idx]
#============================================ 0. check if group is valid
if has_ball and w.ball_abs_pos_last_update < ago40ms: # Ball is included and not up to date
return False
if is_self_included and r.loc_last_update < ago40ms: # Oneself is included and unable to self-locate
return False
# Get players that have been previously seen or heard but not recently
MIAs = [not ot.is_self and ot.state_last_update < ago370ms and ot.state_last_update > 0 for ot in group]
self.MIAs = [ot.state_last_update == 0 or MIAs[i] for i,ot in enumerate(group)] # add players that have never been seen
if sum(MIAs) > max_MIA: # checking if number of recently missing members is not above threshold
return False
# never seen before players are always ignored except when:
# - this is the 0 MIAs round (see explanation above)
# - all are MIA
if (max_MIA == 0 and any(self.MIAs)) or all(self.MIAs):
return False
# Check for invalid members. Conditions:
# - Player is other and not MIA and:
# - last update was >40ms ago OR
# - last update did not include the head (head is important to provide state and accurate position)
if any(
(not ot.is_self and not self.MIAs[i] and
(ot.state_last_update < ago40ms or ot.state_last_update==0 or len(ot.state_abs_pos)<3)# (last update: has no head or is old)
) for i,ot in enumerate(group)
):
return False
return True
def broadcast(self):
'''
Commit messages to teammates if certain conditions are met
Messages contain: positions/states of every moving entity
'''
if not self.check_broadcast_requirements():
return
w = self.world
ot : Other_Robot
group_idx = int((w.time_server * 25)+0.1) % 3 # group number based on server time
group, has_ball, _ = self.groups[group_idx]
#============================================ 1. create combination
# add message number
combination = group_idx
no_of_combinations = 3
# add ball combination
if has_ball:
c, n = self.get_ball_combination(w.ball_abs_pos[0], w.ball_abs_pos[1])
combination += c * no_of_combinations
no_of_combinations *= n
# add group combinations
for i,ot in enumerate(group):
c, n = self.get_player_combination(ot.state_abs_pos, # player position
self.MIAs[i], ot.state_fallen, # is unknown, is down
Radio.TP if ot.is_teammate else Radio.OP) # is teammate
combination += c * no_of_combinations
no_of_combinations *= n
assert(no_of_combinations < 9.61e38) # 88*89^19 (first character cannot be ';')
#============================================ 2. create message
# 1st msg symbol: ignore ';' due to server bug
msg = Radio.SYMB[combination % (Radio.SLEN-1)]
combination //= (Radio.SLEN-1)
# following msg symbols
while combination:
msg += Radio.SYMB[combination % Radio.SLEN]
combination //= Radio.SLEN
#============================================ 3. commit message
self.commit_announcement(msg.encode()) # commit message
def receive(self, msg:bytearray):
w = self.world
r = w.robot
ago40ms = w.time_local_ms - 40
ago110ms = w.time_local_ms - 110
msg_time = w.time_local_ms - 20 # message was sent in the last step
#============================================ 1. get combination
# read first symbol, which cannot be ';' due to server bug
combination = Radio.SYMB_TO_IDX[msg[0]]
total_combinations = Radio.SLEN-1
if len(msg)>1:
for m in msg[1:]:
combination += total_combinations * Radio.SYMB_TO_IDX[m]
total_combinations *= Radio.SLEN
#============================================ 2. get msg ID
message_no = combination % 3
combination //= 3
group, has_ball, _ = self.groups[message_no]
#============================================ 3. get data
if has_ball:
ball_comb = combination % Radio.BP[6]
combination //= Radio.BP[6]
players_combs = []
ot : Other_Robot
for ot in group:
info = Radio.TP if ot.is_teammate else Radio.OP
players_combs.append( combination % (info[7]+2) )
combination //= info[7]+2
#============================================ 4. update world
if has_ball and w.ball_abs_pos_last_update < ago40ms: # update ball if it was not seen
time_diff = (msg_time - w.ball_abs_pos_last_update) / 1000
ball = self.get_ball_position(ball_comb)
w.ball_abs_vel = (ball - w.ball_abs_pos) / time_diff
w.ball_abs_speed = np.linalg.norm(w.ball_abs_vel)
w.ball_abs_pos_last_update = msg_time # (error: 0-40 ms)
w.ball_abs_pos = ball
w.is_ball_abs_pos_from_vision = False
for c, ot in zip(players_combs, group):
# handle oneself case
if ot.is_self:
# the ball's position has a fair amount of noise, whether seen by us or other players
# but our self-locatization mechanism is usually much better than how others perceive us
if r.loc_last_update < ago110ms: # so we wait until we miss 2 visual steps
data = self.get_player_position(c, Radio.TP)
if type(data)==tuple:
x,y,is_down = data
r.loc_head_position[:2] = x,y # z is kept unchanged
r.loc_head_position_last_update = msg_time
r.radio_fallen_state = is_down
r.radio_last_update = msg_time
continue
# do not update if other robot was recently seen
if ot.state_last_update >= ago40ms:
continue
info = Radio.TP if ot.is_teammate else Radio.OP
data = self.get_player_position(c, info)
if type(data)==tuple:
x,y,is_down = data
p = np.array([x,y])
if ot.state_abs_pos is not None: # update the x & y components of the velocity
time_diff = (msg_time - ot.state_last_update) / 1000
velocity = np.append( (p - ot.state_abs_pos[:2]) / time_diff, 0) # v.z = 0
vel_diff = velocity - ot.state_filtered_velocity
if np.linalg.norm(vel_diff) < 4: # otherwise assume it was beamed
ot.state_filtered_velocity /= (ot.vel_decay,ot.vel_decay,1) # neutralize decay (except in the z-axis)
ot.state_filtered_velocity += ot.vel_filter * vel_diff
ot.state_fallen = is_down
ot.state_last_update = msg_time
ot.state_body_parts_abs_pos = {"head":p}
ot.state_abs_pos = p
ot.state_horizontal_dist = np.linalg.norm(p - r.loc_head_position[:2])
ot.state_ground_area = (p, 0.3 if is_down else 0.2) # not very precise, but we cannot see the robot

285
communication/Server_Comm.py Normal file
View File

@@ -0,0 +1,285 @@
from communication.World_Parser import World_Parser
from itertools import count
from select import select
from sys import exit
from world.World import World
import socket
import time
class Server_Comm():
monitor_socket = None
def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, robot_type:int, team_name:str,
world_parser:World_Parser, world:World, other_players, wait_for_server=True) -> None:
self.BUFFER_SIZE = 8192
self.rcv_buff = bytearray(self.BUFFER_SIZE)
self.send_buff = []
self.world_parser = world_parser
self.unum = unum
# During initialization, it's not clear whether we are on the left or right side
self._unofficial_beam_msg_left = "(agent (unum " + str(unum) + ") (team Left) (move "
self._unofficial_beam_msg_right = "(agent (unum " + str(unum) + ") (team Right) (move "
self.world = world
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM )
if wait_for_server: print("Waiting for server at ", host, ":", agent_port, sep="",end=".",flush=True)
while True:
try:
self.socket.connect((host, agent_port))
print(end=" ")
break
except ConnectionRefusedError:
if not wait_for_server:
print("Server is down. Closing...")
exit()
time.sleep(1)
print(".",end="",flush=True)
print("Connected agent", unum, self.socket.getsockname())
self.send_immediate(b'(scene rsg/agent/nao/nao_hetero.rsg ' + str(robot_type).encode() + b')')
self._receive_async(other_players, True)
self.send_immediate(b'(init (unum '+ str(unum).encode() + b') (teamname '+ team_name.encode() + b'))')
self._receive_async(other_players, False)
# Repeat to guarantee that team side information is received
for _ in range(3):
# Eliminate advanced step by changing syn order (rcssserver3d protocol bug, usually seen for player 11)
self.send_immediate(b'(syn)') #if this syn is not needed, it will be discarded by the server
for p in other_players:
p.scom.send_immediate(b'(syn)')
for p in other_players:
p.scom.receive()
self.receive()
if world.team_side_is_left == None:
print("\nError: server did not return a team side! Check server terminal!")
exit()
# Monitor socket is shared by all agents on the same thread
if Server_Comm.monitor_socket is None and monitor_port is not None:
print("Connecting to server's monitor port at ", host, ":", monitor_port, sep="",end=".",flush=True)
Server_Comm.monitor_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM )
Server_Comm.monitor_socket.connect((host, monitor_port))
print("Done!")
def _receive_async(self, other_players, first_pass) -> None:
'''Private function that receives asynchronous information during the initialization'''
if not other_players:
self.receive()
return
self.socket.setblocking(0)
if first_pass: print("Async agent",self.unum,"initialization", end="", flush=True)
while True:
try:
print(".",end="",flush=True)
self.receive()
break
except:
pass
for p in other_players:
p.scom.send_immediate(b'(syn)')
for p in other_players:
p.scom.receive()
self.socket.setblocking(1)
if not first_pass: print("Done!")
def receive(self, update=True):
for i in count(): # parse all messages and perform value updates, but heavy computation is only done once at the end
try:
if self.socket.recv_into(self.rcv_buff, nbytes=4) != 4: raise ConnectionResetError()
msg_size = int.from_bytes(self.rcv_buff[:4], byteorder='big', signed=False)
if self.socket.recv_into(self.rcv_buff, nbytes=msg_size, flags=socket.MSG_WAITALL) != msg_size: raise ConnectionResetError()
except ConnectionResetError:
print("\nError: socket was closed by rcssserver3d!")
exit()
self.world_parser.parse(self.rcv_buff[:msg_size])
if len(select([self.socket],[],[], 0.0)[0]) == 0: break
if update:
if i==1: self.world.log( "Server_Comm.py: The agent lost 1 packet! Is syncmode enabled?")
if i>1: self.world.log(f"Server_Comm.py: The agent lost {i} consecutive packets! Is syncmode disabled?")
self.world.update()
if len(select([self.socket],[],[], 0.0)[0]) != 0:
self.world.log("Server_Comm.py: Received a new packet while on world.update()!")
self.receive()
def send_immediate(self, msg:bytes) -> None:
''' Commit and send immediately '''
try:
self.socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) #Add message length in the first 4 bytes
except BrokenPipeError:
print("\nError: socket was closed by rcssserver3d!")
exit()
def send(self) -> None:
''' Send all committed messages '''
if len(select([self.socket],[],[], 0.0)[0]) == 0:
self.send_buff.append(b'(syn)')
self.send_immediate( b''.join(self.send_buff) )
else:
self.world.log("Server_Comm.py: Received a new packet while thinking!")
self.send_buff = [] #clear buffer
def commit(self, msg:bytes) -> None:
assert type(msg) == bytes, "Message must be of type Bytes!"
self.send_buff.append(msg)
def commit_and_send(self, msg:bytes = b'') -> None:
self.commit(msg)
self.send()
def clear_buffer(self) -> None:
self.send_buff = []
def commit_announcement(self, msg:bytes) -> None:
'''
Say something to every player on the field.
Maximum 20 characters, ascii between 0x20, 0x7E except ' ', '(', ')'
Accepted: letters+numbers+symbols: !"#$%&'*+,-./:;<=>?@[\]^_`{|}~
Message range: 50m (the field is 36m diagonally, so ignore this limitation)
A player can only hear a teammate's message every 2 steps (0.04s)
This ability exists independetly for messages from both teams
(i.e. our team cannot spam the other team to block their messages)
Messages from oneself are always heard
'''
assert len(msg) <= 20 and type(msg) == bytes
self.commit(b'(say ' + msg + b')')
def commit_pass_command(self) -> None:
'''
Issue a pass command:
Conditions:
- The current playmode is PlayOn
- The agent is near the ball (default 0.5m)
- No opponents are near the ball (default 1m)
- The ball is stationary (default <0.05m/s)
- A certain amount of time has passed between pass commands
'''
self.commit(b'(pass)')
def commit_beam(self, pos2d, rot) -> None:
'''
Official beam command that can be used in-game
This beam is affected by noise (unless it is disabled in the server configuration)
Parameters
----------
pos2d : array_like
Absolute 2D position (negative X is always our half of the field, no matter our side)
rot : `int`/`float`
Player angle in degrees (0 points forward)
'''
assert len(pos2d)==2, "The official beam command accepts only 2D positions!"
self.commit( f"(beam {pos2d[0]} {pos2d[1]} {rot})".encode() )
def unofficial_beam(self, pos3d, rot) -> None:
'''
Unofficial beam - it cannot be used in official matches
Parameters
----------
pos3d : array_like
Absolute 3D position (negative X is always our half of the field, no matter our side)
rot : `int`/`float`
Player angle in degrees (0 points forward)
'''
assert len(pos3d)==3, "The unofficial beam command accepts only 3D positions!"
# there is no need to normalize the angle, the server accepts any angle
if self.world.team_side_is_left:
msg = f"{self._unofficial_beam_msg_left }{ pos3d[0]} { pos3d[1]} {pos3d[2]} {rot-90}))".encode()
else:
msg = f"{self._unofficial_beam_msg_right}{-pos3d[0]} {-pos3d[1]} {pos3d[2]} {rot+90}))".encode()
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def unofficial_kill_sim(self) -> None:
''' Unofficial kill simulator command '''
msg = b'(killsim)'
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def unofficial_move_ball(self, pos3d, vel3d=(0,0,0)) -> None:
'''
Unofficial command to move ball
info: ball radius = 0.042m
Parameters
----------
pos3d : array_like
Absolute 3D position (negative X is always our half of the field, no matter our side)
vel3d : array_like
Absolute 3D velocity (negative X is always our half of the field, no matter our side)
'''
assert len(pos3d)==3 and len(vel3d)==3, "To move the ball we need a 3D position and velocity"
if self.world.team_side_is_left:
msg = f"(ball (pos { pos3d[0]} { pos3d[1]} {pos3d[2]}) (vel { vel3d[0]} { vel3d[1]} {vel3d[2]}))".encode()
else:
msg = f"(ball (pos {-pos3d[0]} {-pos3d[1]} {pos3d[2]}) (vel {-vel3d[0]} {-vel3d[1]} {vel3d[2]}))".encode()
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def unofficial_set_game_time(self, time_in_s : float) -> None:
'''
Unofficial command to set the game time
e.g. unofficial_set_game_time(68.78)
Parameters
----------
time_in_s : float
Game time in seconds
'''
msg = f"(time {time_in_s})".encode()
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def unofficial_set_play_mode(self, play_mode : str) -> None:
'''
Unofficial command to set the play mode
e.g. unofficial_set_play_mode("PlayOn")
Parameters
----------
play_mode : str
Play mode
'''
msg = f"(playMode {play_mode})".encode()
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def unofficial_kill_player(self, unum : int, team_side_is_left : bool) -> None:
'''
Unofficial command to kill specific player
Parameters
----------
unum : int
Uniform number
team_side_is_left : bool
True if player to kill belongs to left team
'''
msg = f"(kill (unum {unum}) (team {'Left' if team_side_is_left else 'Right'}))".encode()
self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg )
def close(self, close_monitor_socket = False):
''' Close agent socket, and optionally the monitor socket (shared by players running on the same thread) '''
self.socket.close()
if close_monitor_socket and Server_Comm.monitor_socket is not None:
Server_Comm.monitor_socket.close()
Server_Comm.monitor_socket = None

430
communication/World_Parser.py Normal file
View File

@@ -0,0 +1,430 @@
from math_ops.Math_Ops import Math_Ops as M
from world.Robot import Robot
from world.World import World
import math
import numpy as np
class World_Parser():
def __init__(self, world:World, hear_callback) -> None:
self.LOG_PREFIX = "World_Parser.py: "
self.world = world
self.hear_callback = hear_callback
self.exp = None
self.depth = None
self.LEFT_SIDE_FLAGS = {b'F2L':(-15,-10,0),
b'F1L':(-15,+10,0),
b'F2R':(+15,-10,0),
b'F1R':(+15,+10,0),
b'G2L':(-15,-1.05,0.8),
b'G1L':(-15,+1.05,0.8),
b'G2R':(+15,-1.05,0.8),
b'G1R':(+15,+1.05,0.8)} #mapping between flag names and their corrected location, when playing on the left side
self.RIGHT_SIDE_FLAGS = {b'F2L':(+15,+10,0),
b'F1L':(+15,-10,0),
b'F2R':(-15,+10,0),
b'F1R':(-15,-10,0),
b'G2L':(+15,+1.05,0.8),
b'G1L':(+15,-1.05,0.8),
b'G2R':(-15,+1.05,0.8),
b'G1R':(-15,-1.05,0.8)}
self.play_mode_to_id = None
self.LEFT_PLAY_MODE_TO_ID = {"KickOff_Left":World.M_OUR_KICKOFF, "KickIn_Left":World.M_OUR_KICK_IN, "corner_kick_left":World.M_OUR_CORNER_KICK,
"goal_kick_left":World.M_OUR_GOAL_KICK, "free_kick_left":World.M_OUR_FREE_KICK, "pass_left":World.M_OUR_PASS,
"direct_free_kick_left": World.M_OUR_DIR_FREE_KICK, "Goal_Left": World.M_OUR_GOAL, "offside_left": World.M_OUR_OFFSIDE,
"KickOff_Right":World.M_THEIR_KICKOFF, "KickIn_Right":World.M_THEIR_KICK_IN, "corner_kick_right":World.M_THEIR_CORNER_KICK,
"goal_kick_right":World.M_THEIR_GOAL_KICK, "free_kick_right":World.M_THEIR_FREE_KICK, "pass_right":World.M_THEIR_PASS,
"direct_free_kick_right": World.M_THEIR_DIR_FREE_KICK, "Goal_Right": World.M_THEIR_GOAL, "offside_right": World.M_THEIR_OFFSIDE,
"BeforeKickOff": World.M_BEFORE_KICKOFF, "GameOver": World.M_GAME_OVER, "PlayOn": World.M_PLAY_ON }
self.RIGHT_PLAY_MODE_TO_ID = {"KickOff_Left":World.M_THEIR_KICKOFF, "KickIn_Left":World.M_THEIR_KICK_IN, "corner_kick_left":World.M_THEIR_CORNER_KICK,
"goal_kick_left":World.M_THEIR_GOAL_KICK, "free_kick_left":World.M_THEIR_FREE_KICK, "pass_left":World.M_THEIR_PASS,
"direct_free_kick_left": World.M_THEIR_DIR_FREE_KICK, "Goal_Left": World.M_THEIR_GOAL, "offside_left": World.M_THEIR_OFFSIDE,
"KickOff_Right":World.M_OUR_KICKOFF, "KickIn_Right":World.M_OUR_KICK_IN, "corner_kick_right":World.M_OUR_CORNER_KICK,
"goal_kick_right":World.M_OUR_GOAL_KICK, "free_kick_right":World.M_OUR_FREE_KICK, "pass_right":World.M_OUR_PASS,
"direct_free_kick_right": World.M_OUR_DIR_FREE_KICK, "Goal_Right": World.M_OUR_GOAL, "offside_right": World.M_OUR_OFFSIDE,
"BeforeKickOff": World.M_BEFORE_KICKOFF, "GameOver": World.M_GAME_OVER, "PlayOn": World.M_PLAY_ON }
def find_non_digit(self,start):
while True:
if (self.exp[start] < ord('0') or self.exp[start] > ord('9')) and self.exp[start] != ord('.'): return start
start+=1
def find_char(self,start,char):
while True:
if self.exp[start] == char : return start
start+=1
def read_float(self, start):
if self.exp[start:start+3] == b'nan': return float('nan'), start+3 #handle nan values (they exist)
end = self.find_non_digit(start+1) #we assume the first one is a digit or minus sign
try:
retval = float(self.exp[start:end])
except:
self.world.log(f"{self.LOG_PREFIX}String to float conversion failed: {self.exp[start:end]} at msg[{start},{end}], \nMsg: {self.exp.decode()}")
retval = 0
return retval, end
def read_int(self, start):
end = self.find_non_digit(start+1) #we assume the first one is a digit or minus sign
return int(self.exp[start:end]), end
def read_bytes(self, start):
end = start
while True:
if self.exp[end] == ord(' ') or self.exp[end] == ord(')'): break
end+=1
return self.exp[start:end], end
def read_str(self, start):
b, end = self.read_bytes(start)
return b.decode(), end
def get_next_tag(self, start):
min_depth = self.depth
while True:
if self.exp[start] == ord(")") : #monitor xml element depth
self.depth -= 1
if min_depth > self.depth: min_depth = self.depth
elif self.exp[start] == ord("(") : break
start+=1
if start >= len(self.exp): return None, start, 0
self.depth += 1
start += 1
end = self.find_char(start, ord(" "))
return self.exp[start:end], end, min_depth
def parse(self, exp):
self.exp = exp #used by other member functions
self.depth = 0 #xml element depth
self.world.step += 1
self.world.line_count = 0
self.world.robot.frp = dict()
self.world.flags_posts = dict()
self.world.flags_corners = dict()
self.world.vision_is_up_to_date = False
self.world.ball_is_visible = False
self.world.robot.feet_toes_are_touching = dict.fromkeys(self.world.robot.feet_toes_are_touching, False)
self.world.time_local_ms += World.STEPTIME_MS
for p in self.world.teammates: p.is_visible = False
for p in self.world.opponents: p.is_visible = False
tag, end, _ = self.get_next_tag(0)
while end < len(exp):
if tag==b'time':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
if tag==b'now':
#last_time = self.world.time_server
self.world.time_server, end = self.read_float(end+1)
#Test server time reliability
#increment = self.world.time_server - last_time
#if increment < 0.019: print ("down",last_time,self.world.time_server)
#if increment > 0.021: print ("up",last_time,self.world.time_server)
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'time': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'GS':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
if tag==b'unum':
_, end = self.read_int(end+1) #We already know our unum
elif tag==b'team':
aux, end = self.read_str(end+1)
is_left = bool(aux == "left")
if self.world.team_side_is_left != is_left:
self.world.team_side_is_left = is_left
self.play_mode_to_id = self.LEFT_PLAY_MODE_TO_ID if is_left else self.RIGHT_PLAY_MODE_TO_ID
self.world.draw.set_team_side(not is_left)
self.world.team_draw.set_team_side(not is_left)
elif tag==b'sl':
if self.world.team_side_is_left:
self.world.goals_scored, end = self.read_int(end+1)
else:
self.world.goals_conceded, end = self.read_int(end+1)
elif tag==b'sr':
if self.world.team_side_is_left:
self.world.goals_conceded, end = self.read_int(end+1)
else:
self.world.goals_scored, end = self.read_int(end+1)
elif tag==b't':
self.world.time_game, end = self.read_float(end+1)
elif tag==b'pm':
aux, end = self.read_str(end+1)
if self.play_mode_to_id is not None:
self.world.play_mode = self.play_mode_to_id[aux]
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'GS': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'GYR':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
'''
The gyroscope measures the robot's torso angular velocity (rotation rate vector)
The angular velocity's orientation is given by the right-hand rule.
Original reference frame:
X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+)
New reference frame:
X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+)
'''
if tag==b'n':
pass
elif tag==b'rt':
self.world.robot.gyro[1], end = self.read_float(end+1)
self.world.robot.gyro[0], end = self.read_float(end+1)
self.world.robot.gyro[2], end = self.read_float(end+1)
self.world.robot.gyro[1] *= -1
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'GYR': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'ACC':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
'''
The accelerometer measures the acceleration relative to freefall. It will read zero during any type of free fall.
When at rest relative to the Earth's surface, it will indicate an upwards acceleration of 9.81m/s^2 (in SimSpark).
Original reference frame:
X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+)
New reference frame:
X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+)
'''
if tag==b'n':
pass
elif tag==b'a':
self.world.robot.acc[1], end = self.read_float(end+1)
self.world.robot.acc[0], end = self.read_float(end+1)
self.world.robot.acc[2], end = self.read_float(end+1)
self.world.robot.acc[1] *= -1
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'ACC': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'HJ':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
if tag==b'n':
joint_name, end = self.read_str(end+1)
joint_index = Robot.MAP_PERCEPTOR_TO_INDEX[joint_name]
elif tag==b'ax':
joint_angle, end = self.read_float(end+1)
#Fix symmetry issues 2/4 (perceptors)
if joint_name in Robot.FIX_PERCEPTOR_SET: joint_angle = -joint_angle
old_angle = self.world.robot.joints_position[joint_index]
self.world.robot.joints_speed[joint_index] = (joint_angle - old_angle) / World.STEPTIME * math.pi / 180
self.world.robot.joints_position[joint_index] = joint_angle
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'HJ': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'FRP':
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
'''
The reference frame is used for the contact point and force vector applied to that point
Note: The force vector is applied to the foot, so it usually points up
Original reference frame:
X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+)
New reference frame:
X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+)
'''
if tag==b'n':
foot_toe_id, end = self.read_str(end+1)
self.world.robot.frp[foot_toe_id] = foot_toe_ref = np.empty(6)
self.world.robot.feet_toes_last_touch[foot_toe_id] = self.world.time_local_ms
self.world.robot.feet_toes_are_touching[foot_toe_id] = True
elif tag==b'c':
foot_toe_ref[1], end = self.read_float(end+1)
foot_toe_ref[0], end = self.read_float(end+1)
foot_toe_ref[2], end = self.read_float(end+1)
foot_toe_ref[1] *= -1
elif tag==b'f':
foot_toe_ref[4], end = self.read_float(end+1)
foot_toe_ref[3], end = self.read_float(end+1)
foot_toe_ref[5], end = self.read_float(end+1)
foot_toe_ref[4] *= -1
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'FRP': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'See':
self.world.vision_is_up_to_date = True
self.world.vision_last_update = self.world.time_local_ms
while True:
tag, end, min_depth = self.get_next_tag(end)
if min_depth == 0: break
tag_bytes = bytes(tag) #since bytearray is not hashable, it cannot be used as key for dictionaries
if tag==b'G1R' or tag==b'G2R' or tag==b'G1L' or tag==b'G2L':
_, end, _ = self.get_next_tag(end)
c1, end = self.read_float(end+1)
c2, end = self.read_float(end+1)
c3, end = self.read_float(end+1)
aux = self.LEFT_SIDE_FLAGS[tag_bytes] if self.world.team_side_is_left else self.RIGHT_SIDE_FLAGS[tag_bytes]
self.world.flags_posts[aux] = (c1,c2,c3)
elif tag==b'F1R' or tag==b'F2R' or tag==b'F1L' or tag==b'F2L':
_, end, _ = self.get_next_tag(end)
c1, end = self.read_float(end+1)
c2, end = self.read_float(end+1)
c3, end = self.read_float(end+1)
aux = self.LEFT_SIDE_FLAGS[tag_bytes] if self.world.team_side_is_left else self.RIGHT_SIDE_FLAGS[tag_bytes]
self.world.flags_corners[aux] = (c1,c2,c3)
elif tag==b'B':
_, end, _ = self.get_next_tag(end)
self.world.ball_rel_head_sph_pos[0], end = self.read_float(end+1)
self.world.ball_rel_head_sph_pos[1], end = self.read_float(end+1)
self.world.ball_rel_head_sph_pos[2], end = self.read_float(end+1)
self.world.ball_rel_head_cart_pos = M.deg_sph2cart(self.world.ball_rel_head_sph_pos)
self.world.ball_is_visible = True
self.world.ball_last_seen = self.world.time_local_ms
elif tag==b'mypos':
self.world.robot.cheat_abs_pos[0], end = self.read_float(end+1)
self.world.robot.cheat_abs_pos[1], end = self.read_float(end+1)
self.world.robot.cheat_abs_pos[2], end = self.read_float(end+1)
elif tag==b'myorien':
self.world.robot.cheat_ori, end = self.read_float(end+1)
elif tag==b'ballpos':
c1, end = self.read_float(end+1)
c2, end = self.read_float(end+1)
c3, end = self.read_float(end+1)
self.world.ball_cheat_abs_vel[0] = (c1 - self.world.ball_cheat_abs_pos[0]) / World.VISUALSTEP
self.world.ball_cheat_abs_vel[1] = (c2 - self.world.ball_cheat_abs_pos[1]) / World.VISUALSTEP
self.world.ball_cheat_abs_vel[2] = (c3 - self.world.ball_cheat_abs_pos[2]) / World.VISUALSTEP
self.world.ball_cheat_abs_pos[0] = c1
self.world.ball_cheat_abs_pos[1] = c2
self.world.ball_cheat_abs_pos[2] = c3
elif tag==b'P':
while True:
previous_depth = self.depth
previous_end = end
tag, end, min_depth = self.get_next_tag(end)
if min_depth < 2: #if =1 we are still inside 'See', if =0 we are already outside 'See'
end = previous_end #The "P" tag is special because it's the only variable particle inside 'See'
self.depth = previous_depth
break # we restore the previous tag, and let 'See' handle it
if tag==b'team':
player_team, end = self.read_str(end+1)
is_teammate = bool(player_team == self.world.team_name)
if self.world.team_name_opponent is None and not is_teammate: #register opponent team name
self.world.team_name_opponent = player_team
elif tag==b'id':
player_id, end = self.read_int(end+1)
player = self.world.teammates[player_id-1] if is_teammate else self.world.opponents[player_id-1]
player.body_parts_cart_rel_pos = dict() #reset seen body parts
player.is_visible = True
elif tag==b'llowerarm' or tag==b'rlowerarm' or tag==b'lfoot' or tag==b'rfoot' or tag==b'head':
tag_str = tag.decode()
_, end, _ = self.get_next_tag(end)
c1, end = self.read_float(end+1)
c2, end = self.read_float(end+1)
c3, end = self.read_float(end+1)
if is_teammate:
self.world.teammates[player_id-1].body_parts_sph_rel_pos[tag_str] = (c1,c2,c3)
self.world.teammates[player_id-1].body_parts_cart_rel_pos[tag_str] = M.deg_sph2cart((c1,c2,c3))
else:
self.world.opponents[player_id-1].body_parts_sph_rel_pos[tag_str] = (c1,c2,c3)
self.world.opponents[player_id-1].body_parts_cart_rel_pos[tag_str] = M.deg_sph2cart((c1,c2,c3))
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'P': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'L':
l = self.world.lines[self.world.line_count]
_, end, _ = self.get_next_tag(end)
l[0], end = self.read_float(end+1)
l[1], end = self.read_float(end+1)
l[2], end = self.read_float(end+1)
_, end, _ = self.get_next_tag(end)
l[3], end = self.read_float(end+1)
l[4], end = self.read_float(end+1)
l[5], end = self.read_float(end+1)
if np.isnan(l).any():
self.world.log(f"{self.LOG_PREFIX}Received field line with NaNs {l}")
else:
self.world.line_count += 1 #accept field line if there are no NaNs
else:
self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'see': {tag} at {end}, \nMsg: {exp.decode()}")
elif tag==b'hear':
team_name, end = self.read_str(end+1)
if team_name == self.world.team_name: # discard message if it's not from our team
timestamp, end = self.read_float(end+1)
if self.exp[end+1] == ord('s'): # this message was sent by oneself
direction, end = "self", end+5
else: # this message was sent by teammate
direction, end = self.read_float(end+1)
msg, end = self.read_bytes(end+1)
self.hear_callback(msg, direction, timestamp)
tag, end, _ = self.get_next_tag(end)
else:
self.world.log(f"{self.LOG_PREFIX}Unknown root tag: {tag} at {end}, \nMsg: {exp.decode()}")
tag, end, min_depth = self.get_next_tag(end)

BIN
communication/__pycache__/Radio.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
communication/__pycache__/Server_Comm.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
communication/__pycache__/World_Parser.cpython-313.pyc Normal file
View File

Binary file not shown.