def astar():
h, w = 10, 10
threshold = 0.5
start_point = [5, 3, 0]
goal_state = [9, 9]
w1, w2 = 5, 10
nodes = queue.PriorityQueue()
init_node = Node(start_point, None, None, 0, None)
nodes.put((init_node.getCost(), init_node))
traversed_nodes = []
obs = Obstacle(0.0)
viz = Visualization(obs)
fig, ax = plt.subplots(figsize=(10, 10))
ax.set(xlim=(0, 10), ylim=(0, 10))
ax = viz.addObstacles2Map(ax)
ax.set_aspect("equal")
goal_reached = False
node_array = np.array([[[math.inf for k in range(360)]
for j in range(int(h / threshold))]
for i in range(int(w / threshold))])
full_path = None
goal_reached = False
print('Finding path.........')
while (not nodes.empty()):
current_node = nodes.get()[1]
traversed_nodes.append(current_node)
if checkGoalReached(current_node, goal_state, 1):
print('Goal reached')
print("The cost of path: ", current_node.getCost())
moves, node_path = current_node.getFullPath()
visualize(viz, traversed_nodes, node_path)
goal_reached = True
fp = open('path_points.csv', 'w')
fn = open('path_nodes.csv', 'w')
fv = open('vel_points.csv', 'w')
writer_p = csv.writer(fp)
writer_n = csv.writer(fn)
writer_v = csv.writer(fv)
for move in moves:
writer_v.writerow(move)
for node in node_path:
xi, yi, _ = node.getState()
writer_n.writerow([xi, yi])
points = node.getPathArray()
if points is not None:
for point in points:
xn, yn = point
row = [xn, yn]
writer_p.writerow(row)
xi, yi = xn, yn
fp.close()
fv.close()
fn.close()
else:
branches = getBranches(current_node, 1, w1, w2, obs)
for branch_node in branches:
branch_state = branch_node.getState()
if checkVisited(branch_node,
node_array,
goal_state,
threshold=0.5):
node_array[int(halfRound(branch_state[0]) / threshold),
int(halfRound(branch_state[1]) / threshold),
branch_state[2]] = branch_node.getCost(
) + computeHeuristicCost(
branch_state, goal_state)
nodes.put((branch_node.getCost() +
computeHeuristicCost(branch_state, goal_state),
branch_node))
if (goal_reached): break