git commit -m "first commit"
This commit is contained in:
122
navigations/sbpl/matlab/visualization/plot_3Dpath.m
Executable file
122
navigations/sbpl/matlab/visualization/plot_3Dpath.m
Executable file
@@ -0,0 +1,122 @@
|
||||
% /*
|
||||
% * 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]);
|
||||
Reference in New Issue
Block a user