123 lines
4.4 KiB
Matlab
Executable File
123 lines
4.4 KiB
Matlab
Executable File
% /*
|
|
% * Copyright (c) 2008, Maxim Likhachev
|
|
% * All rights reserved.
|
|
% *
|
|
% * Redistribution and use in source and binary forms, with or without
|
|
% * modification, are permitted provided that the following conditions are met:
|
|
% *
|
|
% * * Redistributions of source code must retain the above copyright
|
|
% * notice, this list of conditions and the following disclaimer.
|
|
% * * Redistributions in binary form must reproduce the above copyright
|
|
% * notice, this list of conditions and the following disclaimer in the
|
|
% * documentation and/or other materials provided with the distribution.
|
|
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
|
% * contributors may be used to endorse or promote products derived from
|
|
% * this software without specific prior written permission.
|
|
% *
|
|
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
% * POSSIBILITY OF SUCH DAMAGE.
|
|
% */
|
|
function[] = plot_3Dpath(solfilename, mapfilename, resolution)
|
|
|
|
%
|
|
%Plots a 3D path overlaid on top of the map.
|
|
%Resolution should be in meters
|
|
%
|
|
%written by Maxim Likhachev
|
|
%---------------------------------------------------
|
|
%
|
|
|
|
%close all;
|
|
|
|
obsthresh = 254;
|
|
|
|
robot_width = 0.6;
|
|
robot_length = 1.5;
|
|
|
|
vehicle = [-robot_length/2.0 -robot_width/2.0
|
|
robot_length/2.0 -robot_width/2.0
|
|
robot_length/2.0 robot_width/2.0
|
|
-robot_length/2.0 robot_width/2.0];
|
|
|
|
|
|
cellsize = resolution;
|
|
x = load(solfilename);
|
|
|
|
%now read in map
|
|
fmap = fopen(mapfilename, 'r');
|
|
xsize = -1;
|
|
ysize = -1;
|
|
while(feof(fmap) ~= 1)
|
|
s = fscanf(fmap, '%s', 1);
|
|
if (strcmp('environment:',s) == 1)
|
|
break;
|
|
elseif (strcmp('discretization(cells):', s) == 1)
|
|
xsize = fscanf(fmap, '%d', 1);
|
|
ysize = fscanf(fmap, '%d', 1);
|
|
end;
|
|
end;
|
|
%read the environment itself
|
|
fprintf(1, 'reading in map of size %d by %d\n', xsize, ysize);
|
|
map = fscanf(fmap, '%d', [xsize, ysize]);
|
|
map = map'; %correct matlab loading order
|
|
|
|
figure(3);
|
|
%h = plot(x(:,1),x(:,2), 'k');
|
|
%h = plot(x(:,2),size(map,1)*cellsize-x(:,1), 'k');
|
|
h = plot(x(:,1),size(map,1)*cellsize-x(:,2), 'k');
|
|
set(h,'LineWidth',3);
|
|
hold on;
|
|
|
|
h = text(x(1,1), size(map,1)*cellsize-x(1,2), 'START');
|
|
set(h,'LineWidth',5);
|
|
h = text(x(size(x,1),1), size(map,1)*cellsize-x(size(x,1),2), 'GOAL');
|
|
set(h,'LineWidth',5);
|
|
|
|
%plot vehicle
|
|
%for pind = 1:ceil(length(x)/40):length(x)
|
|
for pind = 1:5:length(x)
|
|
for i = 1:4
|
|
xstartrot = vehicle(i,1)*cos(x(pind,3)) - vehicle(i,2)*sin(x(pind,3)) + x(pind,1);
|
|
ystartrot = vehicle(i,1)*sin(x(pind,3)) + vehicle(i,2)*cos(x(pind,3)) + x(pind,2);
|
|
if i < 4
|
|
xendrot = vehicle(i+1,1)*cos(x(pind,3)) - vehicle(i+1,2)*sin(x(pind,3)) + x(pind,1);;
|
|
yendrot = vehicle(i+1,1)*sin(x(pind,3)) + vehicle(i+1,2)*cos(x(pind,3)) + x(pind,2);;
|
|
else
|
|
xendrot = vehicle(1,1)*cos(x(pind,3)) - vehicle(1,2)*sin(x(pind,3)) + x(pind,1);;
|
|
yendrot = vehicle(1,1)*sin(x(pind,3)) + vehicle(1,2)*cos(x(pind,3)) + x(pind,2);;
|
|
end;
|
|
%%% plot([xstartrot xendrot], [ystartrot yendrot]);
|
|
%%% plot([ystartrot yendrot], [size(map,1)*cellsize-xstartrot size(map,1)*cellsize-xendrot]);
|
|
plot([xstartrot xendrot], [size(map,1)*cellsize-ystartrot size(map,1)*cellsize-yendrot]);
|
|
end;
|
|
end;
|
|
|
|
if 1
|
|
for row = 1:size(map,1)
|
|
for col = 1:size(map,2)
|
|
if(map(row,col) >= obsthresh)
|
|
% plot(col*cellsize,row*cellsize,'x');
|
|
plot(col*cellsize, (size(map,1)-row)*cellsize,'x');
|
|
elseif(map(row,col) > 0)
|
|
%plot(col*cellsize, (size(map,1)-row)*cellsize,'.');
|
|
end;
|
|
end;
|
|
end;
|
|
end;
|
|
|
|
%%fplot('2',[6 40 0 5]);
|
|
%%fplot('18',[6 40 0 5]);
|
|
%%plot(6*ones(length([2:18]),1), [2:18]);
|
|
|
|
%axis([min(x(:,1))-1 max(x(:,1))+1 min(x(:,2))-1 max(x(:,2))+1]);
|
|
%axis([-1 5 -1 5]);
|