Skip to content

GitLab

Commit 6970fd07 authored by Farid Alijani's avatar Farid Alijani
Browse files

Laser Scanner data updated

parent e4729a17
Expand all Hide whitespace changes
Inline Side-by-side
Files_4_thesis/magnify.m 0 → 100644
View file @ 6970fd07
function magnify(f1)
%
%magnify(f1)
%
% Figure creates a magnification box when under the mouse
% position when a button is pressed. Press '+'/'-' while
% button pressed to increase/decrease magnification. Press
% '>'/'<' while button pressed to increase/decrease box size.
% Hold 'Ctrl' while clicking to leave magnification on figure.
%
% Example:
% plot(1:100,randn(1,100),(1:300)/3,rand(1,300)), grid on,
% magnify;
% Rick Hindman - 7/29/04
if (nargin == 0), f1 = gcf; end;
set(f1, ...
'WindowButtonDownFcn', @ButtonDownCallback, ...
'WindowButtonUpFcn', @ButtonUpCallback, ...
'WindowButtonMotionFcn', @ButtonMotionCallback, ...
'KeyPressFcn', @KeyPressCallback);
return;
function ButtonDownCallback(src,eventdata)
f1 = src;
a1 = get(f1,'CurrentAxes');
a2 = copyobj(a1,f1);
set(f1, ...
'UserData',[f1,a1,a2], ...
'Pointer','fullcrosshair', ...
'CurrentAxes',a2);
set(a2, ...
'UserData',[2,0.2], ... %magnification, frame size
'Color',get(a1,'Color'), ...
'Box','on');
xlabel(''); ylabel(''); zlabel(''); title('');
set(get(a2,'Children'), ...
'LineWidth', 2);
set(a1, ...
'Color',get(a1,'Color')*0.95);
set(f1, ...
'CurrentAxes',a1);
ButtonMotionCallback(src);
return;
function ButtonUpCallback(src,eventdata)
H = get(src,'UserData');
f1 = H(1); a1 = H(2); a2 = H(3);
set(a1, ...
'Color',get(a2,'Color'));
set(f1, ...
'UserData',[], ...
'Pointer','arrow', ...
'CurrentAxes',a1);
if ~strcmp(get(f1,'SelectionType'),'alt'),
delete(a2);
end;
return;
function ButtonMotionCallback(src,eventdata)
H = get(src,'UserData');
if ~isempty(H)
f1 = H(1); a1 = H(2); a2 = H(3);
a2_param = get(a2,'UserData');
f_pos = get(f1,'Position');
a1_pos = get(a1,'Position');
[f_cp, a1_cp] = pointer2d(f1,a1);
set(a2,'Position',[(f_cp./f_pos(3:4)) 0 0]+a2_param(2)*a1_pos(3)*[-1 -1 2 2]);
a2_pos = get(a2,'Position');
set(a2,'XLim',a1_cp(1)+(1/a2_param(1))*(a2_pos(3)/a1_pos(3))*diff(get(a1,'XLim'))*[-0.5 0.5]);
set(a2,'YLim',a1_cp(2)+(1/a2_param(1))*(a2_pos(4)/a1_pos(4))*diff(get(a1,'YLim'))*[-0.5 0.5]);
end;
return;
function KeyPressCallback(src,eventdata)
H = get(gcf,'UserData');
if ~isempty(H)
f1 = H(1); a1 = H(2); a2 = H(3);
a2_param = get(a2,'UserData');
if (strcmp(get(f1,'CurrentCharacter'),'+') | strcmp(get(f1,'CurrentCharacter'),'='))
a2_param(1) = a2_param(1)*1.2;
elseif (strcmp(get(f1,'CurrentCharacter'),'-') | strcmp(get(f1,'CurrentCharacter'),'_'))
a2_param(1) = a2_param(1)/1.2;
elseif (strcmp(get(f1,'CurrentCharacter'),'<') | strcmp(get(f1,'CurrentCharacter'),','))
a2_param(2) = a2_param(2)/1.2;
elseif (strcmp(get(f1,'CurrentCharacter'),'>') | strcmp(get(f1,'CurrentCharacter'),'.'))
a2_param(2) = a2_param(2)*1.2;
end;
set(a2,'UserData',a2_param);
ButtonMotionCallback(src);
end;
return;
% Included for completeness (usually in own file)
function [fig_pointer_pos, axes_pointer_val] = pointer2d(fig_hndl,axes_hndl)
%
%pointer2d(fig_hndl,axes_hndl)
%
% Returns the coordinates of the pointer (in pixels)
% in the desired figure (fig_hndl) and the coordinates
% in the desired axis (axes coordinates)
%
% Example:
% figure(1),
% hold on,
% for i = 1:1000,
% [figp,axp]=pointer2d;
% plot(axp(1),axp(2),'.','EraseMode','none');
% drawnow;
% end;
% hold off
% Rick Hindman - 4/18/01
if (nargin == 0), fig_hndl = gcf; axes_hndl = gca; end;
if (nargin == 1), axes_hndl = get(fig_hndl,'CurrentAxes'); end;
set(fig_hndl,'Units','pixels');
pointer_pos = get(0,'PointerLocation'); %pixels {0,0} lower left
fig_pos = get(fig_hndl,'Position'); %pixels {l,b,w,h}
fig_pointer_pos = pointer_pos - fig_pos([1,2]);
set(fig_hndl,'CurrentPoint',fig_pointer_pos);
if (isempty(axes_hndl)),
axes_pointer_val = [];
elseif (nargout == 2),
axes_pointer_line = get(axes_hndl,'CurrentPoint');
axes_pointer_val = sum(axes_pointer_line)/2;
end;
Files_4_thesis/magnifyOnFigure.m 0 → 100644
View file @ 6970fd07
This diff is collapsed.
Files_4_thesis/scanner_analysis.m
View file @ 6970fd07
......@@ -62,6 +62,7 @@ end
figure;
set(gcf,'color','white');
subplot(1,2,1);
% plot(angle,s_exp_1,'m--','LineWidth',1.1);
% hold on;
plot(angle,s_exp_2,'m--','LineWidth',1.9);
......@@ -74,8 +75,8 @@ hold on;
% hold on;
plot(angle,dist_to_obst,'rx','LineWidth',.05);
title('');
xlabel('${Angle}$ [rad]','interpreter','latex','FontSize',14);
ylabel('${Distance [m]}$','interpreter','latex','FontSize',14);
xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14);
ylabel('${Distance}$ [m]','interpreter','latex','FontSize',14);
axis([-.09 .42 .08 .2]);
% legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
subplot(1,2,2);
......@@ -140,74 +141,157 @@ for i = 1:points_DockedwithNOmar
Y_obst_DockedwithNOmar(i)= dist_to_obst * sin(s_angle_DockedwithNOmar(i));
end
% The robot is totally docked and a box is besides of laser scanner!
S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1);
s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2);
points_DockedwithBOX = size(indx_DockedwithBOX,1);
laser_beam_DockedwithBOX = points_DockedwithBOX - 1;
s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1);
for i = 1:points_DockedwithBOX
s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180;
X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i));
Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i));
X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i));
Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
end
%
% figure;
% set(gcf,'color','white');
% subplot(2,1,1);
%
%
%
%
% subplot(2,1,2);
% plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
% hold on
% plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14);
% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
% axis([-1.9 0 .08 .22]);
% title('Detected marker (zoomed in area from the first plot)');
%
% figure;
% plot(X_DockedwithBOX,Y_DockedwithBOX);
% checking accuracy in robot coordinate system
figure;
set(gcf,'color','white');
subplot(3,1,1);
subplot(3,2,2);
plot(X_DockedwithNOmar,Y_DockedwithNOmar);
hold on;
plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3);
% xlabel('$ X [m] $','interpreter','latex','FontSize',14);
% ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
title('No marker on docking platform');
legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
title('No marker on docking platform, TOP VIEW');
% legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
% grid on
axis([-2.5 4 -4 1]);
axis([-2.5 6 -4 2]);
subplot(3,1,2);
subplot(3,2,4);
plot(X_DockedwithMar,Y_DockedwithMar,'b');
hold on;
plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
title('Marker circular bottle on docking platform');
% grid on
axis([-2.5 4 -4 1]);
hold off;
subplot(3,1,3);
plot(X_DockedwithMar,Y_DockedwithMar,'b');
hold on;
plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3);
xlabel('$ X [m] $','interpreter','latex','FontSize',14);
% ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
ylabel('$ Y $ [m]','interpreter','latex','FontSize',14);
title('Marker circular bottle on docking platform, TOP VIEW');
% grid on
title('Detected marker (zoomed area from middle plot)');
axis([.04 .16 -.12 .12]);
axis([-2.5 6 -4 2]);
figure;
set(gcf,'color','white');
subplot(3,2,6);
% plot(X_DockedwithMar,Y_DockedwithMar,'b');
% hold on;
% plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3);
% xlabel('${X}$ [m]','interpreter','latex','FontSize',14);
% % ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
% % grid on
% title('Detected marker (zoomed area from middle plot), TOP VIEW');
% axis([.04 .16 -.12 .12]);
%
% figure;
% set(gcf,'color','white');
subplot(3,1,1);
subplot(3,2,1);
plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar);
hold on
plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',.2);
% xlabel('Angle [rad]');
% ylabel('Distance to Obstacle [m]');
title('No marker in docking platform');
%
% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
% set(l2,'interpreter','latex','FontSize',12);
title('No marker on docking platform');
axis([-2.2 2.2 -.5 5]);
subplot(3,1,2);
plot(s_angle_DockedwithMar,s_val_DockedwithMar);
subplot(3,2,3);
orig_coordinate_cir = gca; % create axes
plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar);
hold on
plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2);
% xlabel('Angle [rad]');
ylabel('Distance to objects [m]');
title('Marker circular bottle on docking platform');
axis([-2.2 2.2 -.5 5]);
plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2);
ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
axis([-2.3 2.3 -.5 15]);
% ----- Zoomed in ------
hkids_cir = get(orig_coordinate_cir, 'child'); % get original coordinate properties
set(hkids_cir(1),'marker','o','Color','r')
set(hkids_cir(2),'marker','.')
zoomed_coordinate_cir = axes('Position',get(orig_coordinate_cir,'Position'),...
'XAxisLocation','bottom',...
'YAxisLocation','left',...
'Color','none',...
'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
plot(zoomed_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar);
hold on;
plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'Color','r');
subplot(3,1,3);
plot(s_angle_DockedwithMar,s_val_DockedwithMar);
set(zoomed_coordinate_cir,'xlim',[-.15 .32])% axes limit for zoom out,
set(zoomed_coordinate_cir,'ylim',[.08 .2])% axes limit for zoom out,
set(zoomed_coordinate_cir, 'Units', 'normalized', 'Position', [.34 0.47 0.12 0.14])
% ----- Zoomed in ------
subplot(3,2,5);
orig_coordinate_box = gca; % create axes
plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
hold on
plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',10);
title('Detected marker (zoomed area from middle plot)');
xlabel('Angle [rad]');
% ylabel('Distance to Obstacle [m]');
axis([-.15 .35 .08 .15]);
plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
axis([-2.3 2.3 -.15 6]);
% ----- Zoomed in BOX------
hkids_box = get(orig_coordinate_box, 'child'); % get original coordinate properties
set(hkids_box(1),'marker','o','Color','r')
set(hkids_box(2),'marker','.')
zoomed_coordinate_box = axes('Position',get(orig_coordinate_box,'Position'),...
'XAxisLocation','bottom',...
'YAxisLocation','left',...
'Color','none',...
'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
plot(zoomed_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX);
hold on;
plot(zoomed_coordinate_box,s_angle_DockedwithBOX,dist_to_obst,'Color','r');
set(zoomed_coordinate_box,'xlim',[-1.6 -.3])% axes limit for zoom out,
set(zoomed_coordinate_box,'ylim',[.08 .22])% axes limit for zoom out,
set(zoomed_coordinate_box, 'Units', 'normalized', 'Position', [.32 0.18 0.14 0.14])
% ----- Zoomed in BOX------
%% The robot is moving Towards docking...!
Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0);
......@@ -248,76 +332,30 @@ for i = 1:points_out
Y_obst_out(i)= dist_to_obst * sin(s_angle_out(i));
end
figure;
set(gcf,'color','white');
subplot(1,2,1);
plot(s_angle_out,s_val_out,'b','LineWidth',2);
hold on
plot(s_angle_out,dist_to_obst,'r','LineWidth',10);
xlabel('$Angle [rad]$','interpreter','latex','FontSize',14);
ylabel('$Obstacle [m]$','interpreter','latex','FontSize',14);
grid on;
axis([-2.23 2.23 -1 10]);
subplot(1,2,2)
plot(X_out,Y_out,'b','LineWidth',2);
hold on
plot(X_obst_out,Y_obst_out,'r-','LineWidth',4)
xlabel('$ X [m] $','interpreter','latex','FontSize',14);
ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
set(l2,'interpreter','latex','FontSize',12);
grid on;
axis([-1.2 5 -3.5 1.5]);
%% The robot is totally docked and a box is besides of laser scanner!
S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1);
s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2);
points_DockedwithBOX = size(indx_DockedwithBOX,1);
laser_beam_DockedwithBOX = points_DockedwithBOX - 1;
s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1);
for i = 1:points_DockedwithBOX
s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180;
X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i));
Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i));
X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i));
Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
end
figure;
set(gcf,'color','white');
subplot(2,1,1);
plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
hold on
plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
% xlabel('Angle [rad]');
% ylabel('Distance to Obstacle [m]');
title('Box on docking platform');
legend('Surrounding objects','Laser scanner safety line (r = 10 cm)');
axis([-2.2 2.2 -.5 5]);
subplot(2,1,2);
plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
hold on
plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
% xlabel('Angle [rad]');
% ylabel('Distance to Obstacle [m]');
axis([-1.9 0 .08 .22]);
title('Detected marker (zoomed in area from the first plot)');
% figure;
% set(gcf,'color','white');
%
% subplot(1,2,1);
% plot(s_angle_out,s_val_out,'b','LineWidth',2);
% hold on
% plot(s_angle_out,dist_to_obst,'r','LineWidth',10);
% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14);
% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
% grid on;
% % axis([-2.23 2.23 -1 10]);
%
% subplot(1,2,2)
% plot(X_out,Y_out,'b','LineWidth',2);
% hold on
% plot(X_obst_out,Y_obst_out,'r-','LineWidth',4)
% xlabel('$X$ [m] ','interpreter','latex','FontSize',14);
% ylabel('$Y$ [m]','interpreter','latex','FontSize',14);
%
% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
% set(l2,'interpreter','latex','FontSize',12);
% % legend('boxoff');
% grid on;
% % axis([-1.2 5 -3.5 1.5]);
%
% figure;
% plot(X_DockedwithBOX,Y_DockedwithBOX);
\ No newline at end of file
%
\ No newline at end of file
Files_4_thesis/zoomed_in_plot.m 0 → 100644
View file @ 6970fd07
clear all;close all; clc;
x_1=0:.1:2*pi;
x_2=0:.1:2*pi;
y_1=sin(x_1);
y_2=cos(x_2);
orig_coordinate = gca; % create axes
plot(orig_coordinate,x_1,y_1,x_2,y_2); % plot to original coordinate
hkids = get(orig_coordinate, 'child'); % get original coordinate properties
set(hkids(1),'marker','o','Color','r')
set(hkids(2),'marker','*')
zoomed_coordinate = axes('Position',get(orig_coordinate,'Position'),...
'XAxisLocation','bottom',...
'YAxisLocation','left',...
'Color','none',...
'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
plot(zoomed_coordinate,x_1,y_1, 'marker','*') % plot in zoomed axes
hold on;
plot(zoomed_coordinate,x_2,y_2,'marker','o','Color','r');
set(zoomed_coordinate,'xlim',[pi/2 pi])% axes limit for zoom out,
set(zoomed_coordinate, 'Units', 'normalized', 'Position', [.5 0.65 0.25 0.25])
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment