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
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
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
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
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
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")
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)