1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
#!/usr/bin/env python
#
# AStar.py
#
# Run the A* planner on a simple grid. It plans backwards from the
# goal to the start, in case the start is moving.
#
# THIS IS A SKELETON. PLESAE FIX THE COSTS, ETC!!
#
import numpy as np
#
# Constants
#
SQRT2 = np.sqrt(2.0)
#
# A* Planning Algorithm, on a Simple Grid
#
# map should be a numpy array of booleans (True if occupied)
#
# Returns a list of (u,v) tuples, leading from start to goal
#
def AStar(map, us, vs, ug, vg):
print("A* planning from (%d,%d) to (%d,%d)" % (us, vs, ug, vg))
# Transpose the map, so everything can be index by the tuple (u,v)!!!
wall = map.astype(np.bool).T
(w,h) = np.shape(wall)
start = (us, vs)
goal = (ug, vg)
# Prepare the "nodes" - start the total cost as infinite (higher
# than anything to come) and the variables for each state.
seen = np.full((w,h), False)
done = np.full((w,h), False)
costTotal = np.full((w,h), np.inf)
costToGoal = np.full((w,h), 0.0)
successor = np.full((w,h,2), 0, dtype=np.int)
# And clear the (unsorted!) on-deck queue.
onDeck = []
# Function to check a point (relative to a successor point).
def CheckPoint(pt, suc, cGoal):
# Skip if the point is invalid (at edges) or already done.
if ((pt[0]<0) or (pt[0]>=w) or (pt[1]<0) or (pt[1]>=h) or done[pt]):
return
# Check whether the point is blocked (occupied).
# IS THIS WHAT YOU WANT?
if wall[pt]:
return
# Add to the on-deck queue (if not yet seen)
if not seen[pt]:
onDeck.append(pt)
seen[pt] = True
# Estimate the cost from the start to here.
# IS THIS WHAT YOU WANT?
cFromStartEst = np.sqrt((pt[0]-start[0])**2 + (pt[1]-start[1])**2)
# Compute/udpate the costs.
cTotal = cGoal + cFromStartEst
if cTotal < costTotal[pt]:
costToGoal[pt] = cGoal
costTotal[pt] = cTotal
successor[pt] = suc
# Begin placing the start state on-deck, the continually expand...
print("A* building the search tree...")
CheckPoint(tuple(goal), tuple(goal), 0.0)
while True:
# Make sure we still have something to look at!
if len(onDeck) == 0:
return []
# Pop the onDeck point with the lowest (estimated) total cost.
point = onDeck.pop(np.argmin(costTotal[tuple(zip(*onDeck))]))
done[point] = True
# Check whether we have found the start.
if done[start]:
break
# Beyond the pure distance cost, also consider a cost for being
# at this point. How "good" is the point?
# IS THIS WHAT YOU WANT?
cPoint = 0.0
cGoal = costToGoal[point]
# Check the surrounding points. Set the travel cost by the distance.
(u,v) = point
CheckPoint((u+1,v ), (u,v), 1.0 + cPoint + cGoal)
CheckPoint((u+1,v+1), (u,v), SQRT2 + cPoint + cGoal)
CheckPoint((u ,v+1), (u,v), 1.0 + cPoint + cGoal)
CheckPoint((u-1,v+1), (u,v), SQRT2 + cPoint + cGoal)
CheckPoint((u-1,v ), (u,v), 1.0 + cPoint + cGoal)
CheckPoint((u-1,v-1), (u,v), SQRT2 + cPoint + cGoal)
CheckPoint((u ,v-1), (u,v), 1.0 + cPoint + cGoal)
CheckPoint((u+1,v-1), (u,v), SQRT2 + cPoint + cGoal)
# Build and return the path.
path = [start]
while tuple(successor[path[-1]]) != path[-1]:
path.append(tuple(successor[path[-1]]))
print("A* path has %d points" % (len(path)))
return path