def solve_board(self): while not self.is_solution_optimal(): algorithm = AStar(self.board, self.data) self.solution = algorithm.solve_board() self.update_weights() self.write_output() return self.solution
def __init__(self): self.directionFacing = 'north' self.maxSpeed = 100 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1]
def __init__(self): #Create a_star instance to talk to arduino based a_star Romi board self.a_star = AStar() #self.a_star = MockBot() self.encoder_resolution = 12 #according to pololu encoder count self.gear_reduction = 120 #according to romi website self.wheel_diameter = 0.07 #70mm diameter wheels according to website self.wheel_track = 0.141 #149mm outside to outside according to drawing, 8mm thick tires self.motorMax = 400 #internal data self.leftEncoder = None #last encoder state variables for odometry self.rightEncoder = None self.x = 0 self.y = 0 self.th = 0 self.leftMotor = 0 self.rightMotor = 0 #time variables for odometry pulled from http://github.com/hbrobotics/ros_arduino_bridge self.rate = float(50) #hard coding this for now, will be param'd later self.tickes_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi) now = rospy.Time.now() self.then = now self.t_delta = rospy.Duration(1.0 / self.rate) self.t_next = now + self.t_delta rospy.Subscriber("lmotor_cmd", Float32, self.lmotorCallback) rospy.Subscriber("rmotor_cmd", Float32, self.rmotorCallback) self.lwheelPub = rospy.Publisher('lwheel', Int16, queue_size=5) self.rwheelPub = rospy.Publisher('rwheel', Int16, queue_size=5)
def __init__(self, graph, agents, model_name, volume_conflict_table, min_cost_table=0): self.graph = graph self.volume_conflict_table = volume_conflict_table if min_cost_table != 0: self.min_cost_path_table = min_cost_table else: try: with open(model_name + '.mct', 'r') as f: data = f.read() if data != '': self.min_cost_path_table = ast.literal_eval(data) except FileNotFoundError: dj = Dijkstra(graph) dj.traverse() self.min_cost_path_table = dj.paths with open(model_name + '.mct', 'w') as f: f.write(str(self.min_cost_path_table)) self.agents = agents self.agent_dict = {} self.make_agent_dict() self.constraints = Constraints() self.constraint_dict = {} self.a_star = AStar(self)
def __init__(self): self.a_star = AStar() self.imu = LSM6() self.g_y_zero = 0 self.angle = 0 # degrees self.angle_rate = 0 # degrees/s self.distance_left = 0 self.speed_left = 0 self.drive_left = 0 self.last_counts_left = 0 self.distance_right = 0 self.speed_right = 0 self.drive_right = 0 self.last_counts_right = 0 self.motor_speed = 0 self.balancing = False self.calibrated = False self.running = False self.next_update = 0 self.update_thread = None self.cur_cmd = balance_cmd() self.cur_status = 0 self.adj_speed_left = 0 self.adj_speed_right = 0 self.lock = threading.Lock()
def solve(): global cont_solve, animation, drawn_spaces, checked file = "\n".join([ "".join([ "e" if s.end else ("s" if s.start else ("#" if s.wall else " ")) for s in row ]) for row in grid ]) with open("maze.txt", "w") as f: f.write(file) a = AStar(grid_w, grid_h, True, False) path, checked = a.solve() for row in grid: for cell in row: cell.path = False cell.checked = False if path is not None: cont_solve = True drawn_spaces = [] for p in path: grid[p.y][p.x].path = True for c in checked: try: grid[c.y][c.x].checked = True except Exception as e: print(e, c.x, c.y) else: print("NOT SOLVEABLE")
class Follower: def __init__(self): self.bridge = cv_bridge.CvBridge() # cv2.namedWindow("window", 1) self.image_sub = rospy.Subscriber('cv_camera/image_raw', Image, self.image_callback) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.twist = Twist() self.a_star = AStar() def image_callback(self, msg): image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) lower_black = numpy.array([0, 0, 0]) upper_black = numpy.array([120, 100, 80]) mask = cv2.inRange(hsv, lower_black, upper_black) h, w, d = image.shape search_top = 3 * h / 4 search_bot = 3 * h / 4 + 20 mask[0:search_top, 0:w] = 0 mask[search_bot:h, 0:w] = 0 M = cv2.moments(mask) if M['m00'] > 0: cx = int(M['m10'] / M['m00']) cy = int(M['m01'] / M['m00']) # cv2.circle(image, (cx, cy), 20, (0,0,255), -1) cv2.circle(mask, (cx, cy), 20, (0, 0, 255), -1) # BEGIN CONTROL err = cx - w / 2 self.twist.linear.x = 0.2 self.twist.angular.z = -float(err) / 100 self.cmd_vel_pub.publish(self.twist) self.a_star.motors(int(40 + 0.3 * err), int(40 - 0.3 * err))
def __init__(self): self.bridge = cv_bridge.CvBridge() # cv2.namedWindow("window", 1) self.image_sub = rospy.Subscriber('cv_camera/image_raw', Image, self.image_callback) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.twist = Twist() self.a_star = AStar()
def h_cost(self, s): plan = AStar((s[0], s[1]), (self._e[0], self._e[1]), self._map_info) if plan.run(display=False): path = plan.reconstruct_path() d = 0 for i in range(len(path) - 1): d += self.distance(path[i], path[i + 1]) return d
def __init__(self): self.isLost = False self.nextSearchDirection = 0 # (left: 0, right: 1) self.veerSpeed = 30 self.maxSpeed = 70 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1]
def solve_board(self): a_star = AStar(self.board, self.data) root_state = State(self.board, None, None, 0, 0, 0) f_limit = root_state.calculate_f(self.data) while True: solution_str = a_star.solve_board(f_limit) if solution_str is not None: return solution_str f_limit = f_limit + 5 return None
def solve_a_star(self): world = deepcopy(self.world) x = int(self.robot_x / self.cell_width) y = int(self.robot_y / self.cell_height) world[x][y] = -1 a_star = AStar(world, start_point=(x, y), end_point=(self.goal_x, self.goal_y)) a_star.a_star() return a_star.get_path()
def run(self): start_node = NavNode(position=self.board.start, g=0) a_star = AStar( draw=self.gfx.draw if not self.disable_gfx else lambda _: 0, disable_gfx=self.disable_gfx, draw_every=self.draw_every, print_path=self.print_path ) a_star.run(start_node=start_node)
def initialize(): new_graph = gs.Graph() setup.build_graph(new_graph) a_star = AStar() a_star.search(new_graph, '15', '35') standard_links = a_star.standard_links transition_links = a_star.transition_links roundabout_links = a_star.roundabout_links chart.render_chart(roundabout_links, transition_links, standard_links)
def game_loop(start_point, fields_with_bombs): # glowna petla obj_fields = [] a = AStar() current_field = start_point field_to_move = fields_with_bombs[0] fields_with_bombs.remove(field_to_move) path = a.find_path(current_field, field_to_move, map_obj) for y in game_map: for x in y: if not x.has_bomb: r = random.randrange(200) if r % 7 == 0: print('Object position {}'.format(x.get_position())) obj_fields.insert(len(obj_fields), x) # gameDisplay.blit(rock_img, (x.map_x, x.map_y)) print('Ilosc obiektow: {}'.format(len(obj_fields))) while True: for event in pygame.event.get(): if event.type == pygame.QUIT: quit() for y in game_map: for x in y: gameDisplay.fill(x.color, (x.map_x, x.map_y, settings.FIELD_SIZE, settings.FIELD_SIZE)) if x.has_bomb: gameDisplay.blit(bomba_img, (x.map_x, x.map_y)) if len(path) > 0: current_field = path[0] move_robot(current_field) path.remove(current_field) elif len(fields_with_bombs) > 0: field_to_move = fields_with_bombs[0] fields_with_bombs.remove(field_to_move) path = a.find_path(current_field, field_to_move, map_obj) move_robot(current_field) if current_field == field_to_move: make_action_with_bomb(current_field, settings.LABEL_FILE_1, settings.MODEL_FILE_1) current_field.has_bomb = False sleep(1) if len(fields_with_bombs) == 0: quit() pygame.display.update() clock.tick(3)
def keys_mtr(kmsg): a_star = AStar() if len(kmsg.data) == 0 or not key_mapping.has_key(kmsg.data[0]): return mtrs = key_mapping[kmsg.data[0]] mtrs = np.array(mtrs, dtype='int16') mleft = mtrs[0] mright = mtrs[1] # print(mleft, mleft.dtype) # print(mright, mright.dtype) a_star.motors(mleft, mright) time.sleep(0.1)
def __init__(self, dimension, agents, obstacles): self.dimension = dimension self.obstacles = obstacles self.agents = agents self.agent_dict = {} self.make_agent_dict() self.constraints = Constraints() self.constraint_dict = {} self.a_star = AStar(self)
class PlayerAIAStar(PlayerAI): def __init__(self, snake, food, box, wall_list): super().__init__(snake, food, box) self.obstacle_list = [] self.a_star = AStar(box) self.wall_list = wall_list def get_move(self): return self.get_a_star_move() def get_a_star_move(self): self.obstacle_list = self.get_obstacle_list() move_list = self.a_star.get_a_star_move_list(self.head_position(), self.objective_position(), self.obstacle_list) try: move = move_list[0] except: # No path to food possible move = self.survive() return self.get_direction(self.head_position(), move) def survive(self): neighbors = self.a_star.get_neighbors(self.head_position(), self.obstacle_list, direction_dependant=True, direction=self.snake.direction) move = self.head_position() if neighbors: # If not empty move = neighbors[0] return move def get_obstacle_list(self): obstacle_list = [] for body in self.snake.body: if body != self.snake.head(): obstacle_list.append(body.position) for wall in self.wall_list: if wall not in obstacle_list: obstacle_list.append(wall) return obstacle_list def reset_lists(self): self.obstacle_list.clear()
def __init__(self, is_training=True): """ """ rospy.init_node('deepq_carnode') self.rate = rospy.Rate(2) # send 2 observations per second self.camera = picamera.PiCamera() # self.camera.vflip = True # self.camera.hflip = True self.output = picamera.array.PiRGBArray(self.camera) self.last_state = None self.crashed = False self.dead_counter = 0 self.observations = deque() self.width = 320 self.height = 240 self.camera.resolution = (self.width, self.height) self.publish_train = False self.terminal_frame = False self.trainer = rospy.Publisher('/pi_car/start_training', Bool, queue_size=1) self.pub = rospy.Publisher('/pi_car/observation', String, queue_size=15, latch=True) rospy.Subscriber('/pi_car/cmd_resume', Bool, self.resume) self._run = True self._wait = False self.driver = AStar() self.previous_controls = (65, 65) self.current_controls = (75, 35) self.distance_from_router = 1.0 self.recovery_count = 0 self.reward_is_stuck = -.005 self.global_counter = 0 self.recovery_at_counter = 0 self._is_training = True if not self._is_training: self._tf._create_convolutional_network()
def reset_algorithm(self): """Resets the algorithm""" self.costmap_widget.canvas.freeze = True self.costmap[:] = 0.0 Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic) self.costmap_widget.canvas.freeze = False self.costmap_widget.canvas.on_map_update()
def __init__(self, width, height): pygame.init() self.grid = Graph(27, 19) self.screen = pygame.display.set_mode((width, height)) self.start_node = Node(None) self.goal_node = Node(None) self.astar = AStar(self.grid, self.start_node, self.goal_node) self.visual_graph = GraphVisual(self.astar, 40, self.screen)
class mainWindow(): times=1 timestart=time.clock() def __init__(self): gamedata = pickle.load(open('C:/gamedata.p', 'rb')) blockHeights = gamedata['blockHeights'] vis_map = get_visibility_map(blockHeights) self.astar = AStar(blockHeights, vis_map.tolist()) self.start_point = (0,0) self.vis_map = vis_map blocks = np.array(self.vis_map) self.blocks = blocks.transpose() self.end_point = (40,25) self.root = Tkinter.Tk() self.frame = Tkinter.Frame(self.root, width=1024, height=768) self.frame.pack() self.canvas = Tkinter.Canvas(self.frame, width=1024,height=768) self.canvas.place(x=-2,y=-2) self.root.after(100,self.start) # INCREASE THE 0 TO SLOW IT DOWN self.root.mainloop() def start(self): try: self.im = Image.fromarray(np.uint8(cm.gist_yarg(self.blocks)*255)) if self.start_point[0] < self.blocks.shape[1]-1: self.start_point = (self.start_point[0]+1, self.start_point[1]) path = self.astar.get_path(self.start_point[0], self.start_point[1], self.end_point[0], self.end_point[1]) self.im.putpixel(self.start_point,(0,255,0)) for x in path: self.im.putpixel((x[0], x[1]),(255,0,0)) self.im.putpixel(self.end_point,(0,0,255)) newy, newx = self.blocks.shape scale = 8 self.im = self.im.resize((newx*scale, newy*scale)) self.photo = ImageTk.PhotoImage(image=self.im) self.canvas.create_image(0,0,image=self.photo,anchor=Tkinter.NW) self.root.update() self.times+=1 if self.times%33==0: print "%.02f FPS"%(self.times/(time.clock()-self.timestart)) self.root.after(2000,self.start) except Exception as e: print e self.root.after(10,self.start)
def main(): # ヒューリスティクス関数のリスト heuristics_func_list = [ calc_euclidean_distance, calc_manhattan_distance, all_0 ] ## 標準入出力を利用し、対話的に設定を行う print("# 読み込むファイル名を入力してください(何も入力しない場合はmap.csvの読み込みを試みます)") f_name = input() if f_name == '': a_star = AStar() else: a_star = AStar(f_name=f_name) a_star.print_map() print("# スタートの位置を空白区切りで入力してください(X, Yの順)") start = tuple(map(int, input().split())) print("# ゴールの位置を空白区切りで入力してください(X, Yの順)") goal = tuple(map(int, input().split())) print("# 使用するヒューリスティクス関数を以下の番号から指定してください") for i in range(len(heuristics_func_list)): print("# %d: %s" % (i, heuristics_func_list[i].__name__)) func_index = int(input()) route = a_star.search(start, goal, heuristics_func_list[func_index]) print("route: ", end='') for x, y in route: print("[%s, %s], " % (x, y), end='') print("")
def get_solve_graph(algo_enum: AlgoEnum): solve_graph: BaseGlobalSearch = None if algo_enum == AlgoEnum.IDS: solve_graph = IDS(order_int_list, N) elif algo_enum == AlgoEnum.BFS: solve_graph = BFS(order_int_list, N) elif algo_enum == AlgoEnum.A_STAR: solve_graph = AStar(order_int_list, N) return solve_graph
def get_route(vehicle): if vehicle.path_finder_algorithm == PathFinderAlgorithm.VALIDATOR: """ DIJKSTRA """ start = time.time() d_path, d_visited = Dijkstra.find_route_nodes(vehicle) path = PathFinder._print_route_results(start, d_path, d_visited, vehicle, 'Dijkstra') """ ALT ALGORITHM """ start = time.time() alt_path, alt_visited = Alt.find_route_nodes(vehicle) PathFinder._print_route_results(start, alt_path, alt_visited, vehicle, 'ALT') """ A STAR ALGORITHM """ start = time.time() a_star_path, a_star_visited = AStar.find_route_nodes(vehicle) PathFinder._print_route_results(start, a_star_path, a_star_visited, vehicle, 'A*') if GeneralSettings.debug_print: print "--------------------------------------------------------" return path if vehicle.path_finder_algorithm == PathFinderAlgorithm.DIJKSTRA: start = time.time() path, _ = Dijkstra.find_route_nodes(vehicle) if GeneralSettings.debug_print: print("Dijkstra.get_route elapsed time: %.2f ms" % ((time.time() - start) * 1000)) return path elif vehicle.path_finder_algorithm == PathFinderAlgorithm.ALT: start = time.time() path, _ = Alt.find_route_nodes(vehicle) if GeneralSettings.debug_print: print("Alt.get_route elapsed time: %.2f ms" % ((time.time() - start) * 1000)) return path elif vehicle.path_finder_algorithm == PathFinderAlgorithm.A_STAR: start = time.time() path, _ = AStar.find_route_nodes(vehicle) if GeneralSettings.debug_print: print("A star.get_route elapsed time: %.2f ms" % ((time.time() - start) * 1000)) return path else: raise ValueError("Invalid vehicles path finder algorithm value.")
def __init__(self, a_star=None): self.a_star = a_star or AStar() self.encoders = Encoders(self.a_star) self.odometer = Odometer(self.encoders) self.motors = Motors(self.a_star, self.odometer) self.camera = Camera() self.log = logging.getLogger('romi') self.motors.stop()
def get_all_paths(map2d): big_list=dict() for x_start in range(0,12): for y_start in range(0,12): for x_end in range(0,12): for y_end in range(0,12): if x_start == x_end and y_start == y_end: continue elif map2d[x_start][y_start] == 1 or map2d[x_end][y_end] == 1: continue astar = AStar(map2d, Point(x_start, y_start), Point(x_end, y_end)) try: path_list = [(p.y, p.x) for p in astar.start()] except: continue big_list[(y_start, x_start, y_end, x_end)] = path_list # print(list(big_list.keys())[10], big_list[list(big_list.keys())[10]]) return big_list
def _get_path(start_x, start_y, end_x, end_y, cell_table): # Columns and rows set to 0 because they are reassigned when the maze file is read a = list( reversed(AStar(0, 0).solve((start_y, start_x), (end_y, end_x))[0])) # Reverse keys and values cell_table = {v: k for k, v in cell_table.items()} # Get the acc cell positions of path return [cell_table[(a[i].x, a[i].y)] for i in range(0, len(a), 2)]
def go_home(map2d, x,y,home_x,home_y): print(x, y) astar = AStar(map2d, Point(y, x), Point(home_y, home_x)) path_list = [(p.y, p.x) for p in astar.start()] print(path_list) # action = "S" if path_list[0][0]==x: if path_list[0][1] > y: action='R' else: action='L' else: if path_list[0][0] > x: action='D' else: action='U' return action
def __init__(self, width, height): super().__init__((width, height), pygame.SRCALPHA) self.__width = width self.__height = height self.__grid = self.load_maze("data/maze.txt") cell_num_y = ["X" for _ in self.__grid].count("X") cell_num_x = self.__grid[1].count("X") # All cell positions self.cell_pos = [[ Cell(9 * j + (1 * (j + 1)), 9 * i + (1 * (i + 1)), 1) for j in range(cell_num_x) ] for i in range(cell_num_y)] # All horizontal walls self.hori_wall_pos = [[ Wall(10 * j + 1, 10 * i, 1, 2) for j in range(cell_num_x) ] for i in range(cell_num_y)] # All vertical walls self.vert_wall_pos = [[ Wall(10 * j, 10 * i + 1, 1, 1) for j in range(cell_num_x) ] for i in range(cell_num_y)] # All horizontal doors self.hori_door_pos = [[ Door(10 * (j // 2), 10 * (i // 2), 0 if self.__grid[i][j + 1] == "-" else 1, 2) for j in range(0, len(self.__grid[0]) - 1, 2) ] for i in range(0, len(self.__grid), 2)] # All vertical doors self.vert_door_pos = [[ Door(10 * (j // 2), 10 * (i // 2), 0 if self.__grid[i][j - 1] == "|" else 1, 1) for j in range(1, len(self.__grid[0]) + 1, 2) ] for i in range(1, len(self.__grid), 2)] self.maze = AStar(0, 0) path = self.maze.solve()[0] self.cell_colours = [[(255, 255, 255) if self.maze.grid[i][j] not in path else (0, 255, 0) for j in range(1, len(self.maze.grid[0]), 2)] for i in range(1, len(self.maze.grid), 2)]
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=True) self.costmap_widget.canvas.show_start = True self.costmap_widget.canvas.show_goal = True temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic)
def get_order(self, commander, bot): exact_target = commander.level.findRandomFreePositionInBox((commander.game.team.flag.position - 5.0, commander.game.team.flag.position + 5.0)) pomastar = AStar(commander.gamedata['blockHeights'], commander.twodpom) path = pomastar.get_path(bot.position.x, bot.position.y, exact_target.x, exact_target.y) try: if len(path) > 10: exact_target = Vector2(*path[len(path)/2]) target = commander.level.findRandomFreePositionInBox((exact_target-5.0, exact_target+5.0)) else: target = commander.game.team.flag.position # print target if not target: raise "no target found" except: print 'couldnt convert target to vec2' target = flagScoreLocation = commander.game.team.flagScoreLocation targetMin = target - Vector2(2.0, 2.0) targetMax = target + Vector2(2.0, 2.0) goal = commander.level.findRandomFreePositionInBox([targetMin, targetMax]) if not goal: return None, None if (goal - bot.position).length() > 8.0: return (orders.Charge, commander.defender, goal), {'description' : 'rushing to defend'} else: return (orders.Defend, commander.defender, (commander.middle - bot.position)), {'description' : 'defending'}
def reset_algorithm(self): """Resets the algorithm""" self.costmap_widget.canvas.freeze = True self.costmap[:] = 0.0 Obstacle(3,3,3,3).draw(self.costmap) Obstacle(9,5,3,3).draw(self.costmap) Obstacle(16,4,3,3).draw(self.costmap) temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic) self.costmap_widget.canvas.freeze = False self.costmap_widget.canvas.on_map_update()
def __init__(self): gamedata = pickle.load(open('C:/gamedata.p', 'rb')) blockHeights = gamedata['blockHeights'] vis_map = get_visibility_map(blockHeights) self.astar = AStar(blockHeights, vis_map.tolist()) self.start_point = (0,0) self.vis_map = vis_map blocks = np.array(self.vis_map) self.blocks = blocks.transpose() self.end_point = (40,25) self.root = Tkinter.Tk() self.frame = Tkinter.Frame(self.root, width=1024, height=768) self.frame.pack() self.canvas = Tkinter.Canvas(self.frame, width=1024,height=768) self.canvas.place(x=-2,y=-2) self.root.after(100,self.start) # INCREASE THE 0 TO SLOW IT DOWN self.root.mainloop()
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3,3,3,3).draw(self.costmap) Obstacle(9,5,3,3).draw(self.costmap) Obstacle(16,4,3,3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=True) self.costmap_widget.canvas.show_start = True self.costmap_widget.canvas.show_goal = True temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic)
#!/usr/bin/python from a_star import AStar import time a_star = AStar() a_star.leds(0,0,0) time.sleep(0.3) a_star.leds(1,0,0) time.sleep(0.3) a_star.leds(1,1,0) time.sleep(0.3) a_star.leds(0,1,1) time.sleep(0.3) a_star.leds(0,0,1) time.sleep(0.3) a_star.leds(0,0,0) time.sleep(0.3) a_star.leds(0,0,1) time.sleep(0.3) a_star.leds(0,1,1) time.sleep(0.3) a_star.leds(1,1,0) time.sleep(0.3) a_star.leds(1,0,0) time.sleep(0.3) a_star.leds(0,0,0)
from a_star import AStar a_star = AStar() print(a_star.read_battery_millivolts())
return (rand_x,rand_y) gridValues = [[0 for y in range(GRID_HEIGHT)] for x in range(GRID_WIDTH)] currentRobotPositions = [(1,1,),(5,20),(10,40)] numRobots = len(currentRobotPositions) for r in range(0,numRobots): fillRobotSpace(gridValues,currentRobotPositions[r],ROBOT_SIZE,r+4) insertObtsacles(40, gridValues, OBSTACLE_SIZE, AStar.OBSTACLE_VAL) goalPos = insertFinishPt(AStar.FINISH_PT_VAL, gridValues,FINISH_PT_SIZE) aStarProb = AStar(gridValues,currentRobotPositions[0],goalPos,ROBOT_SIZE) NUM_STEPS = 10 NUM_ITERATIONS = 12 for i in range(0,NUM_ITERATIONS): for r in range(0,numRobots): if(currentRobotPositions[r] != None): aStarProb.robotStartPos = currentRobotPositions[r] aStarProb.robotId = r + 4 directions = aStarProb.getDirectionsToGoal() #print directions nextPos = aStarProb.fillGridWithDirections(directions,NUM_STEPS,gridValues) #print nextPos currentRobotPositions[r] = nextPos
def main(args): # initialize pygame pygame.init() # set screen size of phone or desktop window. Adjust to your phone screen = pygame.display.set_mode((1024, 768)) pygame.display.set_caption("Use keyboard arrow keys or mouse. Press TEST to toggle overlay") # game framerate. Higher number is faster FPS = 40 # change the file names to your player graphic and map file player_image_file = "img/direction_24px.png" map_file = "maps/mega_map.json" # change to False (with capital F) to turn off red squares over # collision rectangles TESTING = True # Loading Paths (complete paths to all sections and from section start to parking slots) filename = "maps/paths.json" path = [] paths = loader.json_to_dict(filename) a_star = AStar(filename) # print a_star #initialize json loader, build tileset list, load player graphic initial = json_loader.Initialize(screen, TESTING, map_file, player_image_file, paths, 80) # initialize position of player when game first starts initial_position = (-4574,-4837) map = json_loader.Map(initial, initial_position) map.move(initial_position[0], initial_position[1]) #map.move_to_tile([146,153]) # handle events such as keyboard / touchscreen presses event = json_loader.Event(initial) clock = pygame.time.Clock() # Generating pathfile if args.generate_path: pathfile = open("path"+ str(random.randint(1,100)), 'w') new_path = [] sections = paths["sections"] section = sections[0] slot = -1 new_car = 0 [x,y] = [0,0] students = 10 # path = a_star.a_star(a_star.graph,"Caseta Policia")[0] while True: event.update() if event.direction == "start" and not path: #new_car = random.randint(1,10) map.player.change_car_image("img/car_24px_"+str(students)+".png") astar = a_star.a_star(a_star.graph,"Residencias 1") path = astar[0] a_star.rebuild_graph(astar[1]) #slot += 1 #path = [x for x in section["path_from_start"]] #path += section["slots"][slot]["path_from_section_start"] #path.reverse() if path: move = path.pop() map.move_to_tile(move) time.sleep(.1) event.direction = "car_moving" if not path: map.change_tile(move, 3, students-1) map.player.change_car_image("img/direction_24px.png") event.direction = "stop" students = students - 1 map.update(event.direction) event.direction = "stop" # Path file helper if args.generate_path: if x != map.mapx or y != map.mapy: tile = [map.player.position[0], map.player.position[1]] if tile not in new_path: new_path.append(tile) pathfile.write(str(tile)+",") x = map.mapx y = map.mapy map.display(screen) clock.tick(FPS) pygame.display.update() event.update()
class AStarAlgorithmWidget(AlgorithmWidget): def __init__(self, parent=None): self.heuristic = naive AlgorithmWidget.__init__(self, "A* Algorithm", parent) self.combo_box = QtGui.QComboBox(parent) self.combo_box.addItems(["naive", 'manhattan', 'crow']) self.combo_box.currentIndexChanged.connect(self.select_heuristic) self.buttons.append(self.combo_box) self.pack_buttons() def select_heuristic(self, index): if index == 0: self.heuristic = naive elif index == 1: self.heuristic = manhattan elif index == 2: self.heuristic = crow self.a_star.heuristic_cost_estimate = self.heuristic def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3,3,3,3).draw(self.costmap) Obstacle(9,5,3,3).draw(self.costmap) Obstacle(16,4,3,3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=True) self.costmap_widget.canvas.show_start = True self.costmap_widget.canvas.show_goal = True temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic) def step_solution(self): """Steps the solution""" # self.costmap_widget.canvas.freeze = True # count = 0 # while count < 25: # count += 1 # result = self.a_star.step_solution() # if result == False: # self.costmap_widget.canvas.freeze = False # self.costmap_widget.canvas.on_map_update() # return result # self.costmap_widget.canvas.freeze = False # self.costmap_widget.canvas.on_map_update() # return True return self.a_star.step_solution() def reset_algorithm(self): """Resets the algorithm""" self.costmap_widget.canvas.freeze = True self.costmap[:] = 0.0 Obstacle(3,3,3,3).draw(self.costmap) Obstacle(9,5,3,3).draw(self.costmap) Obstacle(16,4,3,3).draw(self.costmap) temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0]+0.5), floor(temp[1]+0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic) self.costmap_widget.canvas.freeze = False self.costmap_widget.canvas.on_map_update()
def cover_xytable(self): total_distance = 0 flood_covered = [[False for y in col] for col in self.rockpoint] # return all manhattan neighbors def get_neighbors(loc): xx, yy = loc return filter(self.is_in_bounds, [ (xx - 1, yy), (xx, yy - 1), (xx + 1, yy), (xx, yy + 1), ]) S = [(self.radius + 1, self.radius + 1)] # start at 10,10 for funsies self.path = [(self.radius + 1, self.radius + 1, True)] # mark path as exploratory current_location = None while 0 < len(S): new_loc = S.pop() (x, y) = new_loc if current_location is not None: (x0, y0) = current_location # don't do all this twice if flood_covered[x][y]: continue flood_covered[x][y] = True # set up the path planner, from the new point back to the current point # AStar(cost_fn, is_goal_fn, h_fn, successors_fn) cost_fn = lambda _, __ : 1 h_fn = lambda (x, y) : abs(x - x0) + abs(y - y0) # manhattan distance is_goal_fn = lambda path : path == current_location successors_fn = lambda node : [n for n in get_neighbors(node) if (self.is_visited(n) or n == current_location or n == new_loc)] path_planner = AStar(cost_fn, is_goal_fn, h_fn, successors_fn, self.hack_draw_planned_path) # steps to get us to new location current_to_new_location = path_planner.solve(new_loc) if current_to_new_location is None: self.draw_pathplan_failure(current_location, new_loc) raise AssertionError("Couldn't go from " + str(current_location) + " to " + str(new_loc)) # actually visit points total_distance += len(current_to_new_location) - 2 self.path += [(xx, yy, False) for (xx, yy) in current_to_new_location[1:-1]] current_location = self.path[-1][:2] # only get neighbors of points with no displacement... otherwise dead end if not self.visit_point(x, y): continue # we've arrived current_location = new_loc total_distance += 1 # add new neighbors to the list of places we need to check for neighbor in get_neighbors(new_loc): xx, yy = neighbor if not flood_covered[xx][yy]: if self.is_ball_contained(xx, yy): S.append(neighbor) print "Total distance is", total_distance, print "which has % efficiency", round(100.0 * len(self.get_visited_list()) / total_distance, 1)
# Copyright Pololu Corporation. For more information, see https://www.pololu.com/ from a_star import AStar a_star = AStar() a_star.play_notes("o4l16ceg>c8")
from a_star import AStar a_star = AStar() a_star.play("v10l8cg>cgc")
from a_star import AStar a_star = AStar() print(a_star.analog_read(2))