#result = breadth_first_search(problem2)
#result = uniform_cost_search(problem2)
#@result = astar_search(problem2, h=problem2.h_euclid)#manhattan)#y_distance)
ftxt = open(file_name+'.txt','w')
print " Result=",result
print " expansions = ",problem2.expansion
ftxt.write("expansions = "+str(problem2.expansion)+"\n")
ftxt.write("max frontier = "+str(max_frontier_size)+"\n")
if (result is not None):
path = result.path()
ftxt.write("path cost="+str(problem2.total_path_cost(path))+"\n")
ftxt.write("Path="+str(path)+"\n")
print "path cost=",problem2.total_path_cost(path)
print "Path=",path
print "Plotting path ..."
map_loader.plotPath(path, 1.0)# scale)
big_path = cv2.resize(map_loader.path, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_LINEAR)
cv2.imshow("Path",big_path)
cv2.imwrite(file_name+"_path.png",big_path)
else:
ftxt.write('no path!')
ftxt.close()
print " Close the video ..."
problem2.video_encoder.release()
cv2.waitKey(500)
print "Done! - press return to exit"
cv2.waitKey(5000)
def fullPath():
# This is the main part of the demo program
map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image
base_image = "simple" # This is the base file name of the input image for map generation
map_loader.addFrame("/home/kinova/MillenCapstone/MadalynMillenCapstone/animation/","config_space_bw.png")
scale = 0.1
map_loader.createMap(scale) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_NEAREST)
imshow("Image",map_loader.image)
imshow("Map", map_loader.map)
imshow("Big", big_map)
target_dir = "output"
if not os.path.exists(target_dir):
print "Creating target directory <",target_dir,"> ..."
try: os.makedirs(target_dir)
except:
print "Failed to create target path!"
exit()
print "Writing the base images ..."
imwrite(target_dir+"/"+base_image+"_img.png",map_loader.image)
imwrite(target_dir+"/"+base_image+"_map.png",map_loader.map)
imwrite(target_dir+"/"+base_image+"_big_map.png",big_map)
print "Wait for key input..."
#waitKey()
print "Doing the search ..."
grid = UndirectedGraph() # Using Russell and Norvig code
start=(13,20)
goal=(16,30)
# Define the test cases we want to run
tests = [("depth_first_", depth_first_graph_search),
("breadth_first_",breadth_first_search),
("uniform_cost_", uniform_cost_search)]
'''("astar_search_euclid_", astar_search,0),
("astar_search_euclid2_", astar_search,4),
("astar_search_euclid3_", astar_search,5),
("astar_search_euclid025_", astar_search,6),
("astar_search_euclid05_", astar_search,7),
("astar_search_dx_", astar_search,1),
("astar_search_dy_", astar_search,2),
("astar_search_manhattan_", astar_search,3),
("greedy_search_euclid_", greedy_best_first_graph_search,0),
("greedy_search_dx_", greedy_best_first_graph_search,1),
("greedy_search_dy_", greedy_best_first_graph_search,2),
("greedy_search_manhattan_",greedy_best_first_graph_search,3) ]'''
paths = []
smallPath = []
smallestPath = float('inf')
radPoint = 0.123
radAdd = np.pi / 8
for test in tests:
print "Set up the "+test[0]+" ..."
file_name = target_dir+"/"+test[0]+base_image
video_encoder = VideoEncoder(file_name, map_loader.map, frame_rate = 30.0, fps_factor=1.0, comp_height=1.0/scale, comp_width=2.0/scale)
print " output to ",file_name
problem2 = GridProblem(start, goal, grid, map_loader.map,scale,video_encoder)
# Load the correct grid search algorithm and heuristics
print "------------- call ---------------------"
if (len(test) > 2):
if (test[2] == 0):
result, max_frontier_size = test[1](problem2, problem2.h_euclid)
#
elif (test[2] == 1):
result, max_frontier_size = test[1](problem2, problem2.h_x_distance)
#
elif (test[2] == 2):
result, max_frontier_size = test[1](problem2, problem2.h_y_distance)
#
elif (test[2] == 3):
result, max_frontier_size = test[1](problem2, problem2.h_manhattan)
#
elif (test[2] == 4):
result, max_frontier_size = test[1](problem2, problem2.h_euclid2)
#
elif (test[2] == 5):
result, max_frontier_size = test[1](problem2, problem2.h_euclid3)
#
elif (test[2] == 6):
result, max_frontier_size = test[1](problem2, problem2.h_euclid025)
#
elif (test[2] == 7):
result, max_frontier_size = test[1](problem2, problem2.h_euclid05)
#
else:
print "Help",test[2]
else:
result, max_frontier_size = test[1](problem2)
#result,max_frontier_size=depth_first_graph_search(problem2)
print "-------------return---------------------"
#result = depth_first_graph_search(problem2)
#result = breadth_first_search(problem2)
#result = uniform_cost_search(problem2)
#@result = astar_search(problem2, h=problem2.h_euclid)#manhattan)#y_distance)
ftxt = open(file_name+'.txt','w')
print " Result=",result
print " expansions = ",problem2.expansion
ftxt.write("expansions = "+str(problem2.expansion)+"\n")
ftxt.write("max frontier = "+str(max_frontier_size)+"\n")
if (result is not None):
path = result.path()
ftxt.write("path cost="+str(problem2.total_path_cost(path))+"\n")
ftxt.write("Path="+str(path)+"\n")
print "path cost=",problem2.total_path_cost(path)
print "Path=",path
print "Plotting path ..."
map_loader.plotPath(path, 1.0)# scale)
big_path = resize(map_loader.path, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_LINEAR)
imshow("Path",big_path)
imwrite(file_name+"_path.png",big_path)
if len(path) < smallestPath:
smallPath = path
smallestPath = len(path)
else:
ftxt.write('no path!')
ftxt.close()
print " Close the video ..."
problem2.video_encoder.release()
waitKey(500)
for p in smallPath:
paths.append((radPoint, ((math.atan(float(p.state[1])/ float(p.state[0]))) * (np.pi / 180)) - radPoint))
print (float(p.state[1])/ float(p.state[0]))
if radPoint < 3.11:
radPoint = radPoint + radAdd
return paths
#result = breadth_first_search(problem2)
#result = uniform_cost_search(problem2)
#@result = astar_search(problem2, h=problem2.h_euclid)#manhattan)#y_distance)
ftxt = open(file_name+'.txt','w')
print " Result=",result
print " expansions = ",problem2.expansion
ftxt.write("expansions = "+str(problem2.expansion)+"\n")
ftxt.write("max frontier = "+str(max_frontier_size)+"\n")
if (result is not None):
path = result.path()
ftxt.write("path cost="+str(problem2.total_path_cost(path))+"\n")
ftxt.write("Path="+str(path)+"\n")
print "path cost=",problem2.total_path_cost(path)
print "Path=",path
print "Plotting path ..."
map_loader.plotPath(path, 1.0)# scale)
big_path = cv2.resize(map_loader.path, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_LINEAR)
cv2.imshow("Path",big_path)
cv2.imwrite(file_name+"_path.png",big_path)
else:
ftxt.write('no path!')
ftxt.close()
print " Close the video ..."
problem2.video_encoder.release()
cv2.waitKey(500)
