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 }