%% Automatic Scenario Variant Generation for Testing AEB Systems
% This example shows how to automatically generate variants of a seed scenario in which two actors collide. You can generate random variants of a collision scenario that you can use for the design and validation of autonomous driving applications such as automated emergency braking (AEB) systems. In this example, you generate the scenario variants by modifying the speed of the ego actor, the collision, and the arrival times of the actors at their waypoints. However, the collision point remains the same as in the seed scenario.
% If the ego vehicle collides with a target actor at multiple times, this example generates scenario variants based on the first collision instance.
% If the ego vehicle collides with multiple target actors at different times in a scenario, this example generates scenario variants with respect to only one target actor. You can specify the desired target actor to consider for generating the scenario variants.
% This example uses the drivingScenario object to create a seed scenario, and provides helper functions to generate the variants from the seed scenario. The rest of the example demonstrates these steps involved in generating the scenario variants.
% Generate Seed Scenario - Create a collision scenario by using a drivingScenario object. The scenario can contain any number of actors and collision instances. However, this example generates a variant based on only the first collision instance that occurs between the ego vehicle and the specified target actor.
% Generate Variant of Seed Scenario - Modify the seed scenario by using the helper functions helperComputeCollisionTimeInstant, helperComputeCollisionTimeDiff, and helperModifyScenario. These helper functions enable you to generate variants of the seed scenario by altering the speed of the ego vehicle and adjusting the wait time and entry time of the ego and target actor and the starting position of the target actor. These are the steps involved:
%  Find collision time instance - Find the time instant at which collision occurs between the ego vehicle and a specific target  in the seed scenario by using the helper function helperComputeCollisionTimeInstant.
% Specify new ego speed and calculate collision time difference - Specify the new ego speed value and the parameter to alter for generating the variant. Use the helperComputeCollisionTimeDiff function to compute the parameters required for altering the arrival times of the ego and the target actor at each waypoint along their trajectories. The helperComputeCollisionTimeDiff function uses the computed collision time to compute the collision time for the new ego speed value.
% Modify seed scenario - Use the helperModifyScenario function to generate a variant of the seed scenario. The helperModifyScenario function uses the parameters computed by the helperComputeCollisionTimeDiff function to alter the arrival times of the ego and the target actor at each waypoint.
% Visualize Generated Scenario - Simulate and display the generated scenario variants by using the plot function.
%% Generate Seed Scenario
% The seed scenario must be a collision scenario in which collision occurs between the ego vehicle and at least one of the target actors in the scenario. In this example, you generate the Car-to-Pedestrian Nearside Child test scenario of the European new car assessment programme (Euro NCAP) test protocol as the seed scenario. The actor dimensions, positions, speed values, and trajectories are set as per the Euro NCAP test protocol requirements.
% The scenario has an ego vehicle and three actors. Of the three actors, one is a pedestrian, which is the moving target with which the ego vehicle collides. The collision occurs only once during the simulation time. The ego vehicle and the non-ego actors must travel at constant speeds. Hence, the ego vehicle and the target actor must each have a scalar speed value.
% Create a driving scenario object.

scenario = drivingScenario;

%  Specify the width and the road centers to create a road by using the road function. Add two driving lanes and a shoulder lane to the road by using the lanespec function.

roadCenters = [0 0 0; 70 0 0];
marking = [laneMarking("Solid",Color=[0.98 0.86 0.36]), ...
    laneMarking("Solid"),laneMarking("Solid")];
ltype = [laneType("Driving");laneType("Shoulder")];
lspec = lanespec(2,Width=[5.4 2.6],Marking=marking,Type=ltype);
road(scenario,roadCenters,Name="Road",Lanes=lspec);

% Add an ego vehicle to the scenario and set its trajectory by specifying the waypoints and speed. Set the name for the ego vehicle as "Ego in Scenario" or "Vehicle under Test". This example uses the Name field to identify the ego vehicle in a given scenario. Hence, the name for the ego vehicle must contain the word "Ego" or "Test". Set the speed of the ego vehicle to 60 km/hr.

egoVehicle = vehicle(scenario,ClassID=1,Position=[3.3 0 0], ...
    Wheelbase=2.8,Mesh=driving.scenario.carMesh, ...
    Name="Ego in Scenario");
egoWaypoints = [3.3 0 0; 8.3 0 0; 51.3 0 0; 65 0 0];
egoSpeed = 60;
% Convert speed value to m/s
egoSpeed = (egoSpeed*1000)/3600;
trajectory(egoVehicle,egoWaypoints,egoSpeed);

% Add an actor of class Pedestrian to the scenario and set its trajectory by specifying the waypoints and the speed. Specify waypoints and the speed such that it collides with the ego vehicle. Set the name for the target vehicle as "Target Pedestrian". This example uses the Name field to identify the moving target in a given scenario. Hence, the name for the target actor must contain the word "Target". Set the speed of the target actor to 5 km/hr.

movingTarget = actor(scenario,ClassID=4, ...
    Length=0.711, Width=0.5, Height=1.154, ...
    Position=[55.25 -4 0],RCSPattern=[-8 -8;-8 -8], ...
    Mesh=driving.scenario.pedestrianMesh,Name ="Target Pedestrian");
targetWaypoints = [55.25 -4 0; 55.25 -2 0; 55.25 0 0; 55.25 4 0];
targetSpeed = 5;
% Convert speed value to m/s
targetSpeed = (targetSpeed*1000)/3600; 
trajectory(movingTarget,targetWaypoints,targetSpeed);

% Add two vehicles to the shoulder lane of the scenario and set them as static actors.

vehicle(scenario,ClassID=1,Length=4.25, ...
    Position=[50.75 -2.8125 0],FrontOverhang=0.45, ...
    Mesh=driving.scenario.carMesh,Name="Small Obstruction Vehicle");

vehicle(scenario,ClassID=1,Length=4.5,Width=1.825, ...
    Height=1.65,Position=[45.25 -2.8125 0],FrontOverhang=0.7, ...
    Mesh=driving.scenario.carMesh,Name="Large Obstruction Vehicle");

% Save the seed scenario to a .mat fie. This enables you to use it to generate scenario variants by using all three methods.

save("seedScenario.mat","scenario",'-mat');

% Display the seed scenario.

figScene = figure;
set(figScene,Position=[50,50,500,500]);
hPanel1 = uipanel(figScene,Position=[0 0 1 1]);
hPlot1 = axes(hPanel1);
plot(scenario,Parent=hPlot1);
title("Seed Scenario")
while advance(scenario)
    pause(0.01);
end

%
%% Generate Variants of Seed Scenario
% For ego vehicle speeds, you can generate variants of the seed scenario by following these steps:
% Find the time instant at which collision occurs between two actors in the seed scenario.
% Specify a new speed value for the ego vehicle.
% Select the parameter to alter to generate the variant.
% Compute the modifications to the arrival times of the ego vehicle and the target actor at each waypoint along their trajectories.
%% Find Collision Time Instance
% Use the helperComputeCollisionTimeInstant function to compute the first collision time instance between the ego vehicle and a target actor in a scenario. The function identifies the ego vehicle and the target actor by using the ActorIDs or the actor names specified while creating the seed scenario. The ActorID values and the names for each vehicle in the scenario are stored in the Actors field of the scenario object returned by the drivingScenario function.

table([scenario.Actors.ActorID],[scenario.Actors.Name],VariableNames={'ActorID','Name'})

% If any of the Name or ActorID for the ego and target actors are known, you can call the helperComputeCollisionTimeInstant function by using any of these syntaxes:
% time = helperComputeCollisionTimeInstant(scenario,EgoID=value,TargetID=value) - Use this syntax if you know the ActorID values for the ego vehicle and the target actor. The function computes the first collision time instance between the specified ego vehicle and target actor.
% [time,egoID,targetID] = helperComputeCollisionTimeInstant(scenario) - Use this syntax if you do not know the ActorID values for the ego vehicle and the target actor. In this case, the helperComputeCollisionTimeInstant function checks for actors with names that contain the words "Ego" and "Target" and selects them as the ego vehicle and the target actor, respectively. The function computes the first collision time instance between the selected actors, and also returns their ActorID values.
% [time,~,targetID] = helperComputeCollisionTimeInstant(scenario,EgoID=value) - Use this syntax if you do not know the ActorID for the target actor. In this case, the helperComputeCollisionTimeInstant function checks for actor with the name that contains the word "Target" and selects it as the target actor. The function computes the first collision time instance between the ego vehicle and the selected actor. The function also returns the ActorID of the selected target.
% [time,egoID,~] = helperComputeCollisionTimeInstant(scenario,TargetID=value) - Use this syntax if you do not know the ActorID for the ego vehicle. In this case, the helperComputeCollisionTimeInstant function checks for actor with the name that contains the word "Ego" and selects it as the ego vehicle. The function computes the first collision time instance between the selected ego and the target actor.  The function also returns the ActorID of the selected ego vehicle.
% If both the Name and ActorID for the ego and target actors are not known, call the helperComputeCollisionTimeInstant function by using this syntax:
% [time,egoID,targetID] = helperComputeCollisionTimeInstant(scenario) - Use this syntax if you do not know the ActorID values and the names for the ego vehicle and the target actor. In this case, the helperComputeCollisionTimeInstant function selects the vehicle with ActorID value 1 as the ego vehicle and ActorID 2 as the target actor.
% Specify the ActorID of the ego vehicle and the target actor with which it collides in the seed scenario and compute the first collision time instance.

egoID = 1;
targetID = 2;
collisionTime = helperComputeCollisionTimeInstant(scenario,egoID,targetID);

% Store the speed, waypoints, and ActorID data of the ego vehicle and the target actor as structures.

egoData = struct("speed",egoSpeed,"waypoints",egoWaypoints,"id",egoID);
targetData = struct("speed",targetSpeed,"waypoints",targetWaypoints,"id",targetID);

% If the yaw information for the ego vehicle and the target actor is known, you must store the yaw information in the structure along with the other variables in a field named "yaw". For example, if the yaw value for the ego vehicle is 45 degrees, then use this syntax:
% egoData = struct("speed",egoSpeed,"waypoints",egoWaypoints,"id",egoID,"yaw",45).
%% Specify New Ego Speed and Calculate Collision Time Difference
% Specify the new speed value for the ego vehicle as 15 km/hr.

egoNewSpeed = 15; 
% Convert the speed value to m/s
egoNewSpeed = (egoNewSpeed*1000)/3600;

% Use the helperComputeCollisionTimeDiff helper function to compute the modifications to the arrival times of the ego vehicle and the target actor at each waypoint along their trajectories. You can compute the modifications by altering one of these three parameters: WaitTime, EntryTime, or StartingPosition. The function estimates the new collision time for the new ego speed value based on the parameter you select. The helperComputeCollisionTimeDiff function outputs a structure with these fields:
% egoCollisionTimeDiff - Time difference between the actual collision time and the collision time estimated with respect to the ego vehicle and new ego speed.
% targetCollisionTimeDiff - Time difference between the collision times of the target actor and the ego vehicle estimated with respect to the new ego speed.
% waypoints - New waypoints generated for the target actor. The function returns this value only if the starting position of the target actor is modified.
% Alter "WaitTime" to generate scenario variant
% If you select "WaitTime" to generate the scenario variant, either the ego vehicle or target actor must wait at its first waypoint for a particular amount of time. If the new ego speed value causes the ego vehicle to arrive the collision point ahead of the target actor, then the function makes the ego vehicle wait at its first waypoint. The function accomplishes this by setting the speed of the ego vehicle at its first waypoint to zero for the wait time. Similarly, if the target actor arrives the collision point ahead of the ego vehicle, the function makes the target actor wait at its first waypoint. The helperComputeCollisionTimeDiff function returns values for egoCollisionTimeDiff and targetCollisionTimeDiff based on this waiting period.
% Specify the modification method  as "WaitTime" and compute the collision time difference.

method = "WaitTime";
wt_parameters = helperComputeCollisionTimeDiff(scenario,egoNewSpeed,collisionTime,egoData,targetData,method);

% Inspect the output of helperComputeCollisionTimeDiff function. The nonzero output value for egoCollisionTimeDiff indicates that the ego vehicle reaches the collision point ahead of the target actor, so the function makes it wait at its first waypoint. If the value for the targetCollisionTimeDiff field is nonzero, instead, then the target actor reaches the collision point ahead of the ego vehicle, and the function makes it wait at its first waypoint.

wt_parameters

% Alter "EntryTime" to generate scenario variant
% If you select "EntryTime" to generate the scenario variant, the function modifies the entry time of either ego vehicle or target actor.
% The entry time of the ego vehicle is modified if the ego vehicle is estimated to reach the collision point ahead of the target actor.
% The entry time of the target actor is modified if the target actor is estimated to reach the collision point ahead of the ego vehicle.
% Specify the modification method as "EntryTime" and compute the collision time difference.

method = "EntryTime";
et_parameters = helperComputeCollisionTimeDiff(scenario,egoNewSpeed,collisionTime,egoData,targetData,method);

% Inspect the output of helperComputeCollisionTimeDiff function. The nonzero output value for egoCollisionTimeDiff indicates that the ego vehicle reaches the collision point ahead of the target actor, so the function modifies the entry time of the ego vehicle. If the value for the targetCollisionTimeDiff field is nonzero, instead, then the target actor reaches the collision point ahead of the ego vehicle, and the function modifies the entry time of the target actor.

et_parameters

% Alter "StartingPosition" to generate scenario variant
% If you select "StartingPosition" to generate the scenario variant, the function modifies the starting position of the target actor. To use this method, the new ego speed value must be greater than the ego speed in the seed scenario.
% Specify the ego speed value as 70 km/hr. Specify the modification method as "StartingPosition" and compute the collision time difference. Based on the time difference, the function alters the starting position of the target actor.

egoSpeedSP = 70;
% Convert value to m/s
egoSpeedSP = (egoSpeedSP*1000)/3600;
method = "StartingPosition";
sp_parameters = helperComputeCollisionTimeDiff(scenario,egoSpeedSP,collisionTime,egoData,targetData,method)

% Inspect the new target waypoints returned by the helperComputeCollisionTimeDiff function.

sp_parameters.waypoints

%
%% Modify Seed Scenario
% Generate the variant of the seed scenario by using the "WaitTime" method.

load seedScenario
method = "WaitTime";
wtScenario = helperModifyScenario(scenario,egoNewSpeed,egoData,targetData,wt_parameters,method);

%% Display Generated Scenario Variant
% Notice that the ego vehicle waits at the first waypoint before accelerating to the new ego speed value.

figScene = figure;
set(figScene,Position=[50,50,500,500]);
hPanel1 = uipanel(figScene,Position=[0 0 1 1]);
hPlot1 = axes(hPanel1);
plot(wtScenario,Parent=hPlot1);
title({'Scenario Variant - WaitTime method';['Ego speed = ',num2str(egoNewSpeed*3.6),' km/hr']})
while advance(wtScenario)
    pause(0.01);
end

%
% Next generate the variant of the seed scenario by using the "EntryTime" method. Notice that the target pedestrian enters the scenario at a different time. The entry time for the target pedestrian is stored in the scenario object.

load seedScenario
method = "EntryTime";
etScenario = helperModifyScenario(scenario,egoNewSpeed,egoData,targetData,et_parameters,method);
etScenario.Actors(targetID).EntryTime

% Visualize the generated scenario variant.

restart(etScenario)
figScene = figure;
set(figScene,Position=[50,50,500,500]);
hPanel2 = uipanel(figScene,Position=[0 0 1 1]);
hPlot2 = axes(hPanel2);
plot(etScenario,Parent=hPlot2);
title({'Scenario Variant - EntryTime method';['Ego speed = ',num2str(egoNewSpeed*3.6),' km/hr']})
while advance(etScenario)
    pause(0.01);
end

%
% Lastly, generate a variant of the seed scenario by using the "StartingPosition" method.

load seedScenario
method = "StartingPosition";
spScenario = helperModifyScenario(scenario,egoSpeedSP,egoData,targetData,sp_parameters,method);

%
% Visualize the generated scenario.

restart(spScenario)
figScene = figure;
set(figScene,Position=[50,50,500,500]);
hPanel3 = uipanel(figScene,Position=[0 0 1 1]);
hPlot3 = axes(hPanel3);
plot(spScenario,Parent=hPlot3);
title({'Scenario Variant - StartingPosition method';['Ego speed = ',num2str(egoSpeedSP*3.6),' km/hr']})
while advance(spScenario)
    pause(0.01);
end

%
%% Tips
% You can also generate a seed scenario by using the Driving Scenario Designer app. You can save the scenario to a .mat file and load the scenario data to generate the variants.
% When generating scenario variants for testing AEB systems, best practice is for the new ego speed value to be less than the ego speed value in the seed scenario. In this case, you can use only the "WaitTime" and "EntryTime" methods for generating variants.
%% Limitations
% The results are not accurate if the ego vehicle and the target actor collide at a road junction.
% In the "WaitTime" method, if both the ego vehicle and target actor are assigned a wait time, you cannot use the generated scenario variant for closed loop simulations.
% Copyright 2021 The MathWorks, Inc.