def check_right_free(self, return_coords: bool = False):
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.EAST]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
right_coord = self.cur_coord.add(displacement)
if return_coords:
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree]).multiply(2)
viewed_cell = self.cur_coord.add(displacement)
return viewed_cell
else:
if self.arena.coord_is_valid(
right_coord
) and not self.arena.get_cell_at_coord(right_coord).is_dangerous():
return True
else:
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree]).multiply(2)
viewed_cell = self.cur_coord.add(displacement)
if self.arena.coord_is_valid(
viewed_cell) and self.arena.get_cell_at_coord(
viewed_cell).is_obstacle():
adj_obstacle = viewed_cell
surface_orientation = (abs_degree + 180) % 360
self.arena.get_cell_at_coord(
adj_obstacle).set_seen_surface(surface_orientation)
self.arena.get_cell_at_coord(adj_obstacle).set_is_seen(
True)
return False
def calculate_move(self, current_coord, target_coord) -> MoveCommand:
displacement = target_coord.subtract(current_coord)
target_orientation = OT.displacement_to_orientation(displacement)
turn_angle = OT.calc_degree_of_turn(self.robot_info.get_orientation(),
target_orientation)
# update expected robot info
self.expected_robot_info = RobotInfo(target_coord, target_orientation)
return MoveCommand(turn_angle, displacement.manhattan_distance())
def step(self):
# calculate agent percepts
sis = SensorInputSimulation(self.robot_info, self.arena)
obstacle_coord_list, no_obs_coord_list = sis.calculate_percepts()
for coord in obstacle_coord_list:
self.arena.get_cell_at_coord(coord).set_is_explored(True)
for coord in no_obs_coord_list:
self.arena.get_cell_at_coord(coord).set_is_explored(True)
self.arena.get_cell_at_coord(
self.robot_info.get_coord()).set_is_visited(True)
# get agent next move
agent_output = self.agent.step(obstacle_coord_list, no_obs_coord_list,
self.robot_info)
print(agent_output.get_message())
move_command = agent_output.get_move_command()
if move_command == None:
self.quit()
# update internal representation of robot
new_orientation = OrientationTransform.calc_orientation_after_turn(
self.robot_info.get_orientation(), move_command.get_turn_angle())
if new_orientation != self.robot_info.get_orientation():
if abs(new_orientation.value -
self.robot_info.get_orientation().value) == 1 or abs(
new_orientation.value -
self.robot_info.get_orientation().value) == 3:
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
elif abs(new_orientation.value -
self.robot_info.get_orientation().value) == 2:
self.robot_info.set_orientation(
Orientation((new_orientation.value - 1) % 3))
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
unit_move = OrientationTransform.orientation_to_unit_displacement(
new_orientation)
# move = unit_move.multiply(move_command.get_cells_to_advance()) # enable this if robot can move multiple squares
move = unit_move # enable this if robot can move one square at a time
self.robot_info.set_coord(self.robot_info.get_coord().add(move))
# update pygame display
self.update_display()
def main_sensor_parser(sensor_dict, robot_info):
sensor_data = sensor_dict['data']
cur_coord = robot_info.get_coord()
orientation = robot_info.get_orientation()
# cur_coord = Coord(6, 6)
# orientation = Orientation.EAST
obstacleList = []
exploredList = []
i = 0
for key, val in SENSOR_CONSTANTS.items():
sensor_abs_degree = (val['direction'] +
OT.orientation_to_degree[orientation]) % 360
string1 = 'displacement_' + str(orientation.value)
displacement_per_step = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[sensor_abs_degree])
obstacleList.extend(
SensorParser.individual_sensor_parser(cur_coord, val[string1],
displacement_per_step,
sensor_data[key],
val['range'])[0])
exploredList.extend(
SensorParser.individual_sensor_parser(cur_coord, val[string1],
displacement_per_step,
sensor_data[key],
val['range'])[1])
i += 1
# for item in obstacleList:
# for j in item:
# print(j.get_x(), j.get_y())
# print("Break")
# for i in exploredList:
# for z in i:
# print(z.get_x(), z.get_y())
return obstacleList, exploredList
def move_back(self) -> Coord:
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.SOUTH]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
back_coord = self.cur_coord.add(displacement)
return back_coord
def move_right(self) -> Coord:
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.EAST]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
right_coord = self.cur_coord.add(displacement)
return right_coord
def move_forward(self) -> Coord:
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.NORTH]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
front_coord = self.cur_coord.add(displacement)
return front_coord
def check_right_free(self, return_coords: bool = False):
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.EAST]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
right_coord = self.cur_coord.add(displacement)
if return_coords:
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree]).multiply(2)
viewed_cell = self.cur_coord.add(displacement)
return viewed_cell
else:
if self.arena.coord_is_valid(
right_coord
) and not self.arena.get_cell_at_coord(right_coord).is_dangerous():
return True
else:
return False
def update_robot_info(self, move_command):
# update internal representation of robot
new_orientation = OrientationTransform.calc_orientation_after_turn(self.robot_info.get_orientation(), move_command.get_turn_angle())
if new_orientation != self.robot_info.get_orientation():
if abs(new_orientation.value - self.robot_info.get_orientation().value) == 1 or abs(new_orientation.value - self.robot_info.get_orientation().value) == 3:
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
elif abs(new_orientation.value - self.robot_info.get_orientation().value) == 2:
self.robot_info.set_orientation(Orientation((new_orientation.value-1)%3))
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
unit_move = OrientationTransform.orientation_to_unit_displacement(new_orientation)
move = unit_move.multiply(move_command.get_cells_to_advance()) # enable this if robot can move multiple squares
self.robot_info.set_coord(self.robot_info.get_coord().add(move))
def move_back(self) -> Coord:
abs_degree = (self.cur_direction.value * 90 +
OT.orientation_to_degree[Orientation.SOUTH]) % 360
displacement = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[abs_degree])
back_coord = self.cur_coord.add(displacement)
obstacle_cell_abs_degree = (abs_degree + 90) % 360
obstacle_cell = self.cur_coord.add(
OT.orientation_to_unit_displacement(
OT.degree_to_orientation[obstacle_cell_abs_degree]).multiply(
2))
if self.arena.coord_is_valid(
obstacle_cell) and self.arena.get_cell_at_coord(
obstacle_cell).is_obstacle():
surface_orientation = (obstacle_cell_abs_degree + 180) % 360
self.arena.get_cell_at_coord(obstacle_cell).set_seen_surface(
surface_orientation)
self.arena.get_cell_at_coord(obstacle_cell).set_is_seen(True)
return back_coord
def get_agent_output_list(self) -> list:
# Calculate fastest path of coords
coords_list = []
cl_to_waypoint = self.algo.get_next_step(self.arena,
self.robot_info,
self.end_coord,
self.waypoint_coord,
full_path=True)
last_displacement = cl_to_waypoint[-1].subtract(cl_to_waypoint[-2])
last_orientation = OT.displacement_to_orientation(last_displacement)
robot_info_at_waypoint = RobotInfo(cl_to_waypoint[-1],
last_orientation)
cl_to_end = self.algo.get_next_step(self.arena,
robot_info_at_waypoint,
self.end_coord,
None,
full_path=True)
coords_list.extend(cl_to_waypoint)
coords_list.extend(cl_to_end)
return self.calc_agent_output_full_path(coords_list)
def calculate_percepts(
self
) -> list: # this function calculates the coords of ALL cells in the visible range (including those behind obstacles)
orientation = self.robot_info.get_orientation()
cur_coord = self.robot_info.get_coord()
# cells within robot 3x3 are obviously explored and not obstacles
# self.no_obs_coord_list.extend([(coord, 1) for coord in self.arena.get_eight_adjacent_in_arena(cur_coord)]) # distance is 1
# self.no_obs_coord_list.append((cur_coord, 0)) # distance is 0
for sensor in SENSOR_CONSTANTS.values():
sensor_abs_degree = (sensor['direction'] +
OT.orientation_to_degree[orientation]) % 360
displacement_per_step = OT.orientation_to_unit_displacement(
OT.degree_to_orientation[sensor_abs_degree])
sensor_displacement_key = 'displacement_' + str(
orientation.value) # key changes based on orientation of robot
cil = self.get_coords_in_line(sensor[sensor_displacement_key],
displacement_per_step,
sensor['range'])
self.update_explored_coord_lists(cil)
return self.obstacles_coord_list, self.no_obs_coord_list
def get_fastest_path(self, arena: Arena, robot_info: RobotInfo, end: Coord, waypoint:Coord=None):
start = robot_info.get_coord()
start_orientation = robot_info.get_orientation()
if not arena.coord_is_valid(end):
raise Exception(f'FastestPath: end coord out of arena: {end.get_x()}, {end.get_y()}')
end_cell = arena.get_cell_at_coord(end)
if end_cell.is_obstacle():
raise Exception(f'FastestPath: cannot move to obstacle cell: {end.get_x()}, {end.get_y()}')
if waypoint:
# we haven't passed through it yet
target = waypoint
else:
target = end
root = DecisionNode(start, start_orientation, exact_cost=0, parent=None)
fringe_nodes = PriorityQueue()
queue_tie_breaker = 0
fringe_nodes.put((
FastestPathAlgo.heuristic(root.get_coord(), target),
queue_tie_breaker,
root
))
examined = [[False for y in range(MAP_ROW)] for x in range(MAP_COL)]
cur = fringe_nodes.get()[-1]
while True:
# two conditions to exit loop, one may or may not be enabled
cur_coord = cur.get_coord()
cur_orientation = cur.get_orientation()
if not end_cell.is_dangerous():
# normally exit when target is found
if cur_coord.is_equal(target):
# found target so break and find first step
break
else:
# if cell is dangerous, move to any adjacent cell to it \
# (some cells, surrounded by danger, will be left out, but they are likely to be obstacles)
if cur_coord.subtract(target).manhattan_distance() <= 1 \
and cur_coord.subtract(start).manhattan_distance() > 0:
# target not found so return None
return None
if fringe_nodes.qsize() >= 3000:
# if we've got such a long queue, there's probably no way to the target
return None
examined[cur_coord.get_x()][cur_coord.get_y()] = True
adjacent_coords = arena.get_adjacent_safe(cur_coord)
for coord in adjacent_coords:
if examined[coord.get_x()][coord.get_y()]:
# assume heuristic is admissible, otherwise need to assess total_cost \
# and remove node from examined set if g+h < existing g
continue
# get costs
target_orientation = OrientationTransform.displacement_to_orientation(coord.subtract(cur_coord))
degree_of_turn = OrientationTransform.calc_degree_of_turn(cur_orientation, target_orientation)
turning_cost = int(degree_of_turn/90)*TimeCosts.QUARTER_TURN
displacement_cost = TimeCosts.MOVE_ONE_UNIT
cost = turning_cost + displacement_cost
exact_cost = cur.get_exact_cost() + cost
estimated_cost = FastestPathAlgo.heuristic(cur_coord, target)
total_cost = exact_cost + estimated_cost
fringe_node = DecisionNode(coord, target_orientation, exact_cost=exact_cost, parent=cur)
queue_tie_breaker += 1
fringe_nodes.put((total_cost, queue_tie_breaker, fringe_node))
if fringe_nodes.empty():
return None
cur = fringe_nodes.get()[-1]
cur_list = []
while cur.get_parent() and not cur.get_parent().get_coord().is_equal(start):
cur_list.append(cur)
cur = cur.get_parent()
cur_list.append(cur)
cur_list.reverse()
return cur_list
