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 test_initially_moving_to_middle(self):
zone = self.world_left.pitch.zones[2][0]
target_x, target_y = self.strategy_left._get_shooting_coordinates(
self.world_left.our_attacker)
for x, y in zone:
robot = self.get_aimed_robot(
x, y, target_x, target_y, self.world_left.our_attacker.zone)
world = World('left')
self.place_robot(world, world.our_attacker.zone, robot)
strategy = AttackerScoreDynamic(world)
actions = strategy.generate()
left_motor = actions['left_motor']
right_motor = actions['right_motor']
# Both left motor and right motor should be positive or negative.
self.assertEqual(True, left_motor * right_motor > 0)
def test_confuse1_rotation_up(self):
target_x, target_y = self.strategy_left.shooting_pos
aim_x = self.world_left.their_goal.x
aim_y = self.world_left.their_goal.y + self.world_left.their_goal.height / 2 + self.strategy_left.GOAL_CORNER_OFFSET
attacker = self.get_aimed_robot(
target_x, target_y, aim_x, aim_y,
self.world_left.our_attacker.zone)
defender = self.get_aimed_robot(
self.world_left.their_goal.x, self.world_left.their_goal.y,
aim_x, aim_y, self.world_left.their_defender.zone)
world = World('left')
self.place_robot(world, attacker.zone, attacker)
self.place_robot(world, defender.zone, defender)
strategy = AttackerScoreDynamic(world)
actions = strategy.generate()
self.assertTrue(actions['left_motor'] < 0)
self.assertTrue(actions['right_motor'] > 0)
def setUp(self):
self.world_left = World('left')
self.strategy_left = AttackerScoreDynamic(self.world_left)
self.world_right = World('right')
self.strategy_right = AttackerScoreDynamic(self.world_right)
def setUp(self):
self.world_left = World('left')
self.strategy_left = DefaultDefenderDefence(self.world_left)
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)
