Ejemplo n.º 1
0
    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()
Ejemplo n.º 2
0
    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()
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
	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 = []
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
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)