def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
self.path = [
Cell(r - self.path_finder.start.row,
c - self.path_finder.start.column) for r, c in raw_path
] # move rows to correct starting position
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
# Setup subscribers
odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def __init__(self, precision):
rospy.init_node("global_planner", anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param("~position")
startX, startY = positions["startX"], positions["startY"]
targetX, targetY = positions["targetX"], positions["targetY"]
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
# do not crop if target outside of maze
self.map_loader = MapLoader(start, target)
self.map_matrix = self.map_loader.loadMap()
GlobalPlanner.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
self.precision = precision
# Calculate path
t = time.time()
self.path_finder = AStar(self.map_matrix)
raw_path = self.path_finder.search()
print(time.time() - t)
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10)
# Setup subscribers
_ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
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
from copy import deepcopy
from maps import *
from map_loader import MapLoader
from cost_optimization import *
map_loader = MapLoader()
class MapMerge:
def __init__(self):
self.water_shed_map = None
self.water_shed_matrix = None
def get_map_name_by(self, id, basic_name):
return "reg_" + str(float(id)) + "_" + str(basic_name)
def build_watershed_id_to_pixels(self):
water_shed_id_to_pixels = {}
for i in range(len(self.water_shed_matrix)):
row = self.water_shed_matrix[i]
for j in range(len(row)):
cell = row[j]
if cell == self.water_shed_map.no_data_value:
continue
cell_obj = water_shed_id_to_pixels.get(cell, [])
cell_obj.append({"x": i, "y": j})
water_shed_id_to_pixels[cell] = cell_obj
return water_shed_id_to_pixels
def merge_outputs_to_one_by_watershed_map(self, water_shed_map_name, maps,
def h_euclid025(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.25*np.sqrt(dx*dx + dy*dy)
return int(dist)
def h_euclid05(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.5*np.sqrt(dx*dx + dy*dy)
return int(dist)
# 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 = "config_space_bw" # This is the base file name of the input image for map generation
map_loader.addFrame(".",base_image+".png")
scale = 0.25 # scale of map - smaller scale implies larger grid size
map_loader.createMap(scale, (-np.pi, np.pi), (-np.pi, np.pi)) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST)
cv2.imshow("Image",map_loader.image)
cv2.imshow("Map", map_loader.map)
cv2.imshow("Big", big_map)
