Traffic_platoon/platoonManager.m at main · sGraffione/Traffic_platoon · 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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
function [x0,WMR_order,HDV_indexes] = platoonManager(Round,WMRs,HDVs,numWMR,x0_prev,WMR_order_prev,platoonsIdx_prev)
    % Calcolo distanze e priorità
    % 1 - prendere distanze dai Joint della rotonda
    jts = Round.roundJoint;
    xx = [];
    dist = [];
    vehicles = cat(1,[WMRs(:)',HDVs(:)']);
    numWMR = length(vehicles);
    for i = 1:length(vehicles)
        if any(vehicles{i}.direction == [0 1]) % considero solo i mezzi che sono in rotonda o si stanno avvicinando
            currArc = vehicles{i}.currArc;
            wmr_path = vehicles{i}.path;
            jtwmr = [];
            for j = currArc:length(wmr_path)
                jtwmr(j,:) = wmr_path{j}.Joints(end,:);
            end
            [~,idx{i}] = intersect(jts,jtwmr,'rows');
        end
    end
    commonJt = idx{1};
    for i = 2:length(idx)
        commonJt = intersect(commonJt,idx{i});
    end
    % 3 - calcolo la distanza di ogni WMR ai joint comuni
    for i = 1:length(commonJt)
        for j = 1:length(vehicles)
            wmr_path = vehicles{j}.path;
            currArc = vehicles{j}.currArc;
            for k = 1:length(wmr_path)
                if wmr_path{k}.Joints(end,:) == Round.roundJoint(commonJt(i),:)
                    dist(i,j) = wmr_path{currArc}.arclength-vehicles{j}.sOnArc;
                    for kk = currArc+1:k
                        dist(i,j) = dist(i,j) + wmr_path{kk}.arclength;
                    end
                end
            end
        end
    end

    if ~isempty(dist)
        for i = 1:numWMR
            speed(i) = vehicles{i}.speed;
        end
        [~,I] = min(min(dist,[],2));
        [ordDist,WMR_order{1}] = sort(dist(I,:));
        HDV_indexes = (WMR_order{1} > length(WMRs)); % controllo chi è HDV
        % se HDV è il primo, lo escludo
        if HDV_indexes(1)
            virtPlatPos = -(ordDist(2:end)-ordDist(2));
            x0{1} = [virtPlatPos' speed(2:end)']';
            WMR_order{1}(1) = [];
            HDV_indexes(1) = [];
        else
            virtPlatPos = -(ordDist-ordDist(1));
            if ~isempty(x0_prev)
                virtPlatPos(1,:) = virtPlatPos(1,:) + x0_prev(1,1);
            end
            x0{1} = [virtPlatPos' speed(WMR_order{1})']';
        end


    else
        x0{1} = x0_prev;
        WMR_order{1} = WMR_order_prev;
        HDV_indexes = platoonsIdx_prev;
    end


%     leav_arcs = Round.leav_arcs;
%     leaving_WMRs = {};
%     inRound_WMRs = {};
%     approaching_WMRs = {};
%     toa = 1000*ones(length(jts),length(WMRs));
%
%     for i = 1:length(WMRs)
%         if WMRs{i}.direction == 1
%             approaching_WMRs{end+1} = WMRs{i};
%         else
%             if WMRs{i}.direction == 0
%                 inRound_WMRs{end+1} = WMRs{i};
%             else
%                 leaving_WMRs{end+1} = WMRs{i};
%             end
%         end
%     end
%     % calcolo i joint comuni tra i wmr che si avvicinano o sono già in
%     % rotonda
%     for i = 1:length(WMRs)
%         if any(WMRs{i}.direction == [0 1]) % considero solo i mezzi che sono in rotonda o si stanno avvicinando
%             currArc = WMRs{i}.currArc;
%             wmr_path = WMRs{i}.path;
%             jtwmr = [];
%             for j = currArc:length(wmr_path)
%                 jtwmr(j,:) = wmr_path{j}.Joints(end,:);
%             end
%             jtwmr(end-1:end,:) = []; % rimuovo ultimi due joint
%             [~,idx{i}] = intersect(jts,jtwmr,'rows');
%         end
%     end
%
%     x0 = {};
%     WMR_order = {};
%     for j = 1:length(idx)
%         for i = 1:length(idx)
%             commonJt{j,i} = intersect(idx{j},idx{i});
%
%             wmr_path = WMRs{j}.path;
%             currArc = WMRs{j}.currArc;
%             for ii = 1:length(commonJt{j,i})
%                 for k = 1:length(wmr_path)
%                     if wmr_path{k}.Joints(end,:) == jts(commonJt{j,i}(ii),:)
%                         toa(commonJt{j,i}(ii),j) = wmr_path{currArc}.arclength-WMRs{j}.sOnArc;
%                         for kk = currArc+1:k
%                             toa(commonJt{j,i}(ii),j) = toa(commonJt{j,i}(ii),j) + wmr_path{kk}.arclength;
%                         end
%                     end
%                 end
%             end
%         end
%     end
%     [m,I]=min(toa,[],1);
%
%     if isempty(commonJt) || length(idx) == 1
%         x0{end+1} = [0; WMRs{j}.speed];
%         WMR_order{end+1} = 1;
%     else
%         dist = [];
%         for i = 1:length(commonJt)
%             for j = 1:length(WMRs)
%                 wmr_path = WMRs{j}.path;
%                 currArc = WMRs{j}.currArc;
%                 for k = 1:length(wmr_path)
%                     if wmr_path{k}.Joints(end,:) == Round.roundJoint(commonJt{i},:)
%                         dist(i,j) = wmr_path{currArc}.arclength-WMRs{j}.sOnArc;
%                         for kk = currArc+1:k
%                             dist(i,j) = dist(i,j) + wmr_path{kk}.arclength;
%                         end
%                     end
%                 end
%             end
%         end
%
%         if ~isempty(dist)
%             for i = 1:numWMR
%                 speed(i) = WMRs{i}.speed;
%             end
%             [~,I] = min(min(dist,[],2));
%             [ordDist,WMR_order] = sort(dist(I,:));
%             virtPlatPos = -(ordDist-ordDist(1));
%             x0 = [virtPlatPos' speed']';
%         else
%             x0 = x0_prev;
%         end
%     end
end