-
Notifications
You must be signed in to change notification settings - Fork 17
/
Copy pathDistanceTransformPlanner.m
258 lines (223 loc) · 9.4 KB
/
DistanceTransformPlanner.m
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
%DistanceTransformPlanner Distance transform navigation class
%
% A concrete subclass of the abstract Navigation class that implements the
% distance transform navigation algorithm which computes minimum distance
% paths.
%
% Methods::
% DistanceTransformPlanner Constructor plan Compute the cost
% map given a goal and map query Find a path plot Display
% the distance function and obstacle map plot3d Display the distance
% function as a surface display Print the parameters in human
% readable form char Convert to string
%
% Properties (read only)::
% distancemap Distance from each point to the goal. metric The
% distance metric, can be 'euclidean' (default) or 'manhattan'
%
% Example::
%
% load map1 % load map goal = [50,30]; % goal point
% start = [20, 10]; % start point dx =
% DistanceTransformPlanner(map); % create navigation object
% dx.plan(goal) % create plan for specified goal
% dx.query(start) % animate path from this start location
%
% Notes:: - Obstacles are represented by NaN in the distancemap. - The
% value of each element in the distancemap is the shortest distance from
% the
% corresponding point in the map to the current goal.
%
% References:: - Robotics, Vision & Control, Sec 5.2.1,
% Peter Corke, Springer, 2011.
%
% See also Navigation, Dstar, PRM, distancexform.
% Copyright 2022-2023 Peter Corke, Witold Jachimczyk, Remo Pillat
classdef DistanceTransformPlanner < Navigation
properties
metric; % distance metric
distancemap; % distance transform results
end
methods
function dx = DistanceTransformPlanner(world, varargin)
%DistanceTransformPlanner.DistanceTransformPlanner Distance transform constructor
%
% DX = DistanceTransformPlanner(MAP, OPTIONS) is a distance transform navigation object,
% and MAP is an occupancy grid, a representation of a planar
% world as a matrix whose elements are 0 (free space) or 1
% (occupied).
%
% Options::
% 'goal',G Specify the goal point (2x1)
% 'metric',M Specify the distance metric as 'euclidean' (default)
% or 'manhattan'.
% 'inflate',K Inflate all obstacles by K cells.
%
% Other options are supported by the Navigation superclass.
%
% See also Navigation.Navigation.
% TODO NEEDS PROPER ARG HANDLER
opt.metric = {'euclidean', 'manhattan', 'cityblock'};
[opt, args] = tb_optparse(opt, varargin);
% invoke the superclass constructor
dx = dx@Navigation(world, args{:});
dx.metric = opt.metric;
dx.verbose = opt.verbose;
end
function s = char(dx)
%DistanceTransformPlanner.char Convert to string
%
% DX.char() is a string representing the state of the object in
% human-readable form.
%
% See also DistanceTransformPlanner.display, Navigation.char
% most of the work is done by the superclass
s = char@Navigation(dx);
% DistanceTransformPlanner specific stuff
s = char(s, sprintf(' distance metric: %s', dx.metric));
if ~isempty(dx.distancemap)
s = char(s, sprintf(' distancemap: computed:'));
else
s = char(s, sprintf(' distancemap: empty:'));
end
end
% invoked by superclass on a change of goal, mark the distancemap
% as invalid
function goal_change(dx, ~)
dx.distancemap = [];
if dx.verbose
disp('Goal changed -> distancemap cleared');
end
end
function plan(dx, varargin)
%DistanceTransformPlanner.plan Plan path to goal
%
% DX.plan(GOAL) plans a path to the goal given to the constructor,
% updates the internal distancemap where the value of each element is the
% minimum distance from the corresponding point to the goal.
%
% Notes::
% - This may take many seconds.
%
% See also Navigation.path.
if ~isempty(varargin) && isvec(varargin{1},2)
dx.setgoal(varargin{1});
end
% check the goal point is sane
assert(~isempty(dx.goal), 'RTB:DistanceTransformPlanner:plan', 'no goal specified here or in constructor');
occgrid = double(dx.occgridnav);
assert(occgrid(dx.goal(2), dx.goal(1)) == 0, 'RTB:distancexform:badarg', 'goal inside obstacle')
% solve using IPT
switch dx.metric
case {'cityblock', 'manhattan'}
ipt_metric = 'cityblock';
case 'euclidean'
ipt_metric = 'quasi-euclidean';
end
dx.distancemap = double( bwdistgeodesic(occgrid==0, dx.goal(1), dx.goal(2), ipt_metric) );
end
function plot(dx, varargin)
%DistanceTransformPlanner.plot Visualize navigation environment
%
% DX.plot(OPTIONS) displays the occupancy grid and the goal distance
% in a new figure. The goal distance is shown by intensity which
% increases with distance from the goal. Obstacles are overlaid
% and shown in red.
%
% DX.plot(P, OPTIONS) as above but also overlays a path given by the set
% of points P (Mx2).
%
% Notes::
% - See Navigation.plot for options.
%
% See also Navigation.plot.
plot@Navigation(dx, varargin{:}, 'distance', dx.distancemap);
end
function n = next(dx, robot)
if isempty(dx.distancemap)
error('No distancemap computed, you need to plan');
end
% list of all possible directions to move from current cell
directions = [
-1 -1
0 -1
1 -1
-1 0
0 0
1 0
-1 1
0 1
1 1];
x = robot(1); y = robot(2);
% find the neighbouring cell that has the smallest distance
mindist = Inf;
mindir = [];
for d=directions'
% use exceptions to catch attempt to move outside the map
try
if dx.distancemap(y+d(1), x+d(2)) < mindist
mindir = d;
mindist = dx.distancemap(y+d(1), x+d(2));
end
catch
end
end
x = x + mindir(2);
y = y + mindir(1);
if all([x;y] == dx.goal)
n = []; % indicate we are at the goal
else
n = [x; y]; % else return the next closest point to the goal
end
end % next
function plot3d(dx, p, varargin)
%DistanceTransformPlanner.plot3d 3D costmap view
%
% DX.plot3d() displays the distance function as a 3D surface with
% distance from goal as the vertical axis. Obstacles are "cut out"
% from the surface.
%
% DX.plot3d(P) as above but also overlays a path given by the set
% of points P (Mx2).
%
% DX.plot3d(P, LS) as above but plot the line with the MATLAB linestyle LS.
%
% See also Navigation.plot.
opt.pathmarker = {};
opt.startmarker = {};
opt.goalmarker = {};
opt.goal = true;
opt.start = true;
pathmarker = {"go", "MarkerSize", 8, "MarkerEdgeColor", "black", ...
"MarkerFaceColor", "green", "LineWidth", 2};
startmarker = {"bo", "MarkerFaceColor", "w", "MarkerEdgeColor", "k", ...
"MarkerSize", 16, "LineWidth", 1};
goalmarker = {"bp", "MarkerFaceColor", "w", "MarkerEdgeColor", "k", ...
"MarkerSize", 22, "LineWidth", 1};
opt = tb_optparse(opt, varargin);
surf(dx.distancemap);
colormap("gray")
shading interp
if nargin > 1
% plot path if provided
k = sub2ind(size(dx.distancemap), p(:,2), p(:,1));
height = dx.distancemap(k);
hold on
plot3(p(:,1), p(:,2), height, pathmarker{:}, ...
opt.pathmarker{:}, "Tag", "path")
if opt.goal && ~isempty(dx.goal)
plot3(dx.goal(1), dx.goal(2), dx.distancemap(dx.goal(2), dx.goal(1)), ...
goalmarker{:}, opt.goalmarker{:}, 'Tag', 'goal');
end
if opt.start && ~isempty(dx.start)
plot3(dx.start(1), dx.start(2), dx.distancemap(dx.start(2), dx.start(1)) + 0.1, ...
startmarker{:}, opt.startmarker{:}, 'Tag', 'start');
end
hold off
end
xlabel x
ylabel y
zlabel("Distance from goal")
end
end % methods
end % classdef