def __call__(self, begin=None, sample=True, log=True, random_end=False): if not begin: begin = self.begin ret = [] while len(ret) < 40: # print('path planning task begin: ', begin, 'random end ', random_end) # print('path planning task allowed: ', begin in self.allowed_cells_list) self.solver = a_star.AStar(self.env_config['max_y_idx'], self.env_config['max_x_idx']) self.solver.init_grid(self.obstacles_list) end = random.choice(self.allowed_cells_list) if ( self.randomized_target or random_end) else self.end # print('path planning task end: ', end) self.solver.set_task(begin, end) try: ret = self.solver.solve() except: self.log_path(begin, end, [(1, 1), (1, 1)]) # print('bad begin', begin) # print('bad end', end) # print('ret: ', ret) ret = [(x, y) for y, x in ret] if log: self.log_path(begin, end, ret) if sample: ret = self.get_jump_points(ret) for sub_func in self.subscribents: sub_func(ret)
def __init__(self, env_config, randomized_target): self.env_config = env_config self.env_map = cv2.cvtColor(cv2.imread(env_config['env_map']), cv2.COLOR_BGR2RGB) begin = np.nonzero(utils.get_colour_map(self.env_map, 255, 0, 0)) self.begin = begin[0][0], begin[1][0] end = np.nonzero(utils.get_colour_map(self.env_map, 0, 0, 255)) self.end = end[0][0], end[1][0] self.env_map[begin[0], begin[1], :] = (255, 255, 255) kernel_size = env_config['kernel_size'] self.env_map_er = cv2.erode(self.env_map, np.ones((kernel_size, kernel_size), np.uint8), iterations=1) self.obstacles_map = utils.get_colour_map(self.env_map_er, 0, 0, 0) self.obstacles_list = [ (r, c) for r, c in zip(*np.nonzero(self.obstacles_map)) ] # cv2.imwrite('all_map.png', self.env_map) self.allowed_cells_map = cv2.erode(self.env_map_er, np.ones((3, 3), np.uint8), iterations=1) # cv2.imwrite('allowed_cells.png', self.allowed_cells_map) self.allowed_cells_map = utils.get_colour_map(self.allowed_cells_map, 255, 255, 255) self.allowed_cells_list = [ (r, c) for r, c in zip(*np.nonzero(self.allowed_cells_map)) ] self.solver = a_star.AStar(env_config['max_y_idx'], env_config['max_x_idx']) self.subscribents = [] self.randomized_target = randomized_target
def depth_first_search(self): self.canvas.itemconfig("oval", fill="white", outline="white") self.search = a_star.AStar(self.graph) self.search.search_type = "depth-first" self.search.max_nodes = max_nodes self.search.initialize("manhattan distance") self.redraw()
def test_maze(self): a = pf.AStar() walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3), (3, 1), (3, 2), (3, 5), (4, 1), (4, 4), (5, 1)) a.init_grid(8, 10, walls, (0, 0), (5, 6)) path = a.solve() print path
def visualize_loop(self): visualizer = a_star.AStar(self.field) while visualizer.make_step(): for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.QUIT sys.exit() self.draw_screen() self.clock.tick(VISUALIZATION_SPEED) self.draw_screen()
def __init__(self): self.robot = a_star.AStar() try: self.t1 = Thread(target=self.receiver, args=()) self.t1.start() except: print "Error: unable to start thread" self.message = None
def grid_exec(): raw_str = request.args['info'] json_str = json.loads(raw_str) r = json_str['rows'] c = json_str['cols'] start = tuple(json_str['start']) end = tuple(json_str['end']) walls = tuple([tuple(x) for x in json_str['walls']]) a = pf.AStar() a.init_grid(r, c, walls, start, end) path = a.solve() return jsonify(path)
def main(): camera = cameraService.startCamera() robot = a_star.AStar() moves = [(0, 0), (0, 0), (0, 0)] try: while (True): frame = cameraService.getFrameFromCamera(camera=camera) error = cameraService.findDirectionError(frame, show=True) print robot.read_distance() if robot.read_distance() < 3000: if all(x == (0,0) for x in moves): robot.play_notes("eee") time.sleep(0.5) robot.motors(-300, 0) time.sleep(3) robot.motors(0, 0) continue robot.play_notes("cc") time.sleep(0.5) robot.motors(-moves[0][0], -moves[0][1]) time.sleep(1.5) robot.motors(0, 0) del moves[0] moves.append((0, 0)) if all(x == (0,0) for x in moves): robot.motors(-200, 200) time.sleep(2) robot.motors(0, 0) continue if error < -0.05: robot.motors(0, 300) moves.pop() moves.insert(0, (0, 300)) time.sleep(0.5) elif error > 0.05: robot.motors(300, 0) moves.pop() moves.insert(0, (300, 0)) time.sleep(0.5) else: robot.motors(200, 200) # moves.pop() # moves.insert(0, (200, 200)) time.sleep(1.5) robot.motors(0, 0) finally: robot.motors(0, 0) camera.close()
def __init__(self): self.a_star = a_star.AStar() self.map = self.a_star.run() self.max_i = len(self.map) self.max_j = len(self.map[0]) self.block_width = 32 self.block_height = 32 self.screen = pygame.display.set_mode( (self.max_j * self.block_width, self.max_i * self.block_height)) self.white = pygame.image.load('white.png').convert() self.gray = pygame.image.load('gray.png').convert() self.red = pygame.image.load('red.png').convert() self.green = pygame.image.load('green.png').convert() self.blue = pygame.image.load('blue.png').convert() self.frame = pygame.image.load('frame.png').convert_alpha() self.background = [ self.white, self.green, self.red, None, None, None, self.blue, None, None, self.gray ]
ax = plt.gca() ax.set_xlim([0, map.size]) ax.set_ylim([0, map.size]) for i in range(map.size): for j in range(map.size): if map.IsObstacle(i, j): rec = Rectangle((i, j), width=1, height=1, color='gray') ax.add_patch(rec) else: rec = Rectangle((i, j), width=1, height=1, edgecolor='gray', facecolor='w') ax.add_patch(rec) rec = Rectangle((0, 0), width=1, height=1, facecolor='b') ax.add_patch(rec) rec = Rectangle((map.size - 1, map.size - 1), width=1, height=1, facecolor='r') ax.add_patch(rec) plt.axis('equal') plt.axis('off') plt.tight_layout() #plt.show() a_star = a_star.AStar(map) a_star.RunAndSaveImage(ax, plt)
import jsonSocket import logging import time import a_star #import speech logger = logging.getLogger("RI-Project") robot = a_star.AStar() class PiServer(jsonSocket.ThreadedServer): def __init__(self): super(PiServer, self).__init__(address='192.168.0.11') self.timeout = 2.0 # self.voice=speech.Speech() def _processMessage(self, obj): """ virtual method """ if obj != '': x = obj.get("x") y = obj.get("y") obstacle = obj.get("obstacle") x, y = self._parser(x, y) if obstacle == "1": pass # self.voice.obstacle() #print x, y, obstacle try: print robot.read_distance()[0] except:
def main(): """ Generates plots for all requested scenarios for HW#1 Parts 7 & 9 will appear on the same Figure """ # Part 3 - Offline A* Search - Large Grid Size path3_1 = a_star.AStar([.5, -1.5], [0.5, 1.5], 1, 0, online_check=0) path3_1.start_planning() path3_2 = a_star.AStar([4.5, 3.5], [4.5, -1.5], 1, 0, online_check=0) path3_2.start_planning() path3_3 = a_star.AStar([-0.5, 5.5], [1.5, -3.5], 1, 0, online_check=0) path3_3.start_planning() # Part 5 - Online A* Search - Large Grid Size part5_1 = a_star.AStar([.5, -1.5], [0.5, 1.5], 1, 0) part5_1.start_planning() part5_2 = a_star.AStar([4.5, 3.5], [4.5, -1.5], 1, 0) part5_2.start_planning() part5_3 = a_star.AStar([-0.5, 5.5], [1.5, -3.5], 1, 0) part5_3.start_planning() # Part 7 - Online A* Search - Small Grid Size path7_1 = a_star.AStar([2.45, -3.55], [0.95, -1.55], .1, .3) path7_1.start_planning() path7_2 = a_star.AStar([4.95, -0.05], [2.45, 0.25], .1, .3) path7_2.start_planning() path7_3 = a_star.AStar([-0.55, 1.45], [1.95, 3.95], .1, .3) path7_3.start_planning() # Part 9 - Robot Driving Simulation of paths from Part 7 - # The results will appear on the plots genterated in Part 7 a_star.Robot([2.45, -3.55], [0.95, -1.55], final_path=path7_1.fin_path, grid_obj=path7_1.field) a_star.Robot([4.95, -0.05], [2.45, 0.25], final_path=path7_2.fin_path, grid_obj=path7_2.field) a_star.Robot([-0.55, 1.45], [1.95, 3.95], final_path=path7_3.fin_path, grid_obj=path7_3.field) # Part 10 - Robot Driving Simulation with simultaneous Online A* search # using Part 7 Start & Goal Combinations a_star.Robot([2.45, -3.55], [0.95, -1.55]) a_star.Robot([4.95, -0.05], [2.45, 0.25]) a_star.Robot([-0.55, 1.45], [1.95, 3.95]) # Part 11 - Robot Driving Simulation with simultaneous Online A* search # using Part 3 Start & Goal Combinations a_star.Robot([.5, -1.5], [0.5, 1.5]) a_star.Robot([.5, -1.5], [0.5, 1.5], cell_size=1, inflate=0) a_star.Robot([4.5, 3.5], [4.5, -1.5]) a_star.Robot([4.5, 3.5], [4.5, -1.5], cell_size=1, inflate=0) a_star.Robot([-0.5, 5.5], [1.5, -3.5]) a_star.Robot([-0.5, 5.5], [1.5, -3.5], cell_size=1, inflate=0) # Keep plots from closing at the end a_star.plot_show()
def main(): #Clear the screen, prepare the mainlopp, prepare the input file, and the input file list os.system("clear") flag = True f = open("input.txt", "r") file_list = [] #Print welcome message including the premade graph information. Save the graph information as a list print("Project Module #1: \n" "A Star \n" "\n" "Premade problems are:") print f.name for line in f.readlines(): file_list.append(line) sys.stdout.write(line) print( "\n" "'Run 0-X' for premade problem \n" "'Run new' for custom problem \n" "'Speed test 0-X' for testing run speed \n" "'Exit' ends the script \n" "\n") #This is the mainloop - It reads input from the user and executes the commands while flag: the_input = raw_input(" > ") #Run a premade graph if the_input != "Run new" and the_input.startswith("Run"): #Get the correct premade graph and add the numbers to a list number = the_input[4:] line_list = map(int, re.findall(r'\d+', file_list[int(number)])) #Add the walls to a list walls = [] for wall_start in range(7, len(line_list), 4): wall = [] for wall_number in range(wall_start, wall_start + 4): wall.append(line_list[wall_number]) walls.append(wall) #Initialize premade graph, and gui; If the graph is not too large if line_list[1] <= max_breadth and line_list[2] <= max_height: premade_graph = Graph(columns=line_list[1], rows=line_list[2], a_pos_x=line_list[3], a_pos_y=line_list[4], b_pos_x=line_list[5], b_pos_y=line_list[6], walls=walls) #Scale down the size if the graph is too large if line_list[1] > 50 and line_list[2] > 30: size = 25 / 5 else: size = 25 premade_graph_gui = gui.GUI(graph=premade_graph, cellsize=size) _run_gui(premade_graph_gui) else: print "Error: graph too large" #Run a custom graph elif the_input == "Run new": size = input("graph size: ") start = input("Start position: ") end = input("End position: ") walls = input("Wall positions: ") #Initialize custom graph, and gui; If the graph is not too large if size[0] <= max_breadth and size[1] <= max_height: custom_graph = Graph(columns=size[0], rows=size[1], a_pos_x=start[0], a_pos_y=start[1], b_pos_x=end[0], b_pos_y=end[1], walls=walls) #Scale down the size if the graph is too large if size[0] > 50 and size[1] > 30: size = 25 / 5 else: size = 25 custom_graph_gui = gui.GUI(graph=custom_graph, cellsize=size) _run_gui(custom_graph_gui) else: print "Error: graph too large" #Run a speed test for algorithm optimalization purposes elif the_input.startswith("Speed test"): number = the_input[11:] line_list = map(int, re.findall(r'\d+', file_list[int(number)])) #Add the walls to a list walls = [] for wall_start in range(7, len(line_list), 4): wall = [] for wall_number in range(wall_start, wall_start + 4): wall.append(line_list[wall_number]) walls.append(wall) #Initialize the graph and run AStar graph = Graph(columns=line_list[1], rows=line_list[2], a_pos_x=line_list[3], a_pos_y=line_list[4], b_pos_x=line_list[5], b_pos_y=line_list[6], walls=walls) for run in range(10): search = a_star.AStar(graph) a = datetime.datetime.now() search.initialize("manhattan distance") result = search.complete_solver() b = datetime.datetime.now() print result[1], "\n", b - a #Exit the loop elif the_input == "Exit": flag = False f.close()
point_map.point_list_numpy[end_index][3], c='r', label="B点") # 绘制B点 ax.text(point_map.point_list_numpy[0][1] + 100, point_map.point_list_numpy[0][2] + 100, point_map.point_list_numpy[0][3] + 100, "A点") ax.text(point_map.point_list_numpy[end_index][1] + 100, point_map.point_list_numpy[end_index][2] + 100, point_map.point_list_numpy[end_index][3] + 100, "B点") # 设置坐标轴 ax.set_zlabel('Z') ax.set_ylabel('Y') ax.set_xlabel('X') plt.legend(loc="best") # plt.show() # 创建起终点point对象 start_point = point.Point(point_map.point_list_numpy[0][0], point_map.point_list_numpy[0][1], point_map.point_list_numpy[0][2], point_map.point_list_numpy[0][3], point_map.point_list_numpy[0][4], point_map.point_list_numpy[0][5]) end_point = point.Point(point_map.point_list_numpy[end_index][0], point_map.point_list_numpy[end_index][1], point_map.point_list_numpy[end_index][2], point_map.point_list_numpy[end_index][3], point_map.point_list_numpy[end_index][4], point_map.point_list_numpy[end_index][5]) a_star = a_star.AStar(point_map, start_point, end_point) a_star.RunAndSaveImage(ax, plt)