def get_path(self, current_loc, dest_loc): #Case 1: The current loc is in the open, not between shelves if dest_loc.pos_1 == current_loc[2] or dest_loc.pos_1 == current_loc[2]: path = astar.find_path(self.img,current_loc[0],dest_loc.pos) return path if current_loc[2] == (-1,-1): #Go to the loc of the shelf then to the item loc #print(dest_loc) #print(current_loc) if dest_loc.pos_1 != (-1, -1): enter_shelf_loc = dest_loc.pos_1 if self.L2_dis(dest_loc.pos_1,current_loc[0]) < self.L2_dis(dest_loc.pos_2,current_loc[0]) else dest_loc.pos_2 path1 = astar.find_path(self.img,current_loc[0],enter_shelf_loc) path2 = astar.find_path(self.img,path1[-1], dest_loc.pos) else: path1 = [] path2 = astar.find_path(self.img,current_loc[0], dest_loc.pos) #Update current_loc return path1 + path2 else: # Case 2: The current loc is between shelves #Exit the current shelf exit_shelf_loc = current_loc[2] if self.L2_dis(dest_loc.pos,current_loc[2]) < self.L2_dis(dest_loc.pos,current_loc[3]) else current_loc[3] path1 = astar.find_path(self.img,current_loc[0], exit_shelf_loc) if dest_loc.pos_1 != (-1, -1): enter_shelf_loc = dest_loc.pos_1 if self.L2_dis(dest_loc.pos_1,exit_shelf_loc) < self.L2_dis(dest_loc.pos_2,exit_shelf_loc) else dest_loc.pos_2 #Go to the new shelf path2 = astar.find_path(self.img,path1[-1],enter_shelf_loc) #Go to the item path3 = astar.find_path(self.img,path2[-1], dest_loc.pos) elif dest_loc.pos_1 == (-1, -1): path2 = [] path3 = astar.find_path(self.img,path1[-1], dest_loc.pos) return path1 + path2 +path3
def get_astar_path(world, start, goal):
ret_path = astar.find_path(world.get_nbor_cells,
start,
goal,
lambda cell: 1,
lambda cell: world.passable( cell ) )
return ret_path
def test_bestpath(self):
"""ensure that we take the shortest path, and not the path with less elements.
the path with less elements is A -> B with a distance of 100
the shortest path is A -> C -> D -> B with a distance of 60
"""
nodes = {
'A': [('B', 100), ('C', 20)],
'C': [('D', 20)],
'D': [('B', 20)]
}
def neighbors(n):
for n1, d in nodes[n]:
yield n1
def distance(n1, n2):
for n, d in nodes[n1]:
if n == n2:
return d
def cost(n, goal):
return 1
path = list(
astar.find_path('A',
'B',
neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost,
distance_between_fnct=distance))
self.assertEqual(4, len(path))
for i, n in enumerate('ACDB'):
self.assertEqual(n, path[i])
def test_astar():
e1 = Node(100, 200, 100, None)
e2 = Node(200, 200, 100, None)
e3 = Node(300, 200, 100, None)
e4 = Node(400, 200, 100, None)
e1.connect(e2)
e2.connect(e3)
e3.connect(e4)
def neighbors(n):
for n1 in n.get_connected_nodes():
yield n1
def distance(n1, n2):
for e in n1.edges:
if e.get_con_node(n1) == n2:
return e.length
def cost(n, goal):
return abs(n.x - goal.x)
path = list(
astar.find_path(
e1,
e4,
neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost,
distance_between_fnct=distance,
))
assert len(path) == 4
assert path[0] == e1
assert path[1] == e2
assert path[2] == e3
assert path[3] == e4
def find_path(origin, goal, walls):
origin, goal, all_nodes = init_map_graph(origin, goal, walls)
path = astar.find_path(origin, goal, neighbors_fnct=lambda n: n.neighbors, heuristic_cost_estimate_fnct=distance, distance_between_fnct=distance)
path = [[n.x, n.y] for n in path]
d = LineString(path).length
return path, d
def get_path(self, current_loc, dest_loc):
#Case 1: The current loc is in the open, not between shelves
if current_loc[2] == (-1,-1):
#Go to the loc of the shelf then to the item loc
print(dest_loc)
print(current_loc)
enter_shelf_loc = dest_loc[2] if L2_dis(dest_loc[2],current_loc[0]) < L2_dis(dest_loc[3],current_loc[0]) else dest_loc[3]
path1 = astar.find_path(self.img,current_loc[0],enter_shelf_loc)
path2 = astar.find_path(self.img,enter_shelf_loc, dest_loc[0])
#Update current_loc
return path1 + path2
else:
# Case 2: The current loc is between shelves
#Exit the current shelf
exit_shelf_loc = current_loc[2] if L2_dis(dest_loc[0],current_loc[2]) < L2_dis(dest_loc[0],current_loc[3]) else current_loc[3]
path1 = astar.find_path(self.img,current_loc[0], exit_shelf_loc)
enter_shelf_loc = dest_loc[2] if L2_dis(dest_loc[2],exit_shelf_loc) < L2_dis(dest_loc[3],exit_shelf_loc) else dest_loc[3]
#Go to the new shelf
path2 = astar.find_path(self.img,exit_shelf_loc,enter_shelf_loc)
#Go to the item
path3 = astar.find_path(self.img,enter_shelf_loc, dest_loc[0])
return path1 + path2 +path3
def get_path(s1, s2):
""" runs astar on the map"""
def ccw(A, B, C):
return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])
def intersect(A, B, C, D):
return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
def distance(n1, n2):
# n1, n2 co dang ('manual_ally', 'vertex1')
"""computes the distance between two vertices"""
if (n1[1] == 'vertex0' or n2[1] == 'vertex0'):
return 30041975
x1, y1 = vertices[n1[0]][n1[1]]
x2, y2 = vertices[n2[0]][n2[1]]
if (x1 < 0 or x2 < 0):
return 30041975
for objects in vertices:
if (objects == 'ourWall' or objects == 'theirWall'):
vertex1 = vertices[objects]['vertex1']
vertex2 = vertices[objects]['vertex2']
if (intersect((x1, y1), (x2, y2), vertex1, vertex2)):
return 30041975
if (len(vertices[objects]) == 5):
vertex1 = vertices[objects]['vertex1']
vertex2 = vertices[objects]['vertex2']
vertex3 = vertices[objects]['vertex3']
vertex4 = vertices[objects]['vertex4']
in13 = intersect((x1, y1), (x2, y2), vertex1, vertex3)
in24 = intersect((x1, y1), (x2, y2), vertex2, vertex4)
if (in13 or in24):
return 30041975
return (x1 - x2)**2 + (y1 - y2)**2
def neighbor(node):
s = []
for objects in vertices:
for vertex in vertices[objects]:
s.append((objects, vertex))
return s
return astar.find_path(s1,
s2,
neighbors_fnct=neighbor,
heuristic_cost_estimate_fnct=distance,
distance_between_fnct=distance)
def get_path(s1, s2):
""" runs astar on the map"""
def distance(n1, n2):
"""computes the distance between two stations"""
latA, longA = n1.position
latB, longB = n2.position
# convert degres to radians!!
latA, latB, longA, longB = map(
lambda d: d * math.pi / 180, (latA, latB, longA, longB))
x = (longB - longA) * math.cos((latA + latB) / 2)
y = latB - latA
return math.hypot(x, y)
return astar.find_path(s1, s2, neighbors_fnct=lambda s: s.links, heuristic_cost_estimate_fnct=distance, distance_between_fnct=distance)
def find_min_path(cent,finalC,nmap):
lengths=[]
minimum = 1300
print("dolzina na fc",len(finalC))
for i in range(0, len(finalC)):
print(type(nmap), nmap)
print('centre', type(cent), cent)
#print(type(finalC[i]), finalC[i].shape, finalC[i])
path = astar.find_path(nmap, cent, finalC[i])
lengths[i] = len(path)
if lengths[i] < minimum:
final_path = path
minimum = lengths[i]
da=finalC[i]
return final_path,da
def FindPath(self):
global current_loc
current_loc = [(60,30), 0, (-1,-1), (-1,-1)]
print(items_to_buy)
item_pick_up = items_to_buy.copy()
flag = 0
print('removed')
if self.dot_path == []:
pass
else:
for dots in self.dot_path:
self.canvas.remove(dots)
print('removed')
self.dot_path = []
while item_pick_up != []:
if item_pick_up == []:
path = astar.find_path(self.img,current_loc[0],addresses['check_out'][0])
current_loc = addresses['check_out']
flag = 1
near_item = 'check_out'
else:
near_item = find_nearest(current_loc[0],item_pick_up)
path= self.get_path(current_loc,addresses[near_item])
item_pick_up.remove(near_item)
current_loc = addresses[near_item]
#self.canvas.clear()
mylabel = CoreLabel(text=near_item, font_size=10, color=(1, 0, 0, 1))
mylabel.refresh()
# Get the texture and the texture size
texture = mylabel.texture
texture_size = list(texture.size)
with self.canvas:
Color(1.0,0,0)
for i in range(len(path)):
self.dot_path.append(Ellipse(pos = ((path[i][1] * 540 / 64) - 133,((64 - path[i][0]) * 540 / 64) - 6 ), size=(3, 3)))
#(x - 133,y)
Color(0,1,0)
self.dot_path.append(Ellipse(pos = ((path[-1][1] * 540 / 64) - 133,((64 - path[-1][0]) * 540 / 64) - 6 ), size=(5, 5)))
#(self.canvas.add(Rectangle(pos = ((path[-1][1] * 540 / 64) - 133 - texture.size[0] / 2,((64 - path[-1][0]) * 540 / 64) - 6 + 10), texture=texture, size=texture_size)))
#self.add_widget(Label(text = near_item, pos = (7 + (path[-1][1] * 540 / 64) - 133,((64 - path[-1][0]) * 540 / 64) - 6 )))
if flag:
break
def get_path(s1, s2, use_speed_average=False):
""" executa a busca A* """
def distance(n1, n2):
"""calcula a distancia entre dois cruzamentos"""
latA, longA = n1.position
latB, longB = n2.position
x = math.pow(latB - latA, 2)
y = math.pow(longB - longA, 2)
return math.sqrt(x + y)
def time(n1, n2, speed_average):
return distance(n1, n2) / speed_average
return astar.find_path(s1,
s2,
neighbors_fnct=lambda s: s.links,
heuristic_cost_estimate_fnct=time,
distance_between_fnct=distance,
use_speed_average=use_speed_average)
def test(filename, filename_solution):
import astar
import heuristics
load_problem(filename, astar.ProblemDefinition, filename_solution)
solution_node = astar.find_path([0], heuristics.remain_hops_by_min, 1945)
solution = str(tuple(solution_node.path))
solution_cost = solution_node.path_cost
expected_solution = str(astar.ProblemDefinition.SOLUTION)
expected_cost = astar.ProblemDefinition.SOLUTION_COST
print "Cost:", solution_cost, "Path:", solution
return
if expected_solution != solution or expected_cost != solution_cost:
print "Solution found : (%i) %s" % (solution_cost, solution)
print "Solution expected : (%i) %s" % (expected_cost,
expected_solution)
else:
print "Ok"
def get_shortest_path(self, grid, dest):
start = self.path[len(self.path) - 2]
end = self.state
print(start)
print(end)
if start != end:
start_loc_x = np.random.choice(dest[start + "_x"])
start_loc_y = np.random.choice(dest[start + "_y"])
end_loc_x = np.random.choice(dest[end + "_x"])
end_loc_y = np.random.choice(dest[end + "_y"])
print(start_loc_x)
print(start_loc_y)
print(end_loc_x)
print(end_loc_y)
path_to_dest = at.find_path(grid, (start_loc_x, start_loc_y),
(end_loc_x, end_loc_y),
at.possible_moves)[::-1]
self.path_to_dest = path_to_dest
print(self.path_to_dest)
def find_path(id, origin, goal, walls):
start_time = time.time()
origin, goal, wall_segments = init_map_graph(origin, goal, walls)
path = astar.find_path(origin, goal,
neighbors_fnct=lambda n: n.get_neigbors(),
heuristic_cost_estimate_fnct=distance,
distance_between_fnct=distance,
is_goal_reached_fnct=is_goal_reached_fnct
)
path = [[n.x, n.y] for n in path]
d = LineString(path).length
# print("path: ", origin, path[0][0], path[0][1], len(path), d)
print(f'Path {id} found in {time.time() - start_time : 0.2f} seconds with {len(path)} steps and {d:.2f} m')
return path, d
# gets the distance between n1 and n2
def distance(n1, n2):
for dst in nodes[n1]:
if dst['node2'] == n2:
return dst['distance']
# generic cost function
def cost(n, goal):
return 1
if __name__ == '__main__':
createGraph()
path = input("Enter two numbers (0,99) separated by a comma: ")
path_array = path.split(',')
start = time.time()
try:
path = list(
astar.find_path(int(path_array[0]),
int(path_array[1]),
neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost,
distance_between_fnct=distance))
print(path)
except TypeError:
print("No path found")
end = time.time()
total_time = end - start
print("Time Elasped: ", total_time)
capture = True #we going to use this variable to make a infinity while cycle
while capture:
screen = camera.get_image(screen)
img = pygame.surfarray.array3d(screen)
plt.imshow(screen)
plt.show()
display.blit(screen,(0,0))
pygame.display.flip()
# %%
# nmap=
# path = astar.find_path(nmap, (), (0,0))
nmap, random_red = masks.red_mask(img)
print(nmap)
# cv2.imshow('fig1',nmap)
nmap = np.array(nmap)
path = astar.find_path(nmap, (50, 50), (30, 20))
current = (50, 50)
for i in range(0, len(path),4):
nextp = path[i]
vel = np.sqrt((current[1] - nextp[1]) ** 2 + (nextp[0] - current[0]) ** 2)
# dev=float(nextp[1]-current[1])/(nextp[0]-current[0])
alfa = (np.pi) / 2 - (np.arctan2(nextp[1] - current[1], (nextp[0] - current[0])))
print(alfa)
alfad = (alfa * 180) / np.pi
print(alfad)
if int(alfad) <= 0:
sph.roll(int(vel), 360 + int(alfad))
else:
sph.roll(int(vel), int(alfad))
from numpy import *
from astar import find_path
# Edge cost matrix
M = loadtxt('M.dat')
N,N = M.shape
# Node positions
XX = loadtxt('Nodes.dat')
# Find the shortest path using A*
path = find_path(XX,M,0,12)
# Print the path
print("Path: %s" % str(path))
def go_to_bucharest():
"""ensure that we take the shortest path, and not the path with less elements.
the path with less elements is A -> B with a distance of 100
the shortest path is A -> C -> D -> B with a distance of 60
"""
nodes = {
'Arad': [('Sibiu', 140), ('Timisoara', 118), ('Zerind', 75)],
'Bucharest': [('Pitesti', 101), ('Fagaras', 211), ('Giurgiu', 90), ('Urziceni', 85)],
'Craiova': [('Dobreta', 120), ('Rimnicu Vikea', 146), ('Pitesti', 138)],
'Dobreta': [('Mehadia', 75), ('Craiova', 120)],
'Eforie': [('Hirsowa', 85)],
'Fagaras': [('Sibiu', 99), ('Bucharest', 211)],
'Giurgiu': [('Bucharest', 90)],
'Hirsowa': [('Urziceni', 98), ('Eforie', 85)],
'Iasi': [('Neamt', 87), ('Vaslui', 92)],
'Lugoj': [('Timisoara', 111), ('Mehadia', 70)],
'Mehadia': [('Lugoj', 70), ('Dobreta', 75)],
'Neamt': [('Iasi', 87)],
'Oradea': [('Zerind', 71), ('Sibiu', 151)],
'Pitesti': [('Rimnicu Vikea', 97), ('Craiova', 138), ('Bucharest', 101)],
'Rimnicu Vikea': [('Sibiu', 80), ('Craiova', 146), ('Pitesti', 97)],
'Sibiu': [('Arad', 140), ('Oradea', 151), ('Fagaras', 99), ('Rimnicu Vikea', 80)],
'Timisoara': [('Arad', 118), ('Lugoj', 111)],
'Urziceni': [('Bucharest', 85), ('Hirsowa', 98), ('Vaslui', 142)],
'Vaslui': [('Urziceni', 142), ('Iasi', 92)],
'Zerind': [('Arad', 75), ('Oradea', 71)]
}
def neighbors(n):
for n1, d in nodes[n]:
yield n1
def distance(n1, n2):
for n, d in nodes[n1]:
if n == n2:
return d
def cost(n, goal): #always return 1 if have no heuristic
dist_to_buch={
'Arad': 366,
'Bucharest': 0,
'Craiova': 160,
'Dobreta': 242,
'Eforie': 161,
'Fagaras': 176,
'Giurgiu': 77,
'Hirsowa': 151,
'Iasi': 226,
'Lugoj': 244,
'Mehadia': 241,
'Neamt': 234,
'Oradea': 380,
'Pitesti': 10,
'Rimnicu Vikea': 193,
'Sibiu': 253,
'Timisoara': 329,
'Urziceni': 80,
'Vaslui': 199,
'Zerind': 374
}
return dist_to_buch[n]
path = list(astar.find_path('Lugoj', 'Bucharest', neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost, distance_between_fnct=distance))
for x in range(len(path)-1):
print (path[x]+' -> ', end="")
print (path[len(path)-1])
def get_astar_path(world, start, goal):
return_path, pathcost = astar.find_path(world.get_nbor_cells, start, goal,
world.passable)
return return_path, pathcost
def calc_kabel_len(kabel):
assert kabel.objects_associated
if not isinstance(kabel, Kabel):
raise RuntimeError("Kabel is not of Type Kabel")
def neighbors(n):
for n1 in n.get_connected_nodes():
yield n1
def distance(n1, n2):
return n1.distance(n2)
def cost(n, goal):
return abs(n.x - goal.x)
print(kabel)
print(kabel.start_obj)
print(kabel.end_objs[0])
kabel.length = 10000000.0
connected_objects = []
path = None
te = False
for obj in kabel.end_objs:
e1 = kabel.start_obj.associated_node
e2 = obj.associated_node
# TODO: all objects must be connected
if e2 == None:
kabel.length = 0.0
# if te:
# raise RuntimeError("KNX Object: {} {}".format(obj, kabel))
return
print(e1.info())
print(e2.info())
temp_path = list(
astar.find_path(
e1,
e2,
neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost,
distance_between_fnct=distance,
))
le = calc_path_length(temp_path) * 0.01
if le < kabel.length:
kabel.length = le
connected_objects = [obj]
path = temp_path
if type(connected_objects[0]) == Knx:
te = True
# raise RuntimeError("KNX Object: {} {}".format(obj, kabel))
print(" Length after First: {}".format(kabel.length))
unconnected_objects = list(set(kabel.end_objs) - set(connected_objects))
for u in unconnected_objects:
print("Unconnected Objects: {}".format(u))
while len(unconnected_objects) > 0:
min_dist = 1000000.0
path2 = None
obj_to_add = None
for obj1 in connected_objects:
for obj2 in unconnected_objects:
e1 = obj1.associated_node
e2 = obj2.associated_node
temp_path = list(
astar.find_path(
e1,
e2,
neighbors_fnct=neighbors,
heuristic_cost_estimate_fnct=cost,
distance_between_fnct=distance,
))
le = calc_path_length(temp_path) * 0.01
if le < min_dist:
min_dist = le
obj_to_add = obj2
path2 = temp_path
connected_objects.append(obj_to_add)
unconnected_objects.remove(obj_to_add)
kabel.length += min_dist
path += path2
print(" Length next: {}".format(kabel.length))
for e in path2:
print(e)
for u in unconnected_objects:
print("Unconnected Objects: {}".format(u))
for n in path:
kabel.addNode(n)
n.addKabel(kabel)
for n in range(len(path) - 1):
if path[n].is_connected(path[n + 1]):
edge = path[n].get_edge_that_connects_to(path[n + 1])
edge.addKabel(kabel)
return path
time.sleep(0.25)
return goal
if __name__ == "__main__":
with open(sys.argv[1], 'r') as fh:
program = load(fh)
(droid_input, controller_output) = os.pipe()
(controller_input, droid_output) = os.pipe()
droid = Thread(target=run, args=(program, droid_input, droid_output))
droid.start()
start = (0, 0)
map = dict()
goal = explore(controller_input, controller_output, map, start,
all_directions)
print >> sys.stderr, "found goal:", goal
path = list(
astar.find_path(start, goal, lambda n: map[n], lambda a, b: 1,
lambda a, b: 1))
draw_map(map, start, goal, path)
print "steps:", len(path) - 1
os.close(droid_input)
os.close(controller_output)
droid.join()
vis = Visualize(a)
a.add_agents([(1, 1, 2, 2)]) #, (1,0,2,3)
a.add_rocks([(2, 1), (1, 2), (1, 3), (1, 4), (3, 1), (4, 1), (2, 3), (3, 3),
(3, 4)])
vis.draw_world()
vis.draw_agents()
vis.canvas.pack()
vis.canvas.update()
vis.canvas.after(1000)
path = astar.find_path(a.get_nbor_cells, a.aindx_cpos[1], a.aindx_goal[1],
lambda cell: 1,
lambda cell: not a.is_blocked(cell[0], cell[1]))
print path
actions = a.path_to_action(1, path[1:])
print actions
for action in actions:
a.agent_action(1, action)
vis.canvas.update()
vis.canvas.after(1000)
print a.cells
vis.canvas.after(3000)
