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))
