Skip to content

Commit 945192f

Browse files
petercorkepetercorke
committed
handles dynamic replanning
1 parent c42d01c commit 945192f

File tree

1 file changed

+65
-85
lines changed

1 file changed

+65
-85
lines changed

‎roboticstoolbox/mobile/DstarPlanner.py

Lines changed: 65 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -70,27 +70,12 @@ def get_neighbors(self, state):
7070

7171
for i, j in self._neighbours:
7272
try:
73-
if state.x + i > 0 and state.y + j > 0:
73+
if state.x + i >= 0 and state.y + j >= 0:
7474
state_list.append(self.map[state.y + j][state.x + i])
7575
except IndexError:
7676
pass
7777
return state_list
7878

79-
80-
# def set_cost(self, region, cost, modify=None):
81-
82-
# xmin = max(0, region[0])
83-
# xmax = min(self.col, region[1])
84-
# ymin = max(0, region[2])
85-
# ymax = min(self.row, region[3])
86-
87-
# self.costmap[ymin:ymax, xmin:xmax] = cost
88-
89-
# if modify is not None:
90-
# for x in range(xmin, xmax):
91-
# for y in range(ymin, ymax):
92-
# modify.modify_cost(self.map[y][x])
93-
9479
_root2 = np.sqrt(2)
9580

9681
def cost(self, state1, state2):
@@ -127,11 +112,12 @@ def show_h(self):
127112
# - replace equality tests with math.isclose() which is faster than np.isclose()
128113
# - add an offset to inequality tests, X > Y becomes X > Y + tol
129114
class Dstar:
130-
def __init__(self, map):
115+
def __init__(self, map, tol=1e-6):
131116
self.map = map
132117
# self.open_list = set()
133118
self.open_list = []
134119
self.nexpand = 0
120+
self.tol = tol
135121

136122
def process_state(self, verbose=False):
137123
if verbose:
@@ -150,25 +136,27 @@ def process_state(self, verbose=False):
150136
if verbose:
151137
print(f"EXPAND {x}, {k_old:.1f}")
152138

153-
if x.h > k_old + 1e-6:
139+
if x.h > k_old + self.tol:
154140
# RAISE state
155141
if verbose:
156142
print(' raise')
157143
for y in self.map.get_neighbors(x):
158-
if y.t is not Tag.NEW and y.h <= k_old and x.h > y.h + self.map.cost(x, y) + 1e-6:
144+
if (y.t is not Tag.NEW and
145+
y.h <= k_old - self.tol and
146+
x.h > y.h + self.map.cost(x, y) + self.tol):
159147
if verbose:
160148
print(f" {x} cost from {x.h:.1f} to {y.h + self.map.cost(x, y):.1f}; parent from {x.parent} to {y}")
161149
x.parent = y
162150
x.h = y.h + self.map.cost(x, y)
163151

164-
if math.isclose(x.h, k_old, rel_tol=0, abs_tol=1e-6):
152+
if math.isclose(x.h, k_old, rel_tol=0, abs_tol=self.tol):
165153
# normal state
166154
if verbose:
167155
print(' normal')
168156
for y in self.map.get_neighbors(x):
169157
if y.t is Tag.NEW \
170-
or (y.parent == x and not math.isclose(y.h, x.h + self.map.cost(x, y), rel_tol=0, abs_tol=1e-6)) \
171-
or (y.parent != x and y.h > x.h + self.map.cost(x, y) + 1e-6):
158+
or (y.parent == x and not math.isclose(y.h, x.h + self.map.cost(x, y), rel_tol=0, abs_tol=self.tol)) \
159+
or (y.parent != x and y.h > x.h + self.map.cost(x, y) + self.tol):
172160
if verbose:
173161
print(f" reparent {y} from {y.parent} to {x}")
174162
y.parent = x
@@ -178,19 +166,19 @@ def process_state(self, verbose=False):
178166
if verbose:
179167
print(' raise/lower')
180168
for y in self.map.get_neighbors(x):
181-
if y.t is Tag.NEW or (y.parent == x and not math.isclose(y.h, x.h + self.map.cost(x, y), rel_tol=0, abs_tol=1e-6)):
169+
if y.t is Tag.NEW or (y.parent == x and not math.isclose(y.h, x.h + self.map.cost(x, y), rel_tol=0, abs_tol=self.tol)):
182170
if verbose:
183171
print(f" {y} cost from {y.h:.1f} to {y.h + self.map.cost(x, y):.1f}; parent from {y.parent} to {x}; add to frontier")
184172
y.parent = x
185173
self.insert(y, x.h + self.map.cost(x, y))
186174
else:
187-
if y.parent != x and y.h > x.h + self.map.cost(x, y) + 1e-6 and x.t is Tag.CLOSED:
175+
if y.parent != x and y.h > x.h + self.map.cost(x, y) + self.tol and x.t is Tag.CLOSED:
188176
self.insert(x, x.h)
189177
if verbose:
190178
print(f" {x}, {x.h:.1f} add to frontier")
191179
else:
192-
if y.parent != x and x.h > y.h + self.map.cost(y, x) + 1e-6 \
193-
and y.t is Tag.CLOSED and y.h > k_old + 1e-6:
180+
if y.parent != x and x.h > y.h + self.map.cost(y, x) + self.tol \
181+
and y.t is Tag.CLOSED and y.h > k_old + self.tol:
194182
self.insert(y, y.h)
195183
if verbose:
196184
print(f" {y}, {y.h:.1f} add to frontier")
@@ -212,28 +200,32 @@ def insert(self, state, h_new):
212200
state.k = h_new
213201

214202
elif state.t is Tag.OPEN:
215-
# k_new = min(state.k, h_new)
216-
# if state.k == k_new:
217-
# # k hasn't changed, leave vertex in frontier
218-
# return
219-
# else:
220-
# state.k = k_new
221-
# # remove the item from the open list
222-
# # print('node already in open list, remove it first')
223-
# #TODO use bisect on old state.k to find the entry
224-
# for i, item in enumerate(self.open_list):
225-
# if item[1] is state:
226-
# del self.open_list[i]
227-
# break
228-
229-
state.k = min(state.k, h_new)
230-
# remove the item from the open list
231-
# print('node already in open list, remove it first')
232-
#TODO use bisect on old state.k to find the entry
233-
for i, item in enumerate(self.open_list):
234-
if item[1] is state:
235-
del self.open_list[i]
236-
break
203+
k_new = min(state.k, h_new)
204+
if state.k == k_new:
205+
# k hasn't changed, and vertex already in frontier
206+
# just update h and be done
207+
state.h = h_new
208+
return
209+
else:
210+
# k has changed, we need to remove the vertex from the list
211+
# and re-insert it
212+
state.k = k_new
213+
214+
# scan the list to find vertex, then remove it
215+
# this is quite slow
216+
for i, item in enumerate(self.open_list):
217+
if item[1] is state:
218+
del self.open_list[i]
219+
break
220+
221+
# state.k = min(state.k, h_new)
222+
# # remove the item from the open list
223+
# # print('node already in open list, remove it first')
224+
# #TODO use bisect on old state.k to find the entry
225+
# for i, item in enumerate(self.open_list):
226+
# if item[1] is state:
227+
# del self.open_list[i]
228+
# break
237229

238230
elif state.t is Tag.CLOSED:
239231
state.k = min(state.h, h_new)
@@ -244,50 +236,33 @@ def insert(self, state, h_new):
244236
# self.open_list.add(state)
245237
heapq.heappush(self.open_list, (state.k, state))
246238

247-
# def remove(self, state):
248-
# if state.t is Tag.OPEN:
249-
# state.t = Tag.CLOSED
250-
# else:
251-
# state.t = Tag.CLOSED
252-
# print('removing non open state')
253-
# self.open_list.remove(state)
254-
255239
def modify_cost(self, x, newcost):
256240
self.map.costmap[x.y, x.x] = newcost
257241
if x.t is Tag.CLOSED:
258242
self.insert(x, x.parent.h + self.map.cost(x, x.parent))
259-
# return self.get_kmin()
260243
if len(self.open_list) == 0:
261244
return -1
262245
else:
263246
# lowest priority item is always at index 0 according to docs
264247
return self.open_list[0][0]
265248

266-
# def modify(self, state):
267-
# self.modify_cost(state)
268-
# while True:
269-
# k_min = self.process_state()
270-
# if k_min == -1 or k_min >= state.h:
271-
# break
272-
273-
# def showparents(self):
274-
# return
275-
# for y in range(self.map.row):
276-
# if y == 0:
277-
# print(" ", end='')
278-
# for x in range(self.map.col):
279-
# print(f" {x} ", end='')
280-
# print()
281-
# print(f"{y}: ", end='')
282-
# for x in range(self.map.col):
283-
# x = self.map.map[y][x]
284-
# par = x.parent
285-
# if par is None:
286-
# print(' G ', end='')
287-
# else:
288-
# print(f"({par.x},{par.y}) ", end='')
289-
# print()
290-
# print()
249+
def showparents(self):
250+
for y in range(self.map.row-1, -1, -1):
251+
if y == self.map.row-1:
252+
print(" ", end='')
253+
for x in range(self.map.col):
254+
print(f" {x} ", end='')
255+
print()
256+
print(f"{y}: ", end='')
257+
for x in range(self.map.col):
258+
x = self.map.map[y][x]
259+
par = x.parent
260+
if par is None:
261+
print(' G ', end='')
262+
else:
263+
print(f"({par.x},{par.y}) ", end='')
264+
print()
265+
print()
291266
class DstarPlanner(PlannerBase):
292267
r"""
293268
D* path planner
@@ -322,7 +297,7 @@ def __init__(self, occ_grid=None,
322297
self.costmap = np.where(self.occgrid.grid > 0, np.inf, 1)
323298
self.map = Map(self.costmap.shape[0], self.costmap.shape[1])
324299
self.map.costmap = self.costmap
325-
self.dstar = Dstar(self.map)
300+
self.dstar = Dstar(self.map) #, tol=0)
326301

327302

328303
def plan(self, goal, animate=False, progress=True):
@@ -356,6 +331,9 @@ def plan(self, goal, animate=False, progress=True):
356331
print(self.dstar.ninsert, self.dstar.nin)
357332

358333

334+
@property
335+
def nexpand(self):
336+
return self.dstar.nexpand
359337

360338
def query(self, start, sensor=None, animate=False, verbose=False):
361339
self.start = start
@@ -385,16 +363,18 @@ def query(self, start, sensor=None, animate=False, verbose=False):
385363
val = self.dstar.modify_cost(X, newcost)
386364
# propagate the changes to plan
387365
print('propagate')
366+
# self.dstar.showparents()
388367
while val != -1 and val < tmp.h:
389368
val = self.dstar.process_state(verbose=verbose)
390369
# print('open set', len(self.dstar.open_list))
370+
# self.dstar.showparents()
391371

392372
tmp = tmp.parent
393373
# print('move to ', tmp)
394374

395375
status = namedtuple('DstarStatus', ['cost',])
396376

397-
return np.array(path).T, status(cost)
377+
return np.array(path), status(cost)
398378

399379
# # Just feed self._h into plot function from parent as p
400380

@@ -439,7 +419,7 @@ def sensorfunc(pos):
439419
changes = []
440420
for x in range(3, 6):
441421
for y in range(0, 4):
442-
changes.append((x, y, 5))
422+
changes.append((x, y, 100))
443423
return changes
444424

445425
path2, status2 = ds.query(start=start, sensor=sensorfunc, verbose=False)

0 commit comments

Comments
 (0)