8000 commented out __main__ section · hanm2019/robotics-toolbox-python@1542ae3 · GitHub
[go: up one dir, main page]
Skip to content

Commit 1542ae3

Browse files
jhavljhavl
committed
commented out __main__ section
1 parent 538a868 commit 1542ae3

File tree

1 file changed

+59
-49
lines changed

1 file changed

+59
-49
lines changed

‎roboticstoolbox/mobile/DistanceTransformPlanner.py

Lines changed: 59 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -41,12 +41,13 @@ class DistanceTransformPlanner(PlannerBase):
4141
Also known as wavefront, grassfire or brushfire planning algorithm.
4242
4343
Creates a planner that finds the path between two points in the
44-
plane using forward motion. The path comprises a set of points in
44+
plane using forward motion. The path comprises a set of points in
4545
adjacent cells.
4646
4747
:author: Peter Corke_
4848
:seealso: :class:`Planner`
4949
"""
50+
5051
def __init__(self, occgrid=None, metric="euclidean", **kwargs):
5152

5253
super().__init__(occgrid=occgrid, ndims=2, **kwargs)
@@ -85,29 +86,36 @@ def plan(self, goal=None, animate=False, verbose=False):
8586
self.goal = goal
8687

8788
if self._goal is None:
88-
raise ValueError('No goal specified here or in constructor')
89+
raise ValueError("No goal specified here or in constructor")
8990

90-
self._distancemap = distancexform(self.occgrid.grid,
91-
goal=self._goal, metric=self._metric, animate=animate,
92-
verbose=verbose)
91+
self._distancemap = distancexform(
92+
self.occgrid.grid,
93+
goal=self._goal,
94+
metric=self._metric,
95+
animate=animate,
96+
verbose=verbose,
97+
)
9398

9499
# Use plot from parent class
95100

96101
def next(self, position):
97102
if self.distancemap is None:
98103
Error("No distance map computed, you need to plan.")
99104

100-
directions = np.array([
101-
# dy dx
102-
[-1, -1],
103-
[ 0, -1],
104-
[ 1, -1],
105-
[-1, 0],
106-
[ 0, 0],
107-
[ 1, 0],
108-
[ 0, 1],
109-
[ 1, 1],
110-
], dtype=int)
105+
directions = np.array(
106+
[
107+
# dy dx
108+
[-1, -1],
109+
[0, -1],
110+
[1, -1],
111+
[-1, 0],
112+
[0, 0],
113+
[1, 0],
114+
[0, 1],
115+
[1, 1],
116+
],
117+
dtype=int,
118+
)
111119

112120
x = int(position[0])
113121
y = int(position[1])
@@ -137,28 +145,30 @@ def next(self, position):
137145

138146
def plot_3d(self, p=None, ls=None):
139147
fig = plt.figure()
140-
ax = fig.gca(projection='3d')
148+
ax = fig.gca(projection="3d")
141149

142150
distance = self._distancemap
143151
X, Y = np.meshgrid(np.arange(distance.shape[1]), np.arange(distance.shape[0]))
144-
surf = ax.plot_surface(X, Y, distance, #cmap='gray',
145-
linewidth=1, antialiased=False)
152+
surf = ax.plot_surface(
153+
X, Y, distance, linewidth=1, antialiased=False # cmap='gray',
154+
)
146155

147156
if p is not None:
148157
# k = sub2ind(np.shape(self._distancemap), p[:, 1], p[:, 0])
149-
height = distance[p[:,1], p[:,0]]
158+
height = distance[p[:, 1], p[:, 0]]
150159
ax.plot(p[:, 0], p[:, 1], height)
151160

152-
plt.xlabel('x')
153-
plt.ylabel('y')
154-
ax.set_zlabel('z')
161+
plt.xlabel("x")
162+
plt.ylabel("y")
163+
ax.set_zlabel("z")
155164
plt.show()
156165
return ax
157166

158167

159168
import numpy as np
160169

161-
def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=False):
170+
171+
def distancexform(occgrid, goal, metric="cityblock", animate=False, verbose=False):
162172
"""
163173
Distance transform for path planning
164174
@@ -180,7 +190,7 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=Fals
180190
``metric``. In addition:
181191
182192
- Obstacle cells will be set to ``nan``
183-
- Unreachable cells, ie. free cells _inside obstacles_ will be set
193+
- Unreachable cells, ie. free cells _inside obstacles_ will be set
184194
to ``inf``
185195
186196
The cells of the passed occupancy grid are:
@@ -197,19 +207,19 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=Fals
197207

198208
distance = occgrid.astype(np.float32)
199209
distance[occgrid > 0] = np.nan # assign nan to obstacle cells
200-
distance[occgrid==0] = np.inf # assign inf to other cells
210+
distance[occgrid == 0] = np.inf # assign inf to other cells
201211
distance[goal[1], goal[0]] = 0 # assign zero to goal
202-
212+
203213
# create the appropriate distance matrix D
204-
if metric.lower() in ('manhattan', 'cityblock'):
214+
if metric.lower() in ("manhattan", "cityblock"):
205215
# fmt: off
206216
D = np.array([
207217
[ np.inf, 1, np.inf],
208218
[ 1, 0, 1],
209219
[ np.inf, 1, np.inf]
210220
])
211221
# fmt: on
212-
elif metric.lower() == 'euclidean':
222+
elif metric.lower() == "euclidean":
213223
r2 = np.sqrt(2)
214224
# fmt: off
215225
D = np.array([
@@ -236,15 +246,15 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=Fals
236246
display[np.isinf(display)] = 0
237247
if h is None:
238248
plt.figure()
239-
plt.xlabel('x')
240-
plt.ylabel('y')
249+
plt.xlabel("x")
250+
plt.ylabel("y")
241251
ax = plt.gca()
242252
plt.pause(0.001)
243-
cmap = cm.get_cmap('gray')
244-
cmap.set_bad('red')
245-
cmap.set_over('white')
253+
cmap = cm.get_cmap("gray")
254+
cmap.set_bad("red")
255+
cmap.set_over("white")
246256
h = plt.imshow(display, cmap=cmap)
247-
plt.colorbar(label='distance')
257+
plt.colorbar(label="distance")
248258
else:
249259
h.remove()
250260
h = plt.imshow(display, cmap=cmap)
@@ -262,12 +272,13 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=Fals
262272
print(f"{count:d} iterations, {ninf:d} unreachable cells")
263273
return distance
264274

275+
265276
def grassfire_step(G, D):
266277

267278
# pad with inf
268-
H = np.pad(G, max(D.shape) // 2, 'constant', constant_values=np.inf)
279+
H = np.pad(G, max(D.shape) // 2, "constant", constant_values=np.inf)
269280
rows, columns = G.shape
270-
281+
271282
# inspired by https://landscapearchaeology.org/2018/numpy-loops/
272283
minimum = np.full(G.shape, np.inf)
273284
for y in range(3):
@@ -280,7 +291,7 @@ def grassfire_step(G, D):
280291

281292

282293
if __name__ == "__main__":
283-
294+
pass
284295

285296
# # make a simple map, as per the MOOCs
286297
# occgrid = np.zeros((10,10))
@@ -296,16 +307,15 @@ def grassfire_step(G, D):
296307
# dx = distancexform(occgrid, goal, metric='Euclidean')
297308
# print(dx)
298309

310+
# from roboticstoolbox import DistanceTransformPlanner, rtb_loadmat
299311

300-
from roboticstoolbox import DistanceTransformPlanner, rtb_loadmat
312+
# house = rtb_loadmat('data/house.mat')
313+
# floorplan = house['floorplan']
314+
# places = house['places']
301315

302-
house = rtb_loadmat('data/house.mat')
303-
floorplan = house['floorplan']
304-
places = house['places']
305-
306-
dx = DistanceTransformPlanner(floorplan)
307-
print(dx)
308-
dx.goal = [1,2]
309-
dx.plan(places.kitchen)
310-
path = dx.query(places.br3)
311-
dx.plot(path, block=True)
316+
# dx = DistanceTransformPlanner(floorplan)
317+
# print(dx)
318+
# dx.goal = [1,2]
319+
# dx.plan(places.kitchen)
320+
# path = dx.query(places.br3)
321+
# dx.plot(path, block=True)

0 commit comments

Comments
 (0)
0