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))