selnodes.m

航迹规划中的AStar算法

%
%      Manually Select nodes
%
%      Once executed, first select start point
%       Next select destinations, and then obstacles
%
function [s,d,o]=selnodes(nooc,noor,R,GTS,o)
global plot_nodenum;

%%
%%

noOfNodes  = nooc*noor;
nodmat=ones(nooc,noor,3);
nodmat(1:noOfNodes)=1:noOfNodes;
nodmat(:,:,2)=(find(nodmat(:,2,2)==1))*ones(1,noor);
nodmat(:,:,3)=ones(nooc,1)*(find(nodmat(2,:,3)==1));
nodmat(:,:,1)=flipdim((nodmat(:,:,1)),2);
nodmat(:,:,2)=flipdim((nodmat(:,:,2)),2);
nodmat(:,:,3)=flipdim((nodmat(:,:,3)),2);
noddata=reshape(nodmat,noOfNodes,3);
scrsz = get(0,'ScreenSize');
 h=figure(gcf);
set(h,'Position',[scrsz(3)/8 scrsz(4)/8 scrsz(3)-2*scrsz(3)/8 ...
    scrsz(4)-2*scrsz(4)/8]);
    clf;
    hold on;
    
    
Astar_coor=noddata(:,2:3)*GTS;

% Astar_coor=[100 100; 100 200; 100 300; 100 400; 
%              200 100; 200 200; 200 300; 200 400;
%              300 100; 300 200; 300 300; 300 400;
%              400 100; 400 200; 400 300; 400 400;];

netXloc = Astar_coor(:,1)';
netYloc = Astar_coor(:,2)';
axis([min(netXloc)-20  max(netXloc)+30  min(netYloc)-20  max(netYloc)+30])
Astar_connect = zeros(noOfNodes, noOfNodes);
Astar_coord = zeros(noOfNodes, 2);
for i = 1:noOfNodes

    for j = 1:noOfNodes
        distance = sqrt((netXloc(i) - netXloc(j))^2 + (netYloc(i) - netYloc(j))^2);
        ll=isempty(find(o==i, 1));
        lm=isempty(find(o==j, 1));
        if (distance <= R && ll==1 && lm==1)
            matrix(i, j) = distance;   % if set to '1', Dijkstra computes Spath in terms of hops; if set to 'distance', it is the real shortest path
            if i~=j % must be satisfied
                Astar_connect(i, j) = 1;
            else
                Astar_connect(i, j) = 0;
            end
                line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], 'color',[.7 .7 .7],'LineStyle', ':');
        else
            matrix(i, j) = inf;
            Astar_connect(i, j) = 0;
        end;
    end;
end
for i = 1:noOfNodes
     
%         if i==s
%             plot(netXloc(i), netYloc(i),'square','MarkerSize',12,'MarkerFaceColor','g');
%             hold on;
% 
%         end
%         if i==d
%              plot(netXloc(i), netYloc(i),'square','MarkerSize',12,'MarkerFaceColor','r');   
%          end
%             plot(netXloc(i), netYloc(i),'.');      

     
    Astar_coord(i,1) = netXloc(i);
    Astar_coord(i,2) = netYloc(i);
        text(netXloc(i)+4, netYloc(i), num2str(i));
        if (isempty(find(o==i)))
        plot(netXloc(i), netYloc(i),'diamond','MarkerSize',8,'MarkerFaceColor','b');
            hold on;
        end

end;

    
title('Select Start node, end node and Osbtacle nodes and then press ''ENTER'' on Keyboard when done')

dcm_obj = datacursormode(figure(gcf));
set(dcm_obj,'DisplayStyle','window',...
'SnapToDataVertex','on','Enable','on');

 w=0;
figure(gcf)
    w = waitforbuttonpress;
    c_info = getCursorInfo(dcm_obj);
    s=find(Astar_coord(:,1)==c_info.Position(1)...
        & Astar_coord(:,2)==c_info.Position(2));
    set(c_info.Target,'Marker','square','MarkerSize',16,'MarkerFaceColor','g');
    
 w=0;
figure(gcf)
    w = waitforbuttonpress;
    c_info = getCursorInfo(dcm_obj);
    d=find(Astar_coord(:,1)==c_info.Position(1)...
        & Astar_coord(:,2)==c_info.Position(2));
    set(c_info.Target,'Marker','square','MarkerSize',16,'MarkerFaceColor','r');

 w=0;
while w==0
    figure(gcf)
    w = waitforbuttonpress;
    if w==1
        break
    end
    c_info = getCursorInfo(dcm_obj);
    ot=find(Astar_coord(:,1)==c_info.Position(1)...
        & Astar_coord(:,2)==c_info.Position(2));
    o=cat(2,o,ot);

    set(c_info.Target,'MarkerFaceColor','r','MarkerSize',16);
end
%%
%%
%%
%close figure(gcf)