def __init__(self, pitch, ourTeamColor='yellow', otherTeamColor='blue', ballColor='red', record = False, extras = False):
self.pitch = pitch
self.camera = Camera(pitch=pitch)
self.calibration = tools.get_colors(pitch)
self.tracker = MyTracker(self.calibration, self.pitch, extras=extras)
#self.tracker = MyTracker([ourTeamColor, otherTeamColor, ballColor, 'pink', 'bright_green'], self.calibration)
self.points = {} # point dictionary for tracked colors
self.GUI_name = "Killer Robot App"
self.calibration_gui= CameraCalibrationGUI(calibration=self.calibration , name=self.GUI_name)
self.record = record
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up the Arduino communications
self.arduino = Arduino(comm_port, 115200, 1, comms)
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch)
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
self.color = color
self.side = our_side
self.preprocessing = Preprocessing()
self.attacker = Attacker_Controller()
self.defender = Defender_Controller()
def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ["yellow", "blue"]
assert our_side in ["left", "right"]
self.pitch = pitch
# Set up the Arduino communications
self.arduino = Arduino(comm_port, 115200, 1, comms)
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch,
color=color,
our_side=our_side,
frame_shape=frame.shape,
frame_center=center_point,
calibration=self.calibration,
)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch)
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
self.color = color
self.side = our_side
self.preprocessing = Preprocessing()
self.attacker = Attacker_Controller()
self.defender = Defender_Controller()
def __init__(self, pitch, color, our_side, video_port=0):
"""
Entry point for the SDP system.
Params:
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] colour The colour of our teams plates
[string] our_side the side we're on - 'left' or 'right'
[int] video_port port number for the camera
Fields
pitch
camera
calibration
vision
postporcessing
color
side
preprocessing
model_positions
regular_positions
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=pitch)
self.frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(self.frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=self.frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up preprocessing and postprocessing
self.postprocessing = Postprocessing()
self.preprocessing = Preprocessing()
self.color = color
self.side = our_side
self.frameQueue = []
class Controller:
"""
Primary source of robot control. Ties vision and planning together.
"""
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up the Arduino communications
self.arduino = Arduino(comm_port, 115200, 1, comms)
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch)
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
self.color = color
self.side = our_side
self.preprocessing = Preprocessing()
self.attacker = Attacker_Controller()
self.defender = Defender_Controller()
def wow(self):
"""
Ready your sword, here be dragons.
"""
counter = 1L
timer = time.clock()
try:
c = True
while c != 27: # the ESC key
frame = self.camera.get_frame()
pre_options = self.preprocessing.options
# Apply preprocessing methods toggled in the UI
preprocessed = self.preprocessing.run(frame, pre_options)
frame = preprocessed['frame']
if 'background_sub' in preprocessed:
cv2.imshow('bg sub', preprocessed['background_sub'])
# Find object positions
# model_positions have their y coordinate inverted
model_positions, regular_positions = self.vision.locate(frame)
model_positions = self.postprocessing.analyze(model_positions)
# Find appropriate action
self.planner.update_world(model_positions)
attacker_actions = self.planner.plan('attacker')
defender_actions = self.planner.plan('defender')
if self.attacker is not None:
self.attacker.execute(self.arduino, attacker_actions)
if self.defender is not None:
self.defender.execute(self.arduino, defender_actions)
# Information about the grabbers from the world
grabbers = {
'our_defender': self.planner._world.our_defender.catcher_area,
'our_attacker': self.planner._world.our_attacker.catcher_area
}
# Information about states
attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state)
defenderState = (self.planner.defender_state, self.planner.defender_strat_state)
# Use 'y', 'b', 'r' to change color.
c = waitKey(2) & 0xFF
actions = []
fps = float(counter) / (time.clock() - timer)
# Draw vision content and actions
self.GUI.draw(
frame, model_positions, actions, regular_positions, fps, attackerState,
defenderState, attacker_actions, defender_actions, grabbers,
our_color=self.color, our_side=self.side, key=c, preprocess=pre_options)
counter += 1
except:
if self.defender is not None:
self.defender.shutdown(self.arduino)
if self.attacker is not None:
self.attacker.shutdown(self.arduino)
raise
finally:
# Write the new calibrations to a file.
tools.save_colors(self.pitch, self.calibration)
if self.attacker is not None:
self.attacker.shutdown(self.arduino)
if self.defender is not None:
self.defender.shutdown(self.arduino)
class VisionWrapper:
"""
Class that handles vision
"""
def __init__(self, pitch, color, our_side, video_port=0):
"""
Entry point for the SDP system.
Params:
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] colour The colour of our teams plates
[string] our_side the side we're on - 'left' or 'right'
[int] video_port port number for the camera
Fields
pitch
camera
calibration
vision
postporcessing
color
side
preprocessing
model_positions
regular_positions
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=pitch)
self.frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(self.frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=self.frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up preprocessing and postprocessing
self.postprocessing = Postprocessing()
self.preprocessing = Preprocessing()
self.color = color
self.side = our_side
self.frameQueue = []
def update(self):
"""
Gets this frame's positions from the vision system.
"""
self.frame = self.camera.get_frame()
# Apply preprocessing methods toggled in the UI
self.preprocessed = self.preprocessing.run(self.frame, self.preprocessing.options)
self.frame = self.preprocessed['frame']
if 'background_sub' in self.preprocessed:
cv2.imshow('bg sub', self.preprocessed['background_sub'])
# Find object positions
# model_positions have their y coordinate inverted
self.model_positions, self.regular_positions = self.vision.locate(self.frame)
self.model_positions = self.postprocessing.analyze(self.model_positions)
#self.model_positions = self.averagePositions(3, self.model_positions)
def averagePositions(self, frames, positions_in):
"""
:param frames: number of frames to average
:param positions_in: positions for the current frame
:return: averaged positions
"""
validFrames = self.frameQueue.__len__() + 1
positions_out = deepcopy(positions_in)
# Check that the incoming positions have legal values
for obj in positions_out.items():
if (positions_out[obj[0]].velocity is None):
positions_out[obj[0]].velocity = 0
if positions_out[obj[0]].x is None:
positions_out[obj[0]].x = 0
if positions_out[obj[0]].y is None:
positions_out[obj[0]].y = 0
positions_out[obj[0]].angle = positions_in[obj[0]].angle
# Loop over queue
for positions in self.frameQueue:
# Loop over each object in the position dictionary
isFrameValid = True
for obj in positions.items():
# Check if the current object's positions have legal values
if (obj[1].x is None) or (obj[1].y is None) or (obj[1].angle is None) or (obj[1].velocity is None):
isFrameValid = False
else:
positions_out[obj[0]].x += obj[1].x
positions_out[obj[0]].y += obj[1].y
positions_out[obj[0]].velocity += obj[1].velocity
if not isFrameValid and validFrames > 1:
#validFrames -= 1
pass
# Loop over each object in the position dictionary and average the values
for obj in positions_out.items():
positions_out[obj[0]].velocity /= validFrames
positions_out[obj[0]].x /= validFrames
positions_out[obj[0]].y /= validFrames
# If frameQueue is already full then pop the top entry off
if self.frameQueue.__len__() >= frames:
self.frameQueue.pop(0)
# Add our new positions to the end
self.frameQueue.append(positions_in)
return positions_out
def saveCalibrations(self):
tools.save_colors(self.vision.pitch, self.calibration)
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up the Arduino communications
self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600)
time.sleep(0.5)
self.arduino.grabberUp()
self.arduino.grab()
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino)
self.color = color
self.side = our_side
self.timeOfNextAction = 0
self.preprocessing = Preprocessing()
#it doesn't matter whether it is an Attacker or a Defender Controller
self.controller = Attacker_Controller(self.planner._world, self.GUI)
self.robot_action_list = []
class Controller:
"""
Primary source of robot control. Ties vision and planning together.
"""
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ['yellow', 'blue']
assert our_side in ['left', 'right']
self.pitch = pitch
# Set up the Arduino communications
self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600)
time.sleep(0.5)
self.arduino.grabberUp()
self.arduino.grab()
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch, color=color, our_side=our_side,
frame_shape=frame.shape, frame_center=center_point,
calibration=self.calibration)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino)
self.color = color
self.side = our_side
self.timeOfNextAction = 0
self.preprocessing = Preprocessing()
#it doesn't matter whether it is an Attacker or a Defender Controller
self.controller = Attacker_Controller(self.planner._world, self.GUI)
self.robot_action_list = []
def wow(self):
"""
Ready your sword, here be dragons.
"""
counter = 1L
timer = time.clock()
try:
c = True
while c != 27: # the ESC key
frame = self.camera.get_frame()
pre_options = self.preprocessing.options
# Apply preprocessing methods toggled in the UI
preprocessed = self.preprocessing.run(frame, pre_options)
frame = preprocessed['frame']
if 'background_sub' in preprocessed:
cv2.imshow('bg sub', preprocessed['background_sub'])
# Find object positions
# model_positions have their y coordinate inverted
model_positions, regular_positions = self.vision.locate(frame)
model_positions = self.postprocessing.analyze(model_positions)
# Find appropriate action
self.planner.update_world(model_positions)
if time.time() >= self.timeOfNextAction:
if self.robot_action_list == []:
plan = self.planner.plan()
if isinstance(plan, list):
self.robot_action_list = plan
else:
self.robot_action_list = [(plan, 0)]
if self.controller is not None:
self.controller.execute(self.arduino, self)
# Information about the grabbers from the world
grabbers = {
'our_defender': self.planner._world.our_defender.catcher_area,
'our_attacker': self.planner._world.our_attacker.catcher_area
}
# Information about states
robotState = 'test'
# Use 'y', 'b', 'r' to change color.
c = waitKey(2) & 0xFF
actions = []
fps = float(counter) / (time.clock() - timer)
# Draw vision content and actions
self.GUI.draw(
frame, model_positions, actions, regular_positions, fps, robotState,
"we dont need it", '', "we dont need it", grabbers,
our_color='blue', our_side=self.side, key=c, preprocess=pre_options)
counter += 1
if c == ord('a'):
self.arduino.grabberUp()
self.arduino.grab()
except:
if self.controller is not None:
self.controller.shutdown(self.arduino)
raise
finally:
# Write the new calibrations to a file.
tools.save_colors(self.pitch, self.calibration)
if self.controller is not None:
self.controller.shutdown(self.arduino)
class VisionWrapper(object):
def __init__(self, pitch, ourTeamColor='yellow', otherTeamColor='blue', ballColor='red', record = False, extras = False):
self.pitch = pitch
self.camera = Camera(pitch=pitch)
self.calibration = tools.get_colors(pitch)
self.tracker = MyTracker(self.calibration, self.pitch, extras=extras)
#self.tracker = MyTracker([ourTeamColor, otherTeamColor, ballColor, 'pink', 'bright_green'], self.calibration)
self.points = {} # point dictionary for tracked colors
self.GUI_name = "Killer Robot App"
self.calibration_gui= CameraCalibrationGUI(calibration=self.calibration , name=self.GUI_name)
self.record = record
def run(self):
subprocess.call("vision/xawtv.sh")
if (self.record == True):
fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
output_name = 'output_' + str( tools.current_milli_time() ) + '.avi'
out = cv2.VideoWriter(output_name, fourcc, 24, (640,480))
images = []
while(True):
time = tools.current_milli_time()
# Update trackbars
self.calibration_gui.update()
# get updated camera settings
settings = self.calibration_gui.getSettings()
# Update camera settings
self.camera.reset_camera_settings(settings=settings)
# capture frame by frame
frame = self.camera.get_frame()
if (self.record == True):
out.write(frame)
#images.append(frame)
#if len(images) > 2:
# images = images[1:]
image = frame;
#if len(images) == 2:
# image = cv2.addWeighted(images[0], 0.5 ,images[1], 0.5, 0)
# temp2 = cv2.addWeighted(images[2], 0.5 ,images[3], 0.5, 0)
# image = cv2.addWeighted(temp1, 0.5 ,temp2, 0.5, 0)
# Get tracked object points
self.points, modified_frame = self.tracker.get_world_state(image)
# TODO: think about tracked points sometimes jerking arround
# maybe get a threshold of how far of a distance change should
# be accounted
# TODO: implement functions to find robots using points
# dictionary
# TODO: some rudamentary logic can be implemented here
# along with connection to the robot
# Better to have two classes, one for robot logic
# Another for communication handling
World.set_points(self.points)
w = World.get_world()
# Separate field in 6x4 for strategy debugging
for l1 in range(0,640,107):
cv2.line(image, (l1,0), (l1,480), (255,255,255), thickness=1)
for l2 in range(0,480,120):
cv2.line(image, (0,l2), (640,l2), (255,255,255), thickness=1)
for i3 , robot in enumerate(w.robots):
if robot != None:
#print "Angle:" + str(robot.rot)
rads = radians(robot.rot)
x1, y1 = (robot.x_px, robot.y_px)
x2 = int(x1 + cos(rads) * 50);
y2 = int(y1 + sin(rads) * 50);
cv2.arrowedLine(image, (int(x1), int(y1)), (x2,y2), BGR_COMMON[ World.our_team_colour if i3 < 2 else World.oposite_team_colour], 2)
corners = [(int(x1 + cos(rads + radians(45) + 90*pi/180*i)*20) , (int(y1 + sin(rads + radians(45)+ 90*pi/180*i)*20))) for i in range(4)]
cv2.putText (img=image,
text=str(i3),
org=(int(x1-10), int(y1+10)),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1,
color=(255,255,255))
for i2 in range(4):
col = BGR_COMMON[ World.our_primary if i3 % 2 == 0 else World.other_primary]
cv2.line(image, corners[i2] , corners[(i2+1)%4] , col , 2)
if w.ball != None :
ball = World.cm_to_px(w.ball.x , w.ball.y)
ball = (int(ball[0]) , int(ball[1]) )
cv2.circle(image , (ball[0] , ball[1]) , 10 , (255,0,0))
cv2.arrowedLine(image , (ball[0] , ball[1]) ,(int(ball[0] + w.ball.dir[0] ), int(ball[1] + w.ball.dir[1])) ,(255, 0 ,0) , 2)
#cv2.imshow('test', frame)
#cv2.waitKey(0)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# display FPS
time = ((tools.current_milli_time()-time))/1000.0
fps = 1.0 / time
cv2.putText(image, "FPS: "+str(round(fps)), (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, 255)
cv2.imshow(self.GUI_name, image)
# Display the resulting frame
#cv2.imshow('Killer Robot App', image)
# When everything is done, release the capture
out.release()
cv2.destroyAllWindows()
class Controller:
"""
Primary source of robot control. Ties vision and planning together.
"""
def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
*[int] port The camera port to take the feed from
*[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED
power wire
*[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW
power wire
"""
assert pitch in [0, 1]
assert color in ["yellow", "blue"]
assert our_side in ["left", "right"]
self.pitch = pitch
# Set up the Arduino communications
self.arduino = Arduino(comm_port, 115200, 1, comms)
# Set up camera for frames
self.camera = Camera(port=video_port, pitch=self.pitch)
frame = self.camera.get_frame()
center_point = self.camera.get_adjusted_center(frame)
# Set up vision
self.calibration = tools.get_colors(pitch)
self.vision = Vision(
pitch=pitch,
color=color,
our_side=our_side,
frame_shape=frame.shape,
frame_center=center_point,
calibration=self.calibration,
)
# Set up postprocessing for vision
self.postprocessing = Postprocessing()
# Set up main planner
self.planner = Planner(our_side=our_side, pitch_num=self.pitch)
# Set up GUI
self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch)
self.color = color
self.side = our_side
self.preprocessing = Preprocessing()
self.attacker = Attacker_Controller()
self.defender = Defender_Controller()
def wow(self):
"""
Ready your sword, here be dragons.
"""
counter = 1L
timer = time.clock()
try:
c = True
while c != 27: # the ESC key
frame = self.camera.get_frame()
pre_options = self.preprocessing.options
# Apply preprocessing methods toggled in the UI
preprocessed = self.preprocessing.run(frame, pre_options)
frame = preprocessed["frame"]
if "background_sub" in preprocessed:
cv2.imshow("bg sub", preprocessed["background_sub"])
# Find object positions
# model_positions have their y coordinate inverted
model_positions, regular_positions = self.vision.locate(frame)
model_positions = self.postprocessing.analyze(model_positions)
# Find appropriate action
self.planner.update_world(model_positions)
attacker_actions = self.planner.plan("attacker")
defender_actions = self.planner.plan("defender")
if self.attacker is not None:
self.attacker.execute(self.arduino, attacker_actions)
if self.defender is not None:
self.defender.execute(self.arduino, defender_actions)
# Information about the grabbers from the world
grabbers = {
"our_defender": self.planner._world.our_defender.catcher_area,
"our_attacker": self.planner._world.our_attacker.catcher_area,
}
# Information about states
attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state)
defenderState = (self.planner.defender_state, self.planner.defender_strat_state)
# Use 'y', 'b', 'r' to change color.
c = waitKey(2) & 0xFF
actions = []
fps = float(counter) / (time.clock() - timer)
# Draw vision content and actions
self.GUI.draw(
frame,
model_positions,
actions,
regular_positions,
fps,
attackerState,
defenderState,
attacker_actions,
defender_actions,
grabbers,
our_color=self.color,
our_side=self.side,
key=c,
preprocess=pre_options,
)
counter += 1
except:
if self.defender is not None:
self.defender.shutdown(self.arduino)
if self.attacker is not None:
self.attacker.shutdown(self.arduino)
raise
finally:
# Write the new calibrations to a file.
tools.save_colors(self.pitch, self.calibration)
if self.attacker is not None:
self.attacker.shutdown(self.arduino)
if self.defender is not None:
self.defender.shutdown(self.arduino)
