You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
151 lines
4.7 KiB
151 lines
4.7 KiB
%MAKEMAP Make an occupancy map
|
|
%
|
|
% map = makemap(N) is an occupancy grid map (NxN) created by a simple
|
|
% interactive editor. The map is initially unoccupied and obstacles can
|
|
% be added using geometric primitives.
|
|
%
|
|
% map = makemap() as above but N=128.
|
|
%
|
|
% map = makemap(map0) as above but the map is initialized from the
|
|
% occupancy grid map0, allowing obstacles to be added.
|
|
%
|
|
% With focus in the displayed figure window the following commands can
|
|
% be entered:
|
|
% left button click and drag to create a rectangle
|
|
% p draw polygon
|
|
% c draw circle
|
|
% u undo last action
|
|
% e erase map
|
|
% q leave editing mode and return map
|
|
%
|
|
% See also: DXForm, PRM, RRT.
|
|
|
|
function world = makemap(Nw)
|
|
|
|
if nargin < 1
|
|
Nw = 128;
|
|
end
|
|
|
|
if isscalar(Nw)
|
|
% initialize a new map
|
|
world = zeros(Nw, Nw);
|
|
else
|
|
% we were passed a map
|
|
world = Nw;
|
|
end
|
|
|
|
imagesc(world);
|
|
set(gca, 'Ydir', 'normal');
|
|
grid
|
|
binary_cmap = [1 1 1; 1 0 0];
|
|
colormap(binary_cmap);
|
|
caxis([0 1]);
|
|
figure(gcf)
|
|
set(gcf, 'Name', 'makemap');
|
|
|
|
fprintf('makemap:\n');
|
|
fprintf(' left button, click and drag to create a rectangle\n');
|
|
fprintf(' or type the following letters in the figure window:\n');
|
|
fprintf(' p - draw polygon\n');
|
|
fprintf(' c - draw circle\n');
|
|
fprintf(' e - erase map\n');
|
|
fprintf(' u - undo last action\n');
|
|
fprintf(' q - leave editing mode\n');
|
|
|
|
|
|
while 1
|
|
drawnow
|
|
|
|
k = waitforbuttonpress;
|
|
if k == 1,
|
|
% key pressed
|
|
c = get(gcf, 'CurrentCharacter');
|
|
switch c
|
|
case 'p'
|
|
fprintf('click a sequence of points, <enter> when done')
|
|
title('click a sequence of points, <enter> when done')
|
|
xy = ginput;
|
|
|
|
fprintf('\r \r')
|
|
title('')
|
|
|
|
xy = round(xy);
|
|
world_prev = world;
|
|
[X,Y] = meshgrid(1:Nw, 1:Nw);
|
|
world = world + inpolygon(X, Y, xy(:,1), xy(:,2));
|
|
|
|
case 'c'
|
|
fprintf('click a centre point')
|
|
title('click a centre point')
|
|
waitforbuttonpress;
|
|
point1 = get(gca,'CurrentPoint'); % button down detected
|
|
point1 = round(point1(1,1:2)); % extract x and y
|
|
hold on
|
|
h = plot(point1(1), point1(2), '+');
|
|
drawnow
|
|
|
|
fprintf('\rclick a circumference point')
|
|
title('click a circumference point')
|
|
waitforbuttonpress;
|
|
point2 = get(gca,'CurrentPoint'); % button up detected
|
|
point2 = round(point2(1,1:2));
|
|
|
|
fprintf('\r \r')
|
|
title('')
|
|
delete(h);
|
|
drawnow
|
|
hold off
|
|
|
|
r = round(colnorm(point1-point2));
|
|
c = kcircle(r);
|
|
|
|
world_prev = world;
|
|
|
|
% add the circle to the world
|
|
world(point1(2)-r:point1(2)+r,point1(1)-r:point1(1)+r) = ...
|
|
world(point1(2)-r:point1(2)+r,point1(1)-r:point1(1)+r) + c;
|
|
% ensure maximum value of occ.grid is 1
|
|
world = min(world, 1);
|
|
|
|
case 'e'
|
|
world_prev = world;
|
|
world = zeros(Nw, Nw);
|
|
|
|
case 'u'
|
|
if ~isempty(world_prev)
|
|
world = world_prev;
|
|
end
|
|
|
|
otherwise
|
|
break; % key pressed
|
|
end
|
|
else
|
|
% button pressed
|
|
point1 = get(gca,'CurrentPoint'); % button down detected
|
|
point2 = get(gca,'CurrentPoint'); % button up detected
|
|
|
|
point1 = round(point1(1,1:2)); % extract x and y
|
|
point2 = round(point2(1,1:2));
|
|
|
|
x1 = max(1, point1(1));
|
|
x2 = min(Nw, point2(1));
|
|
y1 = max(1, point1(2));
|
|
y2 = min(Nw, point2(2));
|
|
|
|
if x1 > x2
|
|
t = x1; x1 = x2; x2 = t;
|
|
end
|
|
if y1 > y2
|
|
t = y1; y1 = y2; y2 = t;
|
|
end
|
|
|
|
world_prev = world;
|
|
world(y1:y2, x1:x2) = 1;
|
|
end
|
|
|
|
% ensure maximum value of occ.grid is 1
|
|
world = min(world, 1);
|
|
|
|
% update the world display
|
|
set(gco, 'CData', world);
|
|
end
|