Skip to content

Commit 6072ab9

Browse files
petercorkepetercorke
committed
More planners
changed Navigation class to Planner
1 parent 832d35a commit 6072ab9

File tree

4 files changed

+580
-469
lines changed

4 files changed

+580
-469
lines changed

‎roboticstoolbox/mobile/Bug2Planner.py

Lines changed: 96 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -14,51 +14,52 @@
1414
from spatialmath.base.animate import *
1515
from scipy.ndimage import *
1616
from matplotlib import cm
17-
from roboticstoolbox.mobile.navigation import Navigation
17+
import matplotlib.pyplot as plt
18+
from matplotlib import animation
19+
from roboticstoolbox.mobile.Planner import Planner
1820

19-
20-
class Bug2(Navigation):
21+
class Bug2Planner(Planner):
2122
def __init__(self, occ_grid, **kwargs):
2223

2324
"""
2425
2526
%Bug2.Bug2 Construct a Bug2 navigation object
2627
%
27-
% B = Bug2(MAP, OPTIONS) is a bug2 navigation object, and MAP is an occupancy grid,
28-
% a representation of a planar world as a matrix whose elements are 0 (free
29-
% space) or 1 (occupied).
28+
B = Bug2(MAP, OPTIONS) is a bug2 navigation object, and MAP is an occupancy grid,
29+
a representation of a planar world as a matrix whose elements are 0 (free
30+
space) or 1 (occupied).
3031
%
31-
% Options::
32-
% 'goal',G Specify the goal point (1x2)
33-
% 'inflate',K Inflate all obstacles by K cells.
32+
Options::
33+
'goal',G Specify the goal point (1x2)
34+
'inflate',K Inflate all obstacles by K cells.
3435
%
35-
% See also Navigation.Navigation.
36+
See also Navigation.Navigation.
3637
3738
%Navigation.Navigation Create a Navigation object
3839
%
39-
% N = Navigation(OCCGRID, OPTIONS) is a Navigation object that holds an
40-
% occupancy grid OCCGRID. A number of options can be be passed.
40+
N = Navigation(OCCGRID, OPTIONS) is a Navigation object that holds an
41+
occupancy grid OCCGRID. A number of options can be be passed.
4142
%
42-
% Options::
43-
% 'goal',G Specify the goal point (2x1)
44-
% 'inflate',K Inflate all obstacles by K cells.
45-
% 'private' Use private random number stream.
46-
% 'reset' Reset random number stream.
47-
% 'verbose' Display debugging information
48-
% 'seed',S Set the initial state of the random number stream. S must
49-
% be a proper random number generator state such as saved in
50-
% the seed0 property of an earlier run.
43+
Options::
44+
'goal',G Specify the goal point (2x1)
45+
'inflate',K Inflate all obstacles by K cells.
46+
'private' Use private random number stream.
47+
'reset' Reset random number stream.
48+
'verbose' Display debugging information
49+
'seed',S Set the initial state of the random number stream. S must
50+
be a proper random number generator state such as saved in
51+
the seed0 property of an earlier run.
5152
%
52-
% Notes::
53-
% - In the occupancy grid a value of zero means free space and non-zero means
54-
% occupied (not driveable).
55-
% - Obstacle inflation is performed with a round structuring element (kcircle)
56-
% with radius given by the 'inflate' option.
57-
% - Inflation requires either MVTB or IPT installed.
58-
% - The 'private' option creates a private random number stream for the methods
59-
% rand, randn and randi. If not given the global stream is used.
53+
Notes::
54+
- In the occupancy grid a value of zero means free space and non-zero means
55+
occupied (not driveable).
56+
- Obstacle inflation is performed with a round structuring element (kcircle)
57+
with radius given by the 'inflate' option.
58+
- Inflation requires either MVTB or IPT installed.
59+
- The 'private' option creates a private random number stream for the methods
60+
rand, randn and randi. If not given the global stream is used.
6061
%
61-
% See also randstream.
62+
See also randstream.
6263
6364
"""
6465
super().__init__(occ_grid=occ_grid, **kwargs)
@@ -100,102 +101,100 @@ def edge(self):
100101
def k(self):
101102
return self._k
102103

103-
def query(self, start=None, goal=None, animate=False, movie=None, current=False):
104+
# override query method of base class
105+
def query(self, start=None, goal=None, animate=False, movie=None, trail=True):
104106
"""
105-
[summary]
106-
107-
:param start: [description], defaults to None
108-
:type start: [type], optional
109-
:param goal: [description], defaults to None
110-
:type goal: [type], optional
111-
:param animate: [description], defaults to False
107+
Find a path using Bug2
108+
109+
:param start: starting position
110+
:type start: array_like(2)
111+
:param goal: goal position
112+
:type goal: array_like(2)
113+
:param animate: show animation of robot moving over the map,
114+
defaults to False
112115
:type animate: bool, optional
113-
:param movie: [description], defaults to None
114-
:type movie: [type], optional
115-
:param current: [description], defaults to False
116+
:param movie: save animation as a movie, defaults to None. Is either
117+
name of movie or a tuple (filename, frame interval)
118+
:type movie: str or tuple(str, float), optional
119+
:param trail: show the path followed by the robot, defaults to True
116120
:type current: bool, optional
117-
:return: [description]
118-
:rtype: [type]
121+
:return: path from ``start`` to ``goal``
122+
:rtype: ndarray(2,N)
119123
120-
%Bug2.query Find a path
121-
%
122-
% B.query(START, GOAL, OPTIONS) is the path (Nx2) from START (1x2) to GOAL
123-
% (1x2). Row are the coordinates of successive points along the path. If
124-
% either START or GOAL is [] the grid map is displayed and the user is
125-
% prompted to select a point by clicking on the plot.
126-
%
127-
% Options::
128-
% 'animate' show a simulation of the robot moving along the path
129-
% 'movie',M create a movie
130-
% 'current' show the current position as a black circle
131-
%
132-
% Notes::
133-
% - START and GOAL are given as X,Y coordinates in the grid map, not as
134-
% MATLAB row and column coordinates.
135-
% - START and GOAL are tested to ensure they lie in free space.
136-
% - The Bug2 algorithm is completely reactive so there is no planning
137-
% method.
138-
% - If the bug does a lot of back tracking it's hard to see the current
139-
% position, use the 'current' option.
140-
% - For the movie option if M contains an extension a movie file with that
141-
% extension is created. Otherwise a folder will be created containing
142-
% individual frames.
143-
%
144-
% See also Animate.
124+
Compute the path from ``start`` to `goal` assuming the robot is capable
125+
of 8-way motion from its current cell to the next.
126+
127+
.. note::
128+
129+
- ``start`` and `goal` are given as (x,y) coordinates in the grid map,
130+
not as matrix row and column coordinates.
131+
- ``start`` and ``goal`` are tested to ensure they lie in free space
132+
- The Bug2 algorithm is completely reactive, there is no planning so
133+
paths can be very inefficient.
134+
- If the bug does a lot of back tracking it's hard to see the current
135+
position, set ``trail=False``
136+
- For the ``movie`` option specify a file or a tuple containing the
137+
filename and the frame interval (1/fps) in seconds.
138+
139+
:seealso: :class:`MovieWriter`
145140
"""
146-
anim = None
147-
if movie is not None:
148-
anim = Animate(movie)
149-
animate = True
150141

151142
# make sure start and goal are set and valid
152-
self.check_query(start, goal)
143+
self.check_points(start, goal)
153144

154145
# compute the m-line
155146
# create homogeneous representation of the line
156147
# line*[x y 1]' = 0
157-
self._m_line = hom_line(self._start[0], self._start[1],
158-
self._goal[0], self._goal[1])
148+
self._m_line = hom_line(self._start, self._goal)
149+
150+
if movie is not None:
151+
animate = True
159152

160153
if animate:
161154
self.plot()
162155
self.plot_m_line()
156+
plt.pause(0.05)
157+
158+
movie = MovieWriter(movie)
163159

164160
robot = self._start[:]
165161
self._step = 1
166-
path = [self._start]
162+
path = np.r_[self._start]
167163
h = None
168164

165+
trail_line, = plt.plot(0, 0, 'y.')
166+
trail_head, = plt.plot(0, 0, 'ko')
167+
169168
# iterate using the next() method until we reach the goal
170169
while True:
171170
if animate:
172-
plt.plot(robot[0], robot[1], 'y.')
173-
plt.pause(0.1)
174-
if current:
175-
h = self.plot(robot)
176-
plt.draw()
177-
if movie is not None:
178-
anim.plot(h)
179-
if current:
180-
self.delete(h)
171+
trail_head.set_data(robot[0], robot[1])
172+
if trail:
173+
trail_line.set_data(path.T)
174+
175+
plt.pause(0.001)
176+
# plt.draw()
177+
# plt.show(block=False)
178+
# plt.gcf().canvas.flush_events()
179+
180+
movie.add()
181181

182182
# move to next point on path
183183
robot = self.next(robot)
184184

185-
# have we been here before, ie. in a loop
186-
if any([all(robot == x) for x in path]):
187-
raise RuntimeError('trapped')
185+
# # have we been here before, ie. in a loop
186+
# if any([all(robot == x) for x in path]):
187+
# raise RuntimeError('trapped')
188188

189189
# are we there yet?
190190
if robot is None:
191191
break
192192
else:
193-
path.append(robot)
193+
path = np.vstack((path, robot))
194194

195-
if movie is not None:
196-
anim.done()
195+
movie.done()
197196

198-
return np.r_[path]
197+
return path
199198

200199
def plot_m_line(self, ls=None):
201200
if ls is None:
@@ -290,6 +289,7 @@ def plan(self):
290289
# https://github.com/petercorke/toolbox-common-matlab/blob/master/edgelist.m
291290

292291
# these are directions of 8-neighbours in a clockwise direction
292+
# fmt: off
293293
_dirs = np.array([
294294
[-1, 0],
295295
[-1, 1],
@@ -300,7 +300,7 @@ def plan(self):
300300
[ 0, -1],
301301
[-1, -1],
302302
])
303-
303+
# fmt: on
304304
def edgelist(im, p, direction=1):
305305
"""
306306
Find edge of a region
@@ -446,8 +446,8 @@ def adjacent_point(im, seed, pix0):
446446

447447
# Implementation of Peter Corke's matlab homline function from:
448448
# https://github.com/petercorke/spatialmath-matlab/blob/master/homline.m
449-
def hom_line(x1, y1, x2, y2):
450-
line = np.cross(np.r_[x1, y1, 1], np.r_[x2, y2, 1])
449+
def hom_line(p1, p2):
450+
line = np.cross(np.r_[p1[0], p1[1], 1], np.r_[p2[0], p2[1], 1])
451451

452452
# normalize so that the result of x*l' is the pixel distance
453453
# from the line
@@ -465,7 +465,7 @@ def hom_line(x1, y1, x2, y2):
465465
# ind[ind < 0] = -1
466466
# ind[ind >= array_shape[0]*array_shape[1]] = -1
467467
# rows = (ind.astype('int') / array_shape[1])
468-
# cols = ind % array_shape[1]
468+
# cols = ind array_shape[1]
469469
# return rows, cols
470470

471471
# def col_norm(x):

0 commit comments

Comments
 (0)