def solve_labyrinth(lab):
if type(lab) != labyrinth.Labyrinth:
raise TypeError("argument must have type {}, got {}".format(labyrinth.Labyrinth, type(lab)))
#prepare path finder
a = astar.Astar(lab) ; a.chars_unwalkable = '#' ; a.chars_walkable += '+'
st = lab.player_coords
end = lab.exit_coords
#if no keys -- simply find shortest path
if not lab.conf.keys():
return lab.simple_solve(st, end)
keys = lab.keys.copy() ; keys.insert(0, end)
doors = lab.doors.copy()
#find first reachable key
paths = []
for k in keys:
s = a.find_path(st, k, doors)
if s != None:
paths.append(Path(st, k, s, doors.copy(), keys.copy()))
paths[-1].rest_keys.remove(k)
final_paths = []
#check paths with each reachable keys and doors
while sum([len(p.rest_keys) for p in paths]) > 0 :
#check shortest paths first
paths.sort(key=Path.sort_by_pathlen_key)
old_path_count = len(paths)
for p in paths:
#if reached end cell
if not end in p.rest_keys:
final_paths.append(p)
continue
#else test each reachable door
for d in p.rest_doors:
cur_doors = p.rest_doors.copy() ; cur_doors.remove(d)
#and try to get next key
for k in p.rest_keys:
a = astar.Astar(lab) ; a.chars_unwalkable = '#' ; a.chars_walkable += '+'
s = a.find_path(p.end, k, cur_doors)
#if next key found -- add this path for further check
if s != None:
paths.append(Path(p.start, k, p.path+s, cur_doors.copy(), p.rest_keys.copy()))
paths[-1].rest_keys.remove(k)
#remove all paths that was continued/ended at last iteration
paths = paths[old_path_count:]
#sort by solution length
final_paths.sort(key=Path.sort_by_pathlen_key)
#get shortest
return final_paths[0].path
def solve_labyrinth(lab):
#lab = labyrinth.Labyrinth() #TODO: TEMP!!!
if type(lab) != labyrinth.Labyrinth:
raise TypeError("argument must have type {}, got {}".format(labyrinth.Labyrinth, type(lab)))
a = astar.Astar(lab) ; a.chars_unwalkable = '#' ; a.chars_walkable += '+'
st = lab.player_coords
end = lab.exit_coords
if not lab.conf.keys():
return lab.simple_solve(st, end)
keys = lab.keys.copy() ; keys.insert(0, end)
doors = lab.doors.copy()
paths = []
for k in keys:
s = a.find_path(st, k, doors)
if s != None:
paths.append(Path(st, k, s, doors.copy(), keys.copy()))
paths[-1].rest_keys.remove(k)
#print(paths[-1])
#lab.draw_solution(paths[-1].path)
#print('\n\n')
#print(len(paths), paths[0].path, paths[0].rest_keys, paths[0].rest_doors)
final_paths = []
while sum([len(p.rest_keys) for p in paths]) > 0 :
paths.sort(key=Path.sort_by_pathlen_key)
old_path_count = len(paths)
for p in paths:
if not end in p.rest_keys:
final_paths.append(p)
continue
for d in p.rest_doors:
cur_doors = p.rest_doors.copy() ; cur_doors.remove(d)
for k in p.rest_keys:
#print('key:',k, 'door:',d, p.rest_keys, cur_doors)
a = astar.Astar(lab) ; a.chars_unwalkable = '#' ; a.chars_walkable += '+'
s = a.find_path(p.end, k, cur_doors)
if s != None:
paths.append(Path(p.start, k, p.path+s, cur_doors.copy(), p.rest_keys.copy()))
paths[-1].rest_keys.remove(k)
#print(paths[-1])
#lab.draw_solution(paths[-1].path)
#lab.draw_solution(p.path)
#print('\n\n')
paths = paths[old_path_count:]
final_paths.sort(key=Path.sort_by_pathlen_key)
return final_paths[0].path
def do(self, wrld):
meX = wrld.me(self).x
meY = wrld.me(self).y
# Your code here
exit = [0, 0]
#get current position
meX = wrld.me(self).x
meY = wrld.me(self).y
#check every gridcell for the exit. Uses the last exit found as the "goal"
for i in range(wrld.width()):
for j in range(wrld.height()):
if wrld.exit_at(i, j):
exit = [i, j]
astar_obj = astar.Astar([meX, meY], exit, wrld)
self.path = []
self.path = astar_obj.find_path(wrld)
self.first = False
#get the [x,y] coords of the next cell to go to
goTo = self.path.pop(0)
print("GOTO: " , goTo)
#move in direction to get to x,y found in prev step
self.move(-meX + goTo[0], -meY + goTo[1])
def __init__(self, game_control, xml_file, x, y, angle=0):
'''
@brief Constructor.
@brief game_control Referencia a GameControl
@brief xml_file Archivo xml con las características
@brief x Posición x
@brief y Posición y
@brief angle Ángulo inicial del vehículo
'''
basiccar.BasicCar.__init__(self, game_control, xml_file, x, y, angle)
#Simulación se Switch de C o C++.
#Según el estado llamaremos a una función u otra.
self.states = {
gameobject.NORMAL: self.__normal_state,
gameobject.TURBO: self.__turbo_state,
gameobject.NOACTION: self.__normal_state,
gameobject.RUN: self.__run_state,
gameobject.DAMAGED: self.__damaged_state
}
self.falling = False
self.min_scale = 0.3
self.count_scale = 0.02
self.actual_scale = 1
self.target_angle = 0
#Donde almacenaremos los objetivos a los que queremos llegar
self.left_targets = self.passed_targets = deque()
self.actual_target = None
#Donde almacenaremos los puntos intermedios para llegar a los objetivos
self.passed_points = self.left_points = deque()
self.actual_point = None
#Instancia del algoritmo A* del vehiculo
self.astar = astar.Astar()
#Hacemos trampas.
self.rotation_angle = 0.75
self.hud = basiccar.Hud(self, 'hud.xml', True)
self.turbo_time = None
#pts = numpy.array([[300,300],[300,340],[350,320]],numpy.int32) #用numpy形成坐标列表
#cv2.polylines(img,[pts],True,(0,255,255),2) #画多边形
#窗口等待任意键盘按键输入,0为一直等待,其他数字为毫秒数
cv2.waitKey(0)
while True:
if cv2.waitKey(0) == 13:
print("running...")
#执行路径查找
if p_from[0] == 0 or p_from[1] == 0 or p_to[0] == 0 or p_to[1] == 0:
print("start and end not setting.")
continue
#map_res = astar_0.astar(g_map, p_from, p_to)
astar = astar.Astar(g_map)
map_res = astar.run(p_from, p_to)
print(map_res)
if map_res is None or len(map_res) == 0:
continue
#map_res.reverse()
last_p = map_res[0]
for row, col in map_res[1:]:
#print(row,col)
pre_ro = last_p[0] * H_0
pre_co = last_p[1] * W_0
#img_add_new[pre_ro:pre_ro+H_0, pre_co:pre_co+W_0] = img_add_new_copy[pre_ro:pre_ro+H_0, pre_co:pre_co+W_0]
img_add_new[row * H_0:row * H_0 + H_0,
col * W_0:col * W_0 + W_0] = img_man
cv2.imshow("AStar", img_add_new)
last_p = (row, col)
def astar_find_dist(file, node_a, node_b):
return astar.Astar(file, 50, 50, node_a, node_b)
robot = env.GetRobots()[0]
# tuck in the PR2's arms for driving
tuckarms(env,robot);
with env:
# the active DOF are translation in X and Y and rotation about the Z axis of the base of the robot.
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
goalconfig = [2.6,-1.3,-pi/2]
start = time.clock()
#### YOUR CODE HERE ####
#### Implement your algorithm to compute a path for the robot's base starting from the current configuration of the robot and ending at goalconfig. The robot's base DOF have already been set as active. It may be easier to implement this as a function in a separate file and call it here.
planning = astar.Astar(goalconfig)
# planning.fourConnectedAstar('Manhattan', env, robot)
# planning.fourConnectedAstar('Euclidean', env, robot)
# planning.eightConnectedAstar('Manhattan', env, robot)
planning.eightConnectedAstar('Euclidean', env, robot)
# print 'final path:', planning.path
#### Draw the X and Y components of the configurations explored by your algorithm
handles = []
for config in planning.collision_config:
handles.append(env.plot3(points=array((config[0], config[1], 0.3)),
pointsize=4.0,
colors=array(((1, 0, 0)))))
for config in planning.collision_free_config:
handles.append(env.plot3(points=array((config[0], config[1], 0.3)),
pointsize=4.0,
(10, 13, 6), (10, 16, 15), (11, 6, 10), (11, 10, 7), (11, 13, 8),
(11, 12, 6), (12, 11, 6), (13, 10, 6), (13, 11, 8), (13, 27, 9),
(14, 9, 4), (14, 15, 5), (14, 17, 4), (15, 14, 5), (15, 17, 5),
(16, 9, 8), (16, 10, 15), (16, 22, 17), (16, 28, 8), (17, 14, 4),
(17, 15, 5), (17, 20, 9), (18, 19, 7), (18, 27, 10), (18, 28, 8),
(19, 18, 7), (19, 23, 11), (20, 17, 9), (20, 21, 5), (20, 26, 8),
(21, 20, 5), (21, 22, 7), (22, 16, 17), (22, 21, 7), (22, 23, 11),
(22, 25, 9), (22, 28, 12), (23, 19, 11), (23, 22, 11), (23, 24, 9),
(24, 23, 9), (24, 25, 9), (25, 22, 9), (25, 24, 9), (25, 26, 5),
(26, 20, 8), (26, 25, 5), (27, 13, 9), (27, 18, 10), (28, 16, 8),
(28, 18, 8), (28, 22, 12)]
mychart = city.chart(citylist, edgelist)
print "1 to 7: ",
testpath = astar.Astar(1, 7, mychart)
print testpath
print "17 to 1: ",
testpath = astar.Astar(17, 1, mychart)
print testpath
print "1 to 24: ",
testpath = astar.Astar(1, 24, mychart)
print testpath
print "1 to 26: ",
testpath = astar.Astar(1, 26, mychart)
print testpath
print "1 to 17: ",
def simple_solve(self, start=None, end=None, banned_list=[]):
start = self.player_coords if start == None else start
end = self.exit_coords if end == None else end
a = astar.Astar(self)
return a.find_path(start, end, banned_list)