I want to orient my robot based on the distance from the obstacle, but I do not know how to measure the distance from robot center to the obstacle surrounding.
1.- GET/BUILD AN ACCURATE MAP
Let's start binarizing the supplied may and dropping a point bot on a random position excluding all obstacle points.
close all;clear all;clc
% get the map
A1=imread('001.jpg'); % this is your map in question
% figure;imshow(A1)
A2=~imbinarize(A1(:,:,1));
figure(1);
ax1=gca;imshow(A2)
[ny,nx]=size(A2);
[X,Y]=meshgrid(1:nx,1:ny);
% obstacles
P1=[140 311; 153 311; 153 7; 140 7; 140 311]; % 140 311 153 7
P2=[374 491; 386 491; 386 186; 374 186; 374 491]; % 374 491 386 186
P3=[1 497; 496 497; 496 492; 1 492; 1 497]; % 1 497 496 492
P4=[1 6; 496 6; 496 1; 1 1; 1 6]; % 6 1 496 1
P5=[1 491; 3 491; 3 7; 1 7; 1 491]; % left frame 1 491 3 7
P6=[492 491; 496 491; 496 7; 492 7; 492 491]; % right frame % 492 491
Pobst=[P1(:);P2(:);P3(:);P4(:);P5(:);P6(:)];
Nobst=6;
Po
bst=reshape(Pobst,[size(P1,1),size(P1,2),Nobst]);
% map points where bot cannot be
[in1,on1]=inpolygon(X(:),Y(:),P1(:,1),P1(:,2));
[in2,on2]=inpolygon(X(:),Y(:),P2(:,1),P2(:,2));
[in3,on3]=inpolygon(X(:),Y(:),P3(:,1),P3(:,2));
Xout=X(~in1 & ~in2 & ~in3);Yout=Y(~in1 & ~in2 & ~in3);
% dropping bot on random location, excluding obstacle points
nP2=randi([1 numel(Xout)],1,1);
Pnet=[Xout(nP2) Yout(nP2)];
% Pnet=[47 365]; % static test point
hold(ax1,'on')
plot(ax1,Pnet(1),Pnet(2),'or','LineStyle','none');
2.- 360º SWEEP
Now let's do a 360º ray sweep.
NOT using angles BUT directly covering all outer perimeter points, from wherever the bot is located.
Ns=2*size(A2,1)+2*size(A2,2)-4; % amount outer perimeter points
% outer perimeter defined starting top left corner and then CW
P0=[ones(1,size(A2,1)) 2:1:size(A2,2)-1 size(A2,2)*ones(1,size(A2,1)) size(A2,2)-1:-1:2; % x
1:1:size(A2,1) size(A2,1)*ones(1,size(A2,2)-2) size(A2,1):-1:1 ones(1,size(A2,2)-2)]'; % y
PR=[0 0];
for k1=1:10:Ns
L2=floor(linspace(Pnet(2),P0(k1,1),max([Pnet(1) Pnet(2) abs(P0(k1,2)-Pnet(2)) abs(P0(k1,1)-Pnet(1)) ]) )); % x
L1=floor(linspace(Pnet(1),P0(k1,2),max([Pnet(1) Pnet(2) abs(P0(k1,2)-Pnet(2)) abs(P0(k1,1)-Pnet(1)) ]) )); % y
p1=1;
while A2(L2(p1),L1(p1))==0
p1=p1+1;
end
PR=[PR;L1(p1) L2(p1)]; % logging perimeter point [x y]
end
PR(1,:)=[];
Variable PR
contains the points you are asking for, for a particular bot position Pnet
Quick check all rays and nearest obstacle points are ok
for k2=1:10:Ns
plot(ax1,PR(k2,1),PR(k2,2),'r*')
plot(ax1,[Pnet(1) PR(k2,1)],[Pnet(2) PR(k2,2)],'g-')
end
3.- Comment on Obtained Perimeter
At this point, for a particular bot position Pnet
the variable perimeter PR
contains all the closest obstacle points.
As here done, the perimeter obstacle points in PR
are the 1st point of/belonging to the hard obstacle that the boot should avoid.
Therefore the bot should NOT consider this point but the 1st one available, thius is, 1 pixel closer to the bot, along each ray, that is NOT part of any obstacle.
4.- Do not use the bot centre point to know when/where/how to avoid obstacles
When working out obstacle avoidance the centre of the bot is hardly ever of any use. Yes you can use it for some calculations but the chances that just considering the bot a a point your bot is going to hit things.
My suggestion is to :
4.1.- find the furthermost point of the robot, not the centre of the robot
4.2.- assume the bot as a ball with radius that furthest point
4.3.- and then include an additional safety area around the ball, not the point that is your bot.
Yes again, you may actually want the bot to hit a surface, but if you do not control when your bot is not hitting things there's no way you are going to be able to know where/when/how to have it hitting any surface of choice ay time soon.