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)