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):
# Set up Arduino communications.
# self.robot =
self.calib_file = "vision/calibrations/calibrations.json"
self.vision = Vision(self.calib_file)
self.gui = GUI(self.vision.calibrations)
self.task = None
# Set up world
self.world = World(self.calib_file)
# Set up post-processing
self.postprocessing = PostProcessing()
self.wait_for_vision = True
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)
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 Runner(object):
def __init__(self):
# Set up Arduino communications.
# self.robot =
self.calib_file = "vision/calibrations/calibrations.json"
self.vision = Vision(self.calib_file)
self.gui = GUI(self.vision.calibrations)
self.task = None
# Set up world
self.world = World(self.calib_file)
# Set up post-processing
self.postprocessing = PostProcessing()
self.wait_for_vision = True
def run(self):
"""
Constantly updates the vision feed, and positions of our models
"""
counter = 0
timer = time.clock()
# wait 10 seconds for arduino to connect
print("Connecting to Arduino, please wait till confirmation message")
time.sleep(4)
# This asks nicely for goal location, etc
self.initiate_world()
try:
c = True
while c != 27: # the ESC key
if self.task is None:
print("Please enter the task you wish to execute:")
self.task = sys.stdin.readline().strip()
t2 = time.time()
# change of time between frames in seconds
delta_time = t2 - timer
timer = t2
# getting all the data from the world state
data, modified_frame = self.vision.get_world_state()
# update the gui
self.gui.update(delta_time, self.vision.frame, modified_frame, data)
# Update our world with the positions of robot and ball
self.world.update_positions(data)
# Only run the task every 20 cycles, this allows us to catch up with vision
if counter % 21 == 0:
self.task_execution()
key = cv2.waitKey(4) & 0xFF
if key == ord('q'):
break
# self.save_calibrations()
counter += 1
finally:
pass
# self.robot.stop()
def initiate_world(self):
print("Which side are we?")
self.world.our_side = sys.stdin.readline().strip()
if self.world.our_side == "left":
self.world.defender_region.left = 40 # 40, 320
self.world.defender_region.right = 320 # 320, 600
self.world.attacker_region.left = 320 # 40, 320
self.world.attacker_region.right = 600 #320, 600
self.world.our_goal.x = 40 # 600 or 40
self.world.our_goal.y = 235
self.world.their_goal.x = 600 # 40 or 600
self.world.their_goal.y = 245
else:
self.world.defender_region.left = 320 # 40, 320
self.world.defender_region.right = 600 # 320, 600
self.world.attacker_region.left = 40 # 40, 320
self.world.attacker_region.right = 320 #320, 600
self.world.our_goal.x = 600 # 600 or 40
self.world.our_goal.y = 235
self.world.their_goal.x = 40 # 40 or 600
self.world.their_goal.y = 245
self.world.our_robot.team_color = "blue"
self.world.teammate.team_color = "blue"
self.world.their_attacker.team_color = "yellow"
self.world.their_defender.team_color = "yellow"
self.world.our_robot.group_color = "green"
self.world.teammate.group_color = "pink"
self.world.their_attacker.group_color = "pink"
self.world.their_defender.group_color = "green"
self.world.safety_padding = 25
self.world.pitch_boundary_left = 40
self.world.pitch_boundary_right = 600
self.world.pitch_boundary_bottom = 450
self.world.pitch_boundary_top = 30
def task_execution(self):
"""
Executes the current task
"""
# Only execute a task if the robot isn't currently in the middle of doing one
print ("Task: ", self.task)
task_to_execute = None
if self.task == 'task_vision':
task_to_execute = self.world.task.task_vision
if self.task == 'task_move_to_ball':
task_to_execute = self.world.task.task_move_to_ball
if self.task == 'task_kick_ball_in_goal':
task_to_execute = self.world.task.task_kick_ball_in_goal
if self.task == 'task_move_and_grab_ball':
task_to_execute = self.world.task.task_move_and_grab_ball
if self.task == 'task_rotate_and_grab':
task_to_execute = self.world.task.task_rotate_and_grab
if self.task == 'task_grab_rotate_kick':
task_to_execute = self.world.task.task_grab_rotate_kick
if self.task == 'task_defender':
task_to_execute = self.world.task.task_defender
if self.task == 'task_defender_kick_off':
task_to_execute = self.world.task.task_defender_kick_off
if self.task == 'task_attacker':
task_to_execute = self.world.task.task_attacker
if self.task == 'task_attacker_kick_off':
task_to_execute = self.world.task.task_attacker_kick_off
if self.task == 'task_penalty':
task_to_execute = self.world.task.task_penalty
if self.task == 'task_goalie':
task_to_execute = self.world.task.task_penalty_goalie
# if there's a task to do, let's try it
if self.task:
# if it's executed fine, then we've completed the task. otherwise we're going to loop round and try again
if task_to_execute():
self.task = None
print("Task: COMPLETED")
def save_calibrations(self):
dump_calibrations(self.vision.calibrations, self.calib_file)
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)