TreeGenerator/Main.m at main · MOBATSim/TreeGenerator · 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
%% States
VehicleEgo = State(10,0,10);
VehiclesOther = [State(24,0,2) State(20,3.7,8) State(0,0,13)];


%% Horizon
deltaT = 1;
maxDepth = 6;
count = 1;


% All maneuvers
allActions = Maneuver.getAllActions;


RootNodes = []; % fully expanded nodes
UnsafeNodes =[];
% Create the root node
n1 = Node([],[],count,VehicleEgo,allActions, 0);
count = count + 1;

leafNodes = n1;

for k =1:maxDepth

    OtherVehicleStateDistributions =[];
    for VehicleOther = VehiclesOther
        OtherVehicleStateDistribution = Maneuver.moveConstantSpeed(VehicleOther,deltaT*k);
        OtherVehicleStateDistributions = [OtherVehicleStateDistributions OtherVehicleStateDistribution];

    end


    for leafNode = leafNodes

        newleafNodes =[];

        if leafNode.UnsafetyValue < 0.02
            % Only the safe states

            for act = allActions
                % expand
                newLeafNode = leafNode.expand(act,deltaT,count,VehiclesOther,OtherVehicleStateDistributions);


                newleafNodes = [newleafNodes newLeafNode];
                count = count + 1;
            end

        else
            UnsafeNodes = [UnsafeNodes leafNode];
            % Unsafe states
            disp(['SrcID: ',num2str(leafNode.sourceNodeID),'->Act:',leafNode.sourceEdgeName{1}.getName,'-> Node', num2str(leafNode.id),' ', 'Unsafety Value: ',num2str(leafNode.UnsafetyValue)])


        end


        RootNodes = [RootNodes leafNode];
        leafNodes = [leafNodes newleafNodes];

        leafNodes(1) = [];


        % Add all the new leaf nodes to an array to expand in the next depth
    end
end

count = count - 1;

tree = Tree(RootNodes,leafNodes);