DataGen_MLPlanner/plotNodeStatus.m at master · wyattraich/DataGen_MLPlanner · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
function [] = plotNodeStatus(nodes,sim,alive_nodes,close_nodes,far_nodes,path_nodes,occ_nodes)

markersize = 15;
plot([nodes(1,alive_nodes)],[nodes(2,alive_nodes)],'bs',...
    'MarkerEdgeColor','b',...
    'MarkerFaceColor','b',...
    'MarkerSize',markersize);
hold on
plot([nodes(1,close_nodes)],[nodes(2,close_nodes)],'cs',...
    'MarkerEdgeColor','c',...
    'MarkerFaceColor','c',...
    'MarkerSize',markersize);
plot([nodes(1,far_nodes)],[nodes(2,far_nodes)],'ms',...
    'MarkerEdgeColor','m',...
    'MarkerFaceColor','m',...
    'MarkerSize',markersize);
plot([nodes(1,path_nodes)],[nodes(2,path_nodes)],'ks',...
    'MarkerEdgeColor','k',...
    'MarkerFaceColor','k',...
    'MarkerSize',markersize);
plot([nodes(1,occ_nodes)],[nodes(2,occ_nodes)],'ws',...
    'MarkerEdgeColor','w',...
    'MarkerFaceColor','w',...
    'MarkerSize',markersize);
plot(sim.start_pos(1),sim.start_pos(2),'cd',...
    'MarkerSize',10,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g');
plot(sim.goal_pos(1),sim.goal_pos(2),'md',...
    'MarkerSize',10,...
    'MarkerEdgeColor','r',...
    'MarkerFaceColor','r');
hold off
daspect([1 1 1])
xlim([0.5 size(sim.map,1)+0.5])
ylim([0.5 size(sim.map,2)+0.5])

% image([nodes(alive_nodes).x],[nodes(alive_nodes).y],[0 0 1]); hold on
% image([nodes(close_nodes).x],[nodes(close_nodes).y],[0 1 0]);
% image([nodes(far_nodes).x],[nodes(far_nodes).y],[1 0 0]);
% image([nodes(path_nodes).x],[nodes(path_nodes).y],[1 1 1]);
% set(gca,'YDir','normal')
% xlim([1 map_x_size])
% ylim([1 map_y_size])

% for ii=1:map_x_size
%     for jj=1:map_y_size
%         v = 0;
%         if ismember(alive_nodes,findNode(ii,jj))
%             v = 1;
%         elseif ismember(close_nodes,findNode(ii,jj))
%             v = 0.5;
%         end
%         imagesc(ii,jj,v); hold on
%     end
% end
%
% set(gca,'YDir','normal')
% xlim([1 map_x_size])
% ylim([1 map_y_size])


% rectangle('Position',[[nodes(alive_nodes).x]-.5 [nodes(alive_nodes).y]-.5 ones(1,length(alive_nodes)) ones(1,length(alive_nodes))],'FaceColor','b'); hold on
% rectangle('Position',[[nodes(close_nodes).x]-.5 [nodes(close_nodes).y]-.5 1 1],'FaceColor','c'); hold on
% rectangle('Position',[[nodes(far_nodes).x]-.5 [nodes(far_nodes).y]-.5 1 1],'FaceColor','m'); hold on

% for ii=1:length(alive_nodes)
%     rectangle('Position',[nodes(alive_nodes(ii)).x-.5 nodes(alive_nodes(ii)).y-.5 1 1],...
%               'FaceColor','b'); hold on
% end
% for ii=1:length(close_nodes)
%     rectangle('Position',[nodes(close_nodes(ii)).x-.5 nodes(close_nodes(ii)).y-.5 1 1],...
%               'FaceColor','c'); hold on
% end
% for ii=1:length(far_nodes)
%     rectangle('Position',[nodes(far_nodes(ii)).x-.5 nodes(far_nodes(ii)).y-.5 1 1],...
%               'FaceColor','y'); hold on
% end
% for ii=1:length(path_nodes)
%     rectangle('Position',[nodes(path_nodes(ii)).x-.5 nodes(path_nodes(ii)).y-.5 1 1],...
%               'FaceColor','k'); hold on
% end
% rectangle('Position',[start_x-.5 start_y-.5 1 1],...
%           'FaceColor','g'); hold on
% rectangle('Position',[start_x-.5 start_y-.5 1 1],...
%           'FaceColor','r'); hold on

end