def load_graph(data):
g = Graph()
for i, row in enumerate(data):
for j, element in enumerate(row):
g.add_node(str(i) + '_' + str(j))
for m, n in get_neighbors(data, i, j):
g.add_edge(str(m) + '_' + str(n), str(i) + '_' + str(j), data[i][j])
return g
plotter2.add_obstacles(workspace_obstacles)
plotter2.add_c_space_obstacles(c_space_obstacles)
plotter2.add_visibility_graph(lines)
plotter2.add_robot(source, dist)
plotter2.show_graph()
# step 3:
with open(query, 'r') as f:
dest = tuple(map(float, f.readline().split(',')))
lines = get_visibility_graph(c_space_obstacles, source, dest)
# TODO: fill in the next line
graph = Graph()
for l in lines:
graph.add_node(l.coords[0])
graph.add_node(l.coords[1])
graph.add_edge(l.coords[0], l.coords[1], l.length)
graph.add_edge(l.coords[1], l.coords[0], l.length)
visited, path = dijsktra(graph, source)
shortest_path = []
curr = dest
while True:
shortest_path.insert(0, curr)
if curr == source:
break
curr = path[curr]
cost = visited[dest]
# shortest_path, cost = None, None
def handler(event, context):
status_code = 200
total_elapsed = 0
total_turns = 0
m = eval(event['map_matrix'])
map_matrix = Map.map_matrix_transfer(m)
start_position = eval(event['start_position'])
start_direction = event['start_direction']
# Algorithm, default=DFS
algorithm = 'dfs'
if 'algorithm' in event:
algorithm = event['algorithm']
if algorithm == "bfs" or algorithm == "dfs":
if algorithm == "bfs":
# run with bfs
robot = Robot(map_matrix, start_position, start_direction)
sweeper = BFSSweeper(robot)
else:
# run with dfs
robot = Robot(map_matrix, start_position, start_direction)
sweeper = DFSSweeper(robot)
start = time.time()
sweeper.sweep()
path_history = robot.path_history
finished_x = path_history[len(path_history) - 1][0]
finished_y = path_history[len(path_history) - 1][1]
back_path_steps = 0
if str(start_position['x']) + '_' + str(start_position['y']) != str(
finished_x) + '_' + str(finished_y):
graph = Graph()
# print(map_matrix)
for i in range(len(map_matrix)):
for j in range(len(map_matrix[i])):
floor_type = map_matrix[i][j]
if floor_type == 0:
graph.add_node(str(j) + '_' + str(i))
near_spots = find_adjacent_spot(i, j, map_matrix)
for spot_inf in near_spots:
spot = spot_inf[0]
weight = spot_inf[1]
graph.add_edge(str(j) + '_' + str(i), spot, weight)
back_path_result = shortest_path(
graph,
str(finished_x) + '_' + str(finished_y),
str(start_position['x']) + '_' + str(start_position['y']))
back_path_steps = back_path_result[0]
back_path_arr = back_path_result[1]
for back_spot_str in back_path_arr:
back_spot_arr = back_spot_str.split('_')
back_spot_x = int(back_spot_arr[0])
back_spot_y = int(back_spot_arr[1])
path_history.append([back_spot_x, back_spot_y])
elapsed = time.time() - start
path = Map.path_append_attributes(m, path_history)
total_elapsed += elapsed
total_steps = robot.move_count + back_path_steps
total_turns += robot.turn_count
result = {
"path": str(path),
"steps_taken": total_steps,
"turns_taken": total_turns,
"times_taken": round(total_elapsed * 1000, 6)
}
elif algorithm == "dijkstra":
if 'end_position' in event:
end_position = eval(event['end_position'])
graph = Graph()
for i in range(len(map_matrix)):
for j in range(len(map_matrix[i])):
floor_type = map_matrix[i][j]
if floor_type == 0:
graph.add_node(str(j) + '_' + str(i))
near_spots = find_adjacent_spot(i, j, map_matrix)
for spot_inf in near_spots:
spot = spot_inf[0]
weight = spot_inf[1]
graph.add_edge(str(j) + '_' + str(i), spot, weight)
start = time.time()
to_path_result = shortest_path(
graph,
str(start_position['x']) + '_' + str(start_position['y']),
str(end_position['x']) + '_' + str(end_position['y']))
back_path_result = shortest_path(
graph,
str(end_position['x']) + '_' + str(end_position['y']),
str(start_position['x']) + '_' + str(start_position['y']))
elapsed = time.time() - start
to_path_steps = to_path_result[0]
to_path_arr = to_path_result[1]
back_path_steps = back_path_result[0]
back_path_arr = back_path_result[1]
path_history = []
for to_spot_str in to_path_arr:
to_spot_arr = to_spot_str.split('_')
to_spot_x = int(to_spot_arr[0])
to_spot_y = int(to_spot_arr[1])
path_history.append([to_spot_x, to_spot_y])
for back_spot_str in back_path_arr:
back_spot_arr = back_spot_str.split('_')
back_spot_x = int(back_spot_arr[0])
back_spot_y = int(back_spot_arr[1])
path_history.append([back_spot_x, back_spot_y])
path = Map.path_append_attributes(m, path_history)
total_steps = to_path_steps + back_path_steps
result = {
"path": str(path),
"steps_taken": total_steps,
"turns_taken": 0,
"times_taken": round(elapsed * 1000, 6)
}
else:
status_code = 401
result = {
"type": "Params error",
"code": "401",
"reason": "Required param not exist"
}
return {
"statusCode": status_code,
"headers": {
"Content-Type": "application/json"
},
"body": result
}