def main(): node = LocalGoalNode(node_name='global_planner') node.setup() world_dim = [ rospy.get_param('/world/x/min', -20), rospy.get_param('/world/x/max', 20), rospy.get_param('/world/y/min', -20), rospy.get_param('/world/y/max', 20), rospy.get_param('/world/z/min', 0), rospy.get_param('/world/z/max', 4), ] start_pos = [ float(rospy.get_param('/start/x', 0)), float(rospy.get_param('/start/y', 0)), float(rospy.get_param('/start/z', 1)) ] goal = [ float(rospy.get_param('/goal/x', 6)), float(rospy.get_param('/goal/y', -7)), float(rospy.get_param('/goal/z', 1)) ] # Global Path planning rospy.loginfo('Start global path planning...') try: path, processing_time = a_star(node, start_pos, goal, world_dim=world_dim, display=False) path[-1][0], path[-1][1], path[-1][2] = goal[0], goal[1], goal[2] except NoPathFound as e: # No path found rospy.logerr('No path found') rospy.logerr(e) rospy.set_param('/autopilot/done', 3) exit(1) except AssertionError as e: # The goal is not valid rospy.logerr('Configuration not valid') rospy.logerr(e) rospy.set_param('/autopilot/done', 4) exit(1) rospy.loginfo('Global path found !') d = 0 for i in range(len(path) - 1): d += np.linalg.norm(np.array(path[i]) - np.array(path[i + 1])) rospy.loginfo('Distance: {}'.format(d)) with open( '/home/rhidra/research_data/best_m{}_c{}_t{}.json'.format( node.mapId, node.configId, node.trialId), 'w') as f: f.write(str(d))
#compute 2-norm by row,The position of the minimum start_min_dist = np.linalg.norm(np.array(start)-np.array(skel_cells),axis=1).argmin() near_start = skel_cells[start_min_dist] goal_min_dist = np.linalg.norm(np.array(goal)-np.array(skel_cells),axis=1).argmin() near_goal = skel_cells[goal_min_dist] return near_start,near_goal skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne) print(start_ne,goal_ne) print(skel_start,skel_goal) def heuristic_func(position, goal_position): return np.sqrt((position[0] - goal_position[0])**2 + (position[1] - goal_position[1])**2) #A* on the skeleton path,cost = a_star(invert(skeleton).astype(np.int),heuristic_func,tuple(skel_start),tuple(skel_goal)) print("Path length = {0}, path cost = {1}".format(len(path),cost)) #compare to regular A* on the gird path2, cost2 = a_star(grid, heuristic_func, start_ne, goal_ne) print("Path length = {0}, path cost = {1}".format(len(path2), cost2)) plt.imshow(grid,origin='lower') plt.imshow(skeleton,cmap='Greys',origin='lower',alpha=0.7) plt.plot(start_ne[1],start_ne[0],'rx') plt.plot(goal_ne[1],goal_ne[0],'rx') pp = np.array(path) plt.plot(pp[:, 1], pp[:, 0], 'g') pp2 = np.array(path2) plt.plot(pp2[:, 1], pp2[:, 0], 'r')
plt.imshow(grid, cmap='Greys', origin='lower') plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7) plt.plot(start_ne[1], start_ne[0], 'rx') plt.plot(goal_ne[1], goal_ne[0], 'x') plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne) print(start_ne, goal_ne) print(skel_start, skel_goal) path, cost = a_star( invert(skeleton).astype(np.int), euclidean, tuple(skel_start), tuple(skel_goal)) print(cost) # adding start and goal to path path.insert(0, tuple(start_ne)) path.append(tuple(goal_ne)) # Compare to regular A* on the grid path2, cost2 = a_star(grid, euclidean, start_ne, goal_ne) print(cost2) plt.imshow(grid, cmap='Greys', origin='lower') plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7) # For the purposes of the visual the east coordinate lay along # the x-axis and the north coordinates long the y-axis.
def heuristic_func(position, goal_position): h = abs(position[0] - goal_position[0]) + abs(position[1] - goal_position[1]) # h = int(h) return h # Compute the lowest cost path with `a_star`. # In[25]: # TODO: use `a_star` to compute the lowest cost path path, cost = a_star(grid, heuristic_func, start_ne, goal_ne) print(path, cost) # Let's plot the path! # In[19]: plt.imshow(grid, cmap='Greys', origin='lower') # For the purposes of the visual the east coordinate lay along # the x-axis and the north coordinates long the y-axis. plt.plot(start_ne[1], start_ne[0], 'x') plt.plot(goal_ne[1], goal_ne[0], 'x') if path is not None: pp = np.array(path)
n = goal path_cost = branch[n][0] while branch[n][1] != start: path.append(branch[n][1]) n = branch[n][1] path.append(branch[n][1]) return path[::-1], path_cost start = list(g.nodes)[0] k = np.random.randint(len(g.nodes)) print(k, len(g.nodes)) goal = list(g.nodes)[k] path, cost = a_star(g, heuristic, start, goal) print(len(path), path) # In[50]: path_pairs = zip(path[:-1], path[1:]) for (n1, n2) in path_pairs: print(n1, n2) # ## Step 7 - Visualize Path # In[ ]: fig = plt.figure() plt.imshow(grid, cmap='Pastel1_r', origin='lower')
near_start = skel_cells[start_min_dist] goal_min_dist = np.linalg.norm(np.array(goal) - np.array(skel_cells), axis=1).argmin() near_goal = skel_cells[goal_min_dist] return near_start, near_goal skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne) print(start_ne, goal_ne) print(skel_start, skel_goal) def heuristic_func(position, goal_position): return np.sqrt((position[0] - goal_position[0])**2 + (position[1] - goal_position[1])**2) # Run A* on the skeleton path, cost = a_star(skeleton, heuristic_func, tuple(skel_start), tuple(skel_goal)) print("Path length = {0}, path cost = {1}".format(len(path), cost)) # Compare to regular A* on the grid path2, cost2 = a_star(grid, heuristic_func, start_ne, goal_ne) print("Path length = {0}, path cost = {1}".format(len(path2), cost2)) plt.imshow(grid, cmap='Greys', origin='lower') plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7) # For the purposes of the visual the east coordinate lay along # the x-axis and the north coordinates long the y-axis. plt.plot(start_ne[1], start_ne[0], 'x') plt.plot(goal_ne[1], goal_ne[0], 'x') pp = np.array(path) plt.plot(pp[:, 1], pp[:, 0], 'g')
grid = pickle.load(in_grid) in_grid.close() in_edges = open("edges_p.pickel", mode='rb') edges = pickle.load(in_edges) in_edges.close() print('Fount %5d edges' % len(edges)) plt.imshow(grid, origin='lower', cmap='Greys') #plt.show() for e in edges: p1 = e[0] p2 = e[1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() G = create_graph(edges) start = 34 goal = 300 start_closest = closest_point(G, start) goal_closest = closest_point(G, goal) path, cost = a_star(G, heuristic, start_closest, goal_closest) print(len(path))