def calibrate(self, sense):
is_calibrated = False
front_direction = self.robot.direction
right_direction = (front_direction + 1) % 4
front_direction_vector = Direction.get_direction_vector(
front_direction)
right_direction_vector = Direction.get_direction_vector(
right_direction)
# Check front
can_calibrate_front = False
for i in range(-1, 2):
c = self.robot.pos[0] + 2 * front_direction_vector[
0] + i * right_direction_vector[0]
r = self.robot.pos[1] + 2 * front_direction_vector[
1] + i * right_direction_vector[1]
if c < 0 or c > NUM_COLS - 1 or r < 0 or r > NUM_ROWS - 1 or self.explored_map[
r][c] == Cell.OBSTACLE:
can_calibrate_front = True
break
if can_calibrate_front:
self.on_calibrate(is_front=True)
is_calibrated = True
# Check right
if self.steps_without_calibration >= MIN_STEPS_WITHOUT_CALIBRATION:
can_calibrate_right = True
for i in [-1, 1]:
c = self.robot.pos[0] + i * front_direction_vector[
0] + 2 * right_direction_vector[0]
r = self.robot.pos[1] + i * front_direction_vector[
1] + 2 * right_direction_vector[1]
if 0 <= c < NUM_COLS and 0 <= r < NUM_ROWS and self.explored_map[
r][c] != Cell.OBSTACLE:
can_calibrate_right = False
if can_calibrate_right:
self.on_calibrate(is_front=False)
self.steps_without_calibration = 0
is_calibrated = True
if is_calibrated and sense:
self.sense_and_repaint()
def sense_and_repaint(self, sensor_values=None):
if sensor_values is None:
sensor_values = self.robot.sense()
for i in range(len(sensor_values)):
sensor_value = sensor_values[i]
sensor = self.robot.sensors[i]
direction_vector = Direction.get_direction_vector(
sensor.get_current_direction(self.robot))
current_sensor_pos = sensor.get_current_pos(self.robot)
if sensor_value == -1:
continue
elif sensor_value is None:
for j in range(*sensor.get_range()):
pos_to_mark = (current_sensor_pos[0] +
j * direction_vector[0],
current_sensor_pos[1] +
j * direction_vector[1])
if 0 <= pos_to_mark[0] <= NUM_COLS - 1 and 0 <= pos_to_mark[
1] <= NUM_ROWS - 1:
self.explored_map[pos_to_mark[1]][
pos_to_mark[0]] = Cell.FREE
else:
for j in range(sensor.get_range()[0],
min(sensor.get_range()[1], sensor_value + 1)):
pos_to_mark = (current_sensor_pos[0] +
j * direction_vector[0],
current_sensor_pos[1] +
j * direction_vector[1])
if 0 <= pos_to_mark[0] <= NUM_COLS - 1 and 0 <= pos_to_mark[
1] <= NUM_ROWS - 1:
if j != sensor_value:
self.explored_map[pos_to_mark[1]][
pos_to_mark[0]] = Cell.FREE
else:
self.explored_map[pos_to_mark[1]][
pos_to_mark[0]] = Cell.OBSTACLE
if pos_to_mark not in self.obstacles:
self.obstacles[pos_to_mark] = {0, 1, 2, 3}
self.remove_obstacle_side(pos_to_mark)
for r in range(START_POS[1] - 1, START_POS[1] + 2):
for c in range(START_POS[0] - 1, START_POS[0] + 2):
self.explored_map[r][c] = Cell.FREE
for r in range(GOAL_POS[1] - 1, GOAL_POS[1] + 2):
for c in range(GOAL_POS[0] - 1, GOAL_POS[0] + 2):
self.explored_map[r][c] = Cell.FREE
self.on_update_map()
def check_if_contains_corners(self, obstacles, direction):
obstacle_direction = (direction + 2) % 4
direction_vector = Direction.get_direction_vector(obstacle_direction)
for obstacle in obstacles:
pos = (obstacle[0] + 2 * direction_vector[0],
obstacle[1] + 2 * direction_vector[1])
if not self.is_pos_safe(pos, consider_unexplored=False):
print(pos, obstacle)
return True
return False
def possible_remaining_unexplored(self, goal):
d = set()
for direction in range(4):
for sensor in self.robot.sensors:
sensor_direction = (direction + sensor.direction -
Direction.EAST) % 4
reverse_sensor_direction = (sensor_direction + 2) % 4
direction_vector = Direction.get_direction_vector(
reverse_sensor_direction)
for i in range(*sensor.get_range()):
sensor_pos = (goal[0] + i * direction_vector[0],
goal[1] + i * direction_vector[1])
if sensor_pos[0] < 0 or sensor_pos[
0] >= NUM_COLS or sensor_pos[1] < 0 or sensor_pos[
1] >= NUM_ROWS or self.explored_map[sensor_pos[
1]][sensor_pos[0]] == Cell.OBSTACLE:
break
if direction == Direction.NORTH:
pos = sensor_pos[0] + sensor.pos[1], sensor_pos[
1] - sensor.pos[0]
elif direction == Direction.EAST:
pos = sensor_pos[0] - sensor.pos[0], sensor_pos[
1] - sensor.pos[1]
elif direction == Direction.SOUTH:
pos = sensor_pos[0] - sensor.pos[1], sensor_pos[
1] + sensor.pos[0]
else: # Direction.WEST
pos = sensor_pos[0] + sensor.pos[0], sensor_pos[
1] + sensor.pos[1]
if self.is_pos_safe(pos):
d.add((pos, direction))
return d
def sense(self):
sensor_values = []
for sensor in self.sensors:
direction_vector = Direction.get_direction_vector(
sensor.get_current_direction(self))
sensor_pos = sensor.get_current_pos(self)
sensor_range = sensor.get_range()
for i in range(1, sensor_range[1]):
pos_to_check = (sensor_pos[0] + i * direction_vector[0],
sensor_pos[1] + i * direction_vector[1])
if pos_to_check[0] < 0 or pos_to_check[0] >= NUM_COLS or pos_to_check[1] < 0 or pos_to_check[1] >= NUM_ROWS or\
self.map[pos_to_check[1]][pos_to_check[0]] == Cell.OBSTACLE:
if i < sensor_range[0]:
sensor_values.append(-1)
else:
sensor_values.append(i)
break
else:
sensor_values.append(None)
return sensor_values
def find_right_pos(self):
direction_vector = Direction.get_direction_vector(
(self.robot.direction + 1) % 4)
current_pos = self.robot.pos
return current_pos[0] + direction_vector[0], current_pos[
1] + direction_vector[1]