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 __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")
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): #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 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 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, 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)
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 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 __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 performance_computation(): n_times = 100 adaptive = AdaptiveAStar(maze=def_mazes[1]) a_star = AStar(maze=def_mazes[1]) reverse_a_star = ReverseAStar(maze=def_mazes[1]) all_a_star_moves = [] all_adaptive_moves = [] all_reverse_moves = [] adaptive_a_star_times = [] a_star_times = [] reverse_a_star_times = [] for _ in range(n_times): new_start = a_star.maze.random_cell() a_star.maze.start = new_start a_star.reset_bot() a_star_time_start = time.time() a_star_pops, a_star_moves = a_star.run_a_star_with_bot() a_star.pop_counter = 0 all_a_star_moves.append(a_star_moves) a_star_times.append(round(time.time() - a_star_time_start, 4)) reverse_a_star_time_start = time.time() reverse_a_star.reset_bot() reverse_a_star.maze.start = new_start reverse_pops, reverse_moves = reverse_a_star.run_reverse_a_star() all_reverse_moves.append(reverse_moves) reverse_a_star_times.append(round(time.time() - reverse_a_star_time_start, 4)) adaptive_time_start = time.time() adaptive.maze.start = adaptive.maze.maze[a_star.maze.start.row][a_star.maze.start.col] adaptive.reset_bot() adaptive_pops, adaptive_moves = adaptive.execute_algorithm() adaptive.pop_counter = 0 all_adaptive_moves.append(adaptive_moves) adaptive_a_star_times.append(round(time.time() - adaptive_time_start, 4)) moves_obj = { "adaptive_moves": all_adaptive_moves, "a_star_moves": all_a_star_moves, "reverse_moves": all_reverse_moves } times_obj = { "adaptive_times": adaptive_a_star_times, "a_star_times": a_star_times, "reverse_times": reverse_a_star_times } with open("moves_new.json", "w") as moves_file: json.dump(moves_obj, moves_file) with open("times_new.json", "w") as times_file: json.dump(times_obj, times_file)
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 _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 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 __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 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 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 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 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 get_algorithm(algorithm_num, board, data): if algorithm_num == 0: return AStar(board, data) elif algorithm_num == 1: return IDAStar(board, data) elif algorithm_num == 2: return BiDirAStar(board, data) elif algorithm_num == 3: return ReinforcementLearning(board, data) else: print("Algorithm cannot be: " + str(algorithm_num)) exit(1)
def callback(directions): a_star = AStar() if directions.forward: a_star.motors(200, 200) elif directions.back: a_star.motors(-200, -200) elif directions.left: a_star.motors(200, -200) elif directions.right: a_star.motors(-200, 200) else: a_star.motors(0, 0)
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 TestSensor(unittest.TestCase): a_star = AStar() tcs = Adafruit_TCS34725.TCS34725() tcs.set_interrupt(False) turnWheelSpeed=70 def testSensorColor(self): self.assertTrue(getSensorColor(self.tcs)=='red' or getSensorColor(self.tcs)=='green' or getSensorColor(self.tcs)=='blue') def testRecalibrateLeft(self): calibrateLeft(self.a_star, self.tcs, 1000, self.turnWheelSpeed) self.assertTrue(checkIfGreen(self.tcs)) def testRecalibrateRight(self): calibrateRight(self.a_star, self.tcs, 1000, self.turnWheelSpeed) self.assertTrue(checkIfGreen(self.tcs))
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 start_search(event=None): global start, goal, walls, map_creation, a_star if map_creation.start == None or map_creation.goal == None: map_creation.random_map() start = map_creation.start goal = map_creation.goal walls = map_creation.walls a_star = AStar(start, goal, R_C, R_C, walls) t2 = threading.Thread(target=star_thread) t2.daemon = True t2.start() draw_path()
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 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 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