Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 155 lines (133 sloc) 5.08 kb
f4ab236 @bickfordb add nav
bickfordb authored
1 import math
2 import time
3e34dc2 move heapq import up
Evan Klitzke authored
3 import heapq
f4ab236 @bickfordb add nav
bickfordb authored
4
2d84fcf @bickfordb progress
bickfordb authored
5 import mars_math
6
f4ab236 @bickfordb add nav
bickfordb authored
7 def A_star(start, goal, successors, edge_cost, heuristic_cost_to_goal=lambda position, goal:0):
8 """Very general a-star search. Start and goal are objects to be compared
9 with the 'is' operator, successors is a function that, given a node, returns
10 other nodes reachable therefrom, edge_cost is a function that returns the
11 cost to travel between two nodes, and heuristic_cost_to_goal is an
12 admissible heuristic function that gives an underestimate of the cost from a
13 position to the goal."""
14 closed = set()
15 open = [(0, 0, (start,))]
16 while open:
17 heuristic_cost, cost_so_far, path = heapq.heappop(open)
18 tail = path[-1]
19 if tail in closed:
20 continue
21 if tail == goal:
22 return path
23 closed.add(tail)
24 for new_tail in successors(tail):
25 new_cost_so_far = cost_so_far + edge_cost(tail, new_tail)
26 new_heuristic_cost = new_cost_so_far + heuristic_cost_to_goal(new_tail, goal)
27 heapq.heappush(open, (new_cost_so_far, new_heuristic_cost, path+(new_tail,)))
28 raise RuntimeError('No path found.')
29
30 class MapGrid(object):
31 """a map grid centered on the origin with width w and height h and resolution
1099f87 @bickfordb add obstacles
bickfordb authored
32 map:
33 ...........w,h
34 . .
35 . 0,0 .
36 . .
37 -w,-h.......
38
39
40
41 grid:
f4ab236 @bickfordb add nav
bickfordb authored
42 ............ (resolution w, resolution h)
43 . .
44 . .
45 . .
46 0...........
47
48 """
49 def __init__(self, width, height, resolution):
50 self.width = float(width)
51 self.height = float(height)
1099f87 @bickfordb add obstacles
bickfordb authored
52 resolution = int(resolution)
f4ab236 @bickfordb add nav
bickfordb authored
53 self.grid_width = resolution
54 self.grid_height = resolution
32ee3b7 @bickfordb integerize nodes
bickfordb authored
55 self.resolution = resolution
56 self.obstacles = set()
f4ab236 @bickfordb add nav
bickfordb authored
57
58 def node(self, x, y):
59 adj_x = x + (self.width / 2.0)
60 adj_y = y + (self.height / 2.0)
61 i = int(self.grid_width * adj_x / self.width)
62 j = int(self.grid_height * adj_y / self.height)
1099f87 @bickfordb add obstacles
bickfordb authored
63 return self._encode(i, j)
f4ab236 @bickfordb add nav
bickfordb authored
64
65 def cost(self, node1, node2):
1099f87 @bickfordb add obstacles
bickfordb authored
66 return self.distance(node1, node2)
67
f4ab236 @bickfordb add nav
bickfordb authored
68 def distance(self, node1, node2):
1099f87 @bickfordb add obstacles
bickfordb authored
69 i1, j1 = self._decode(node1)
70 i2, j2 = self._decode(node2)
71 return math.hypot(float(i1 - i2), float(j1 - j2))
72
73 def _decode(self, node):
74 i, j = node
75 return i, j
76
77 def _encode(self, i, j):
78 assert 0 <= i < self.resolution, i
79 assert 0 <= j < self.resolution, j
80 return (i, j)
f4ab236 @bickfordb add nav
bickfordb authored
81
82 def coord(self, node):
1099f87 @bickfordb add obstacles
bickfordb authored
83 i, j = self._decode(node)
84 assert i >= 0, i
85 assert j >= 0, j
f4ab236 @bickfordb add nav
bickfordb authored
86 x = self.width * i / float(self.grid_width)
87 x -= self.width / 2.0
88 y = self.height * j / float(self.grid_height)
89 y -= self.height / 2.0
2d84fcf @bickfordb progress
bickfordb authored
90 return mars_math.Point(x, y)
f4ab236 @bickfordb add nav
bickfordb authored
91
1099f87 @bickfordb add obstacles
bickfordb authored
92 def add_obstacle(self, point, radius):
93 """Add an obstacle to the grid
94 Args:
95 """
96 # find the units this will take up on the grid
2d84fcf @bickfordb progress
bickfordb authored
97 gwidth = int(round((self.resolution / self.width) * radius, 0))
98 gheight = int(round((self.resolution / self.height) * radius, 0))
1099f87 @bickfordb add obstacles
bickfordb authored
99
100 node = self.node(*point)
101 center_i, center_j = self._decode(node)
102 start_i = max(0, center_i - gwidth)
103 end_i = min(self.grid_width, center_i + gwidth)
104 start_j = max(0, center_j - gheight)
105 end_j = min(self.grid_height, center_j + gheight)
106
107 print "adding obstacle from:", self.coord(self._encode(start_i, start_j)), "to", self.coord(self._encode(end_i, end_j))
108 for i in range(start_i, end_i + 1):
109 for j in range(start_j, end_j + 1):
110 self.obstacles.add(self._encode(i, j))
ccc0261 @bickfordb add ne, sw, se, nw directions to nav
bickfordb authored
111
f4ab236 @bickfordb add nav
bickfordb authored
112 def path(self, start, goal):
1099f87 @bickfordb add obstacles
bickfordb authored
113 """Find a path from start to goal"""
2d84fcf @bickfordb progress
bickfordb authored
114 start_ = self.node(start.x, start.y)
115 goal_ = self.node(goal.x, goal.y)
dedb8eb @bickfordb start path strategy
bickfordb authored
116 result = A_star(start_, goal_, self.adjacent, self.cost, self.distance)
117 return map(self.coord, result)
f4ab236 @bickfordb add nav
bickfordb authored
118
119 def adjacent(self, node):
32ee3b7 @bickfordb integerize nodes
bickfordb authored
120 for i in self._adjacent(node):
121 if i not in self.obstacles:
122 yield i
1099f87 @bickfordb add obstacles
bickfordb authored
123
32ee3b7 @bickfordb integerize nodes
bickfordb authored
124 def _adjacent(self, node):
1099f87 @bickfordb add obstacles
bickfordb authored
125 i, j = self._decode(node)
32ee3b7 @bickfordb integerize nodes
bickfordb authored
126 res = self.resolution
127 w = self.grid_width
128 h = self.grid_height
1099f87 @bickfordb add obstacles
bickfordb authored
129 if i + 1 < w: # RIGHT
130 yield self._encode(i + 1, j)
131 if j + 1 < h: # UP
132 yield self._encode(i, j + 1)
133 if i + 1 < w and j + 1 < h: # UP RIGHT
134 yield self._encode(i + 1, j + 1)
135 if i + 1 < w and j > 0: # DOWN RIGHT
136 yield self._encode(i + 1, j - 1)
ccc0261 @bickfordb add ne, sw, se, nw directions to nav
bickfordb authored
137 if i > 0 and j > 0: # DOWN LEFT
1099f87 @bickfordb add obstacles
bickfordb authored
138 yield self._encode(i - 1, j - 1)
ccc0261 @bickfordb add ne, sw, se, nw directions to nav
bickfordb authored
139 if i > 0: # LEFT
1099f87 @bickfordb add obstacles
bickfordb authored
140 yield self._encode(i - 1, j)
ccc0261 @bickfordb add ne, sw, se, nw directions to nav
bickfordb authored
141 if j > 0: # DOWN
1099f87 @bickfordb add obstacles
bickfordb authored
142 yield self._encode(i, j - 1)
143 if i > 0 and j + 1 < h: # UP LEFT
144 yield self._encode(i - 1, j + 1)
f4ab236 @bickfordb add nav
bickfordb authored
145
146 if __name__ == '__main__':
dedb8eb @bickfordb start path strategy
bickfordb authored
147 m = MapGrid(10, 10, 100)
f4ab236 @bickfordb add nav
bickfordb authored
148 ts = time.time()
1099f87 @bickfordb add obstacles
bickfordb authored
149 m.add_obstacle((1.0, 1.0), 1.0)
150 path = m.path((0, 0), (4.5, 4.5))
f4ab236 @bickfordb add nav
bickfordb authored
151 te = time.time()
ccc0261 @bickfordb add ne, sw, se, nw directions to nav
bickfordb authored
152 print path
f4ab236 @bickfordb add nav
bickfordb authored
153 print te-ts
1099f87 @bickfordb add obstacles
bickfordb authored
154
Something went wrong with that request. Please try again.