You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
103 lines
4.8 KiB
Python
103 lines
4.8 KiB
Python
from agent.Agent import Agent as Agent
|
|
from cpp.localization import localization
|
|
from math_ops.Math_Ops import Math_Ops as M
|
|
from scripts.commons.Script import Script
|
|
from world.commons.Draw import Draw
|
|
from world.commons.Other_Robot import Other_Robot
|
|
|
|
|
|
class Localization():
|
|
|
|
def __init__(self,script:Script) -> None:
|
|
self.script = script
|
|
|
|
|
|
def execute(self):
|
|
|
|
a = self.script.args
|
|
d = self.draw = Draw(True, 0, a.i, 32769) # using independent draw object so that the internal agent drawings can be disabled
|
|
|
|
# Args: Server IP, Agent Port, Monitor Port, Uniform No., Team name, Enable Log, Enable Draw
|
|
self.script.batch_create(Agent, ((a.i,a.p,a.m,1,a.t,False,False),)) # one teammate (dummy goalkeeper without communication)
|
|
self.script.batch_create(Agent, ((a.i,a.p,a.m,5,"Opponent",False,False),)) # one opponent
|
|
self.script.batch_create(Agent, ((a.i,a.p,a.m,9,a.t,False,False),)) # one main agent (the one who draws its world)
|
|
|
|
# Beam dummy goalkeeper
|
|
self.script.batch_unofficial_beam( ((-14,0,0.5,0),), slice(0,1))
|
|
|
|
p : Agent = self.script.players[-1] # p identifies the main agent
|
|
p.scom.unofficial_set_play_mode("PlayOn")
|
|
|
|
# Execute
|
|
while True:
|
|
self.script.batch_commit_and_send(slice(0,1)) # dummy agent does not think
|
|
self.script.batch_execute_agent(slice(1,None)) # execute normal agents
|
|
self.script.batch_receive(slice(0,1), False) # receive & don't update dummy's world state (to save cpu resources)
|
|
self.script.batch_receive(slice(1,None)) # receive & update world state
|
|
|
|
if p.world.vision_is_up_to_date:
|
|
if p.world.robot.loc_is_up_to_date: # localization will draw the world of the last agent to be executed
|
|
localization.print_python_data() # print data received by the localization module
|
|
localization.draw_visible_elements(not p.world.team_side_is_left) # draw visible elements
|
|
localization.print_report() # print report with stats
|
|
print("\nPress ctrl+c to return.")
|
|
d.circle( p.world.ball_abs_pos, 0.1,6,Draw.Color.purple_magenta,"world", False)
|
|
else:
|
|
d.annotation( p.world.robot.cheat_abs_pos, "Not enough visual data!", Draw.Color.red,"world", False)
|
|
|
|
for o in p.world.teammates:
|
|
if o.state_last_update != 0 and not o.is_self: # skip if other robot was not yet seen
|
|
self._draw_other_robot(p, o, Draw.Color.white)
|
|
|
|
for o in p.world.opponents:
|
|
if o.state_last_update != 0: # skip if other robot was not yet seen
|
|
self._draw_other_robot(p, o, Draw.Color.red)
|
|
|
|
d.flush("world")
|
|
|
|
|
|
|
|
def _draw_other_robot(self, p:Agent, o:Other_Robot, team_color):
|
|
#p - player that sees
|
|
#o - other robot (player that is seen)
|
|
|
|
d = self.draw
|
|
white = Draw.Color.white
|
|
green = Draw.Color.green_light
|
|
gray = Draw.Color.gray_20
|
|
|
|
time_diff = p.world.time_local_ms - o.state_last_update
|
|
if time_diff > 0:
|
|
white = Draw.Color.gray_40
|
|
green = Draw.Color.get(107, 139, 107)
|
|
gray = Draw.Color.gray_50
|
|
|
|
#orientation
|
|
if len(o.state_abs_pos)==3:
|
|
line_tip = o.state_abs_pos + (0.5*M.deg_cos(o.state_orientation),0.5*M.deg_sin(o.state_orientation),0)
|
|
d.line( o.state_abs_pos, line_tip, 3, white, "world", False)
|
|
else:
|
|
temp_pos = M.to_3d(o.state_abs_pos, 0.3)
|
|
line_tip = temp_pos + (0.5*M.deg_cos(o.state_orientation),0.5*M.deg_sin(o.state_orientation),0)
|
|
d.line( temp_pos, line_tip, 3, Draw.Color.yellow, "world", False)
|
|
|
|
#body parts
|
|
for pos in o.state_body_parts_abs_pos.values():
|
|
d.sphere( pos, 0.07, green,"world", False)
|
|
|
|
#player ground area
|
|
d.circle( o.state_ground_area[0], o.state_ground_area[1], 6, team_color,"world", False)
|
|
|
|
#distance
|
|
midpoint = (o.state_abs_pos[0:2] + p.world.robot.loc_head_position[0:2])/2
|
|
d.line( o.state_abs_pos[0:2], p.world.robot.loc_head_position[0:2], 1, gray, "world", False)
|
|
d.annotation( midpoint, f'{o.state_horizontal_dist:.2f}m', white, "world", False)
|
|
|
|
#velocity
|
|
arrow_tip = o.state_abs_pos[0:2] + o.state_filtered_velocity[0:2]
|
|
d.arrow( o.state_abs_pos[0:2], arrow_tip, 0.2, 4, green, "world", False)
|
|
|
|
#state
|
|
state_color = white if not o.state_fallen else Draw.Color.yellow
|
|
d.annotation( (o.state_abs_pos[0],o.state_abs_pos[1],1),
|
|
f"({o.unum}) {'Fallen' if o.state_fallen else 'Normal'}", state_color, "world", False) |