Ejemplo n.º 1
0
 def center_above_bucket(self):
     '''
     centering above bucket
     '''
     self.darknet_client.change_camera("bottom")
     stopwatch = Stopwatch()
     stopwatch.start()
     bbox = self.darknet_client.predict()[0].normalize(480, 480)
     while bbox is None and stopwatch.time() < self.MAX_TIME_SEC:
         bbox = self.darknet_client.predict()[0].normalize(480, 480)
         sleep(0.3)
     if bbox is None:
         self._logger.log("Could not locate bucket")
         return 0
     position_x = bbox.x
     position_y = bbox.y
     Kp = 0.001
     i = 0
     while position_x > self.POSITION_THRESHOLD and position_y > self.POSITION_THRESHOLD:
         self._control.set_lin_velocity(front=position_y * Kp,
                                        right=position_x * Kp)
         bbox = self.darknet_client.predict()[0].normalize(480, 480)
         if bbox is not None:
             position_x = bbox.x
             position_y = bbox.y
         i += 1
         if i == 1000:
             self._logger.log("Could not center above bucket")
             return 0
     return 1
Ejemplo n.º 2
0
    def find_gate(self):
        self._logger.log("finding the gate")
        config = self.config['search']
        MAX_ANG_SPEED = config['max_ang_speed']
        MOVING_AVERAGE_DISCOUNT = config['moving_avg_discount']
        CONFIDENCE_THRESHOLD = config['confidence_threshold']
        MAX_TIME_SEC = config['max_time_sec']

        self._control.set_ang_velocity(0, 0, MAX_ANG_SPEED)

        stopwatch = Stopwatch()
        stopwatch.start()
        self._logger.log("started to find gate loop")

        result = self.darknet_client.predict()

        if not result:
            self._logger.log("Empty bounding box")
        else:
            self._logger.log("Num of bb: "+ str(len(result)))
            self._logger.log(str(result[0]))
        while stopwatch.time() < MAX_TIME_SEC:
            if self.is_this_gate(img):
            # TODO: zlokalizuj bramkę
                gate = {"x", "y", "angle"}
            # TODO: zlokalizuj przeszkodę
                obstacle = "{"x", "y"}
            # more info in create_path comment
            #    self.create_path(gate, obstacle)    #W trajektorii musi byc uwzgledniona przeszkoda funkcja wyszukujaca przeszkode is_this_obstacle(bounding_box, img):
            return True
Ejemplo n.º 3
0
    def find_gate(self):
        self._logger.log("finding the gate")
        config = self.config['search']
        MAX_TIME_SEC = config['max_time_sec']
        MODE = config['mode']

        stopwatch = Stopwatch()
        stopwatch.start()
        self._logger.log("started find gate loop")

        while stopwatch.time() < config['max_time_sec']:

            if MODE == "mvg_avg":
                for i in range(config['number_of_samples']):
                    if self.is_this_gate():
                        return True
            elif MODE == "simple":
                bbox = False
                bbox = self.darknet_client.predict()[0].normalize(480, 480)
                if not bbox:
                    self._logger.log("gate not found")
                    return False
                self._logger.log("gate found")
                return True
            #self.movements.rotate_angle(0, 0, config['rotation_angle'])

        self._logger.log("gate not found")
        return False
Ejemplo n.º 4
0
    def run(self):
        self._logger.log("Bucket grab task exector started")

        self.darknet_client.load_model("bucket")
        '''
        TO DO: uncomment loading model when its done
        '''
        #self.darknet_client.load_model('buckets_task')
        stopwatch = Stopwatch()
        stopwatch.start()

        ## THIS LOOP SHOULD FIND AT LEAST FIRST BBOX WITH BUCKET
        while not self.find_buckets():
            self._logger.log("Finding buckets in progress")
            if stopwatch.time() >= self.MAX_TIME_SEC:
                self._logger.log("Finding buckets time expired")
                return 0

        #self.darknet_client.load_model("bucket")
        i = 0
        # THIS LOOP SHOULD FIND BUCKET WITH PINGER
        if self.bucket == 'pinger':
            while i < self.PINGER_LOOP_COUNTER:
                if self.find_pinger_bucket():
                    self._logger.log("Found pinger bucket")
                    i = self.PINGER_LOOP_COUNTER
                    self.grab_marker()
                    self._logger.log("Marker grabbed")
                    return 1
                i += 1
            self._logger.log("Marker could not be grabbed")
            return 0

        elif self.bucket == 'blue':
            k = 0
            while k < self.BLUE_LOOP_COUNTER:
                if self.find_blue_bucket():
                    self._logger.log("Found blue bucket")
                    k = self.BLUE_LOOP_COUNTER
                    self.grab_marker()
                    self._logger.log("Marker grabbed")
                    return 1
                k += 1
            self._logger.log("Marker could not be grabbed")
            return 0

        elif self.bucket == 'red':
            l = 0
            while l < self.ANY_BUCKET_COUNTER:
                if self.find_random_bucket():
                    self._logger.log("Found random bucket")
                    l = self.ANY_BUCKET_COUNTER
                    self.grab_marker()
                    self._logger.log("Marker grabbed")
                    return 1
                l += 1
            self._logger.log("Grabbing marker failed")
            return 0
Ejemplo n.º 5
0
    def center_on_gate(self):
        config = self.config['centering']
        MAX_TIME_SEC = config['max_time_sec']
        MAX_CENTER_DISTANCE = config['max_center_distance']

        stopwatch = Stopwatch()
        stopwatch.start()

        while stopwatch.time() <= MAX_TIME_SEC:

            bbox = self.darknet_client.predict()[0].normalize(480, 480)
            if bbox.x <= MAX_CENTER_DISTANCE & bbox.y <= MAX_CENTER_DISTANCE:
                self._logger.log("centered on gate successfully")
                return True
            center_rov(move=self._control,
                       Bbox=bbox,
                       depth_sensor=self.depth_sensor)
        self._logger.log("couldn't center on gate")
        return False
Ejemplo n.º 6
0
    def run(self):
        MAX_TIME_SEC = self.config['search']['max_time_sec'] #w configu trzeba zmienic bo na razie jest do samego gate
        self._logger.log("Gate task executor started")
        self._control.pid_turn_on()

        stopwatch = Stopwatch()
        stopwatch.start()

        if stopwatch.time() >= MAX_TIME_SEC:
            self._logger.log("TIME EXPIRED GATE NOT FOUND")
            self._control.set_ang_velocity(0, 0, 0)

        if not self.dive():
            self._logger.log("GateTE: diving in progress")

        if not self.find_gate():
            self._logger.log("GateTE: finding the gate in progress")

        if not self.go_trough_gate():
            self._logger.log("GateTE: going through the gate")
Ejemplo n.º 7
0
class GameGUI:

    FPS_LIMIT = 60
    AI_TIMESTEP_DELAY = 100
    HUMAN_TIMESTEP_DELAY = 1000
    CELL_SIZE = 20

    SNAKE_CONTROL_KEYS = [
        pygame.K_UP, pygame.K_LEFT, pygame.K_DOWN, pygame.K_RIGHT
    ]

    def __init__(self):
        pygame.init()
        self.agent = PlayerAgent()
        self.env = None
        self.screen = None
        self.fps_clock = None
        self.timestep_watch = Stopwatch()

    def load_environment(self, environment):
        """
        Load the RL environment into the GUI.
        """

        self.env = environment
        screen_size = (self.env.field.size * self.CELL_SIZE,
                       self.env.field.size * self.CELL_SIZE)

        self.screen = pygame.display.set_mode(screen_size)
        self.screen.fill(Colors.SCREEN_BACKGROUND)
        pygame.display.set_caption('Snake')

    def load_agent(self, agent):
        """ Load the RL agent into the GUI. """
        self.agent = agent

    def render_cell(self, x, y):
        """
        Draw the cell specified by the field coordinates.
        """

        cell_coordinates = pygame.Rect(
            x * self.CELL_SIZE,
            y * self.CELL_SIZE,
            self.CELL_SIZE,
            self.CELL_SIZE,
        )

        if self.env.field[x, y] == CellType.EMPTY:
            pygame.draw.rect(self.screen, Colors.SCREEN_BACKGROUND,
                             cell_coordinates)
        else:
            color = Colors.CELL_TYPE[self.env.field[x, y]]
            pygame.draw.rect(self.screen, color, cell_coordinates, 1)

            internal_padding = self.CELL_SIZE // 6 * 2
            internal_padding = (-internal_padding, -internal_padding)
            internal_square_coords = cell_coordinates.inflate(
                *internal_padding)
            pygame.draw.rect(self.screen, color, internal_square_coords)

    def render(self):
        """ Draw the entire game frame. """
        for x in range(self.env.field.size):
            for y in range(self.env.field.size):
                self.render_cell(x, y)

    def map_key_to_snake_action(self, key):
        """ Convert a keystroke to an environment action. """
        actions = [
            SnakeActions.MAINTAIN_DIRECTION,
            SnakeActions.TURN_LEFT,
            SnakeActions.MAINTAIN_DIRECTION,
            SnakeActions.TURN_RIGHT,
        ]

        ALL_SNAKE_DIRECTIONS = [
            SnakeDirections.NORTH,
            SnakeDirections.EAST,
            SnakeDirections.SOUTH,
            SnakeDirections.WEST,
        ]

        key_idx = self.SNAKE_CONTROL_KEYS.index(key)
        direction_idx = ALL_SNAKE_DIRECTIONS.index(self.env.snake.direction)
        return np.roll(actions, -key_idx)[direction_idx]

    def run(self, num_episodes=1):
        """ Run the GUI player for the specified number of episodes. """
        pygame.display.update()
        self.fps_clock = pygame.time.Clock()

        for episode in range(num_episodes):
            try:
                self.run_episode()
            except QuitRequestedError:
                break

            pygame.time.wait(1500)

    def run_episode(self):
        """ Run the GUI player for a single episode. """

        timestep_result = self.initialize_env()
        is_human_agent = isinstance(self.agent, PlayerAgent)
        timestep_delay = self.get_timestep_delay(is_human_agent)

        default_action = SnakeActions.MAINTAIN_DIRECTION

        while True:
            action = default_action

            for event in pygame.event.get():
                print('Event ', event.type)

                if event.type == KEYDOWN:
                    if is_human_agent and event.key in self.SNAKE_CONTROL_KEYS:
                        action = self.map_key_to_snake_action(event.key)
                    if event.key == pygame.K_ESCAPE:
                        raise QuitRequestedError

                if event.type == QUIT:
                    raise QuitRequestedError

            # Update game state.
            timestep_timed_out = self.timestep_watch.time() >= timestep_delay
            human_made_move = is_human_agent and action != default_action

            if timestep_timed_out or human_made_move:
                self.timestep_watch.reset()

                if not is_human_agent:
                    action = self.agent.next_action(
                        timestep_result.observation, timestep_result.reward)

                self.env.choose_action(action)
                timestep_result = self.env.timestep()

                if timestep_result.is_episode_end:
                    self.agent.end_episode()
                    break

            self.render_scene()

    def initialize_env(self):
        """ Initialize environment for new session. """
        self.timestep_watch.reset()
        timestep_result = self.env.new_episode()
        self.agent.reset_state()

        return timestep_result

    def get_timestep_delay(self, is_human_agent):
        if is_human_agent:
            return self.HUMAN_TIMESTEP_DELAY
        else:
            return self.AI_TIMESTEP_DELAY

    def render_scene(self):
        """ Render current scene. """
        self.render()
        score = self.env.snake.length - self.env.initial_snake_length
        pygame.display.set_caption(f'Snake  [Score: {score:02d}]')
        pygame.display.update()
        self.fps_clock.tick(self.FPS_LIMIT)