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 move(self, movement, sense=False):
if not isinstance(movement, Movement):
if self.direction == Direction.NORTH:
self.pos = (self.pos[0], self.pos[1] + movement)
elif self.direction == Direction.EAST:
self.pos = (self.pos[0] + movement, self.pos[1])
elif self.direction == Direction.SOUTH:
self.pos = (self.pos[0], self.pos[1] - movement)
elif self.direction == Direction.WEST:
self.pos = (self.pos[0] - movement, self.pos[1])
elif movement == Movement.FORWARD:
if self.direction == Direction.NORTH:
self.pos = (self.pos[0], self.pos[1] + 1)
elif self.direction == Direction.EAST:
self.pos = (self.pos[0] + 1, self.pos[1])
elif self.direction == Direction.SOUTH:
self.pos = (self.pos[0], self.pos[1] - 1)
elif self.direction == Direction.WEST:
self.pos = (self.pos[0] - 1, self.pos[1])
elif movement == Movement.BACKWARD:
if self.direction == Direction.NORTH:
self.pos = (self.pos[0], self.pos[1] - 1)
elif self.direction == Direction.EAST:
self.pos = (self.pos[0] - 1, self.pos[1])
elif self.direction == Direction.SOUTH:
self.pos = (self.pos[0], self.pos[1] + 1)
elif self.direction == Direction.WEST:
self.pos = (self.pos[0] + 1, self.pos[1])
elif movement == Movement.RIGHT:
self.direction = Direction((self.direction + 1) % 4)
elif movement == Movement.LEFT:
self.direction = Direction((self.direction - 1) % 4)
return self.on_move(movement)
def send_movement(self, movement, robot):
print(movement)
if movement == Movement.FORWARD:
movement_str = "1"
elif isinstance(movement, Movement):
movement_str = Movement.convert_to_string(movement)
else:
movement_str = str(movement)
# Sample message: M:R 1,2 E
msg = "{} {},{} {}".format(
movement_str,
robot.pos[0],
robot.pos[1],
Direction.convert_to_string(robot.direction),
)
self.send_msg_with_type(RPi.MOVEMENT_MSG, msg)
return self.receive_sensor_values(send_msg=False)
def display(dic):
symbol_len = Symbol.max_len("Symbol")
new_state_len = State.max_len("New State")
new_symbol_len = Symbol.max_len("New Symbol")
direction_len = Direction.max_len("Direction")
total = symbol_len + new_state_len + new_symbol_len + direction_len + 11
for state, value in dic.items():
print(f"State: {state.name}")
print("+" + "-" * total + "+")
pprint("Symbol", symbol_len)
pprint("New State", new_state_len)
pprint("New Symbol", new_symbol_len)
pprint("Direction", direction_len, last=True)
for symbol, tupl in value.items():
pprint(symbol.name[4], symbol_len)
pprint(tupl[0].name, new_state_len)
pprint(tupl[1].name[4], new_symbol_len)
pprint(tupl[2].name, direction_len, last=True)
print("+" + "-" * total + "+")
print("")
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 make_tuple(dst):
state, symbol, direc = dst.split(",")
return (State.by_rep(state), Symbol.by_rep(symbol),
Direction.by_rep(direc))
def get_current_direction(self, robot):
return Direction(
(robot.direction + self.direction - Direction.EAST) % 4)
def getCapturedPosition(self, move):
if (self.isInPossibleCaptures(move)):
return self.getAdjacentPosition(move[0], Adjacent[Direction(move[1] - move[0]).name])
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]