%% Automatic Scenario Generation
% This example shows how to automate scenario generation by using drivingScenario object. In this example, you will automate
% vehicle placements in a scenario by defining their start and the goal positions
% waypoint selection and trajectory generation for the vehicles to traverse from their start positions to goal positions.
% speed adjustment such that the vehicles accelerate or deaccelerate to avoid colliding between other vehicles that travel in the same lane.
% You can use this example to synthesize a number of random scenarios for testing the driving algorithms.
%% Introduction
% The drivingScenario object and the DrivingScenarioDesigner app in the Automated Driving Toolbox™ are efficient tools for generating synthetic driving scenarios. You can create a road network or import a road network from OpenDRIVE®, HERE HD Live Map, and OpenStreetMap®. Then, you can add actors or vehicles to the road network and define their trajectories to synthesize a driving scenario. The waypoints required for generating the trajectories must be selected carefully such that the trajectories of the vehicles lie within the road network and the vehicles does not collide as they travel along their trajectories. Defining such vehicle placements and trajectories often requires multiple trials and is time consuming if you have large road networks and many number of vehicles to configure.
% This example provides helper functions and demonstrates the steps that you can use to automate vehicle placements and trajectory generation by using the drivingScenario object. You can also export the generated scenario to DrivingScenarioDesigner app. The rest of the example demonstrates these steps involved in automating scenario generation.
% Import road network - Import OpenStreetMap® road network into a driving scenario object by using the helper function helperOSMimport.
% Define start and goal positions - Specify region of interests (ROIs) in the road network to select the start and goal positions for vehicles by using the helper function helperSamplePositions.
% Generate vehicle trajectories - Generate waypoints and trajectories by using the helper function helperGenerateWaypoints and Automated Driving Toolbox™ function trajectory.
% Modify speed profiles to avoid collision - Modify the speed profiles of the vehicles in the scenario by using the Simulink model CollisionFreeSpeedManipulator. The model checks the speed profile of each vehicle and prevents them from colliding with each other as they travel along their trajectories. The output from the model is an updated scenario that is free from collision between vehicles. You can convert the output from the CollisionFreeSpeedManipulator Simulink model to a driving scenario object by using the helper function helpergetCFSMScenario.
% Simulate and visualize generated scenario - Simulate and display the automatically generated scenario by using the plot function. You can also read and simulate the automatically generated scenario by using the DrivingScenarioDesigner app.
%% Import Road Network
% You can download a road network from https://www.openstreetmap.org, which provides access to crowd-sourced map data all over the world. The data is licensed under the Open Data Commons Open Database License (ODbL), https://opendatacommons.org/licenses/odbl/.
% Specify the bounding box coordinates to import the MCity test facility map from openstreetmap.org by using the helperOSMImport function. The function returns a driving scenario object that contains the road network from the imported map data. You can also use the roadNetwork function to import a road network from OpenDRIVE®, HERE HD Live Map, or OpenStreetMap® files.

% Import the road network of MCity
minLat =  42.2990;
maxLat =  42.3027;
minLon = -83.6996;
maxLon = -83.6965;
bbox = [minLat maxLat;minLon maxLon];
scenario = helperOSMImport(bbox);
% Create another scenario object for plotting purpose
scenario_in = helperOSMImport(bbox);

% Display the MCity road network by using the plot function.

figure
plot(scenario_in)
title('Imported Road Network')
xlim([-50 190])
ylim([-85 330])

%
%% Define Start and Goal positions
% To create a driving scenario, you must first define specific points on the road network that can serve as start and goal positions for the vehicles in the scenario. Use the helperSamplePositions function to generate a random set of points in the road network that you can select as start and goal positions for the vehicles. You can use one or more name-value pair arguments of helperSamplePositions functions to configure the start and goal positions in different ways.
% Use the 'Seed' name-value pair argument of helperSamplePositions function to specify the random generator settings to be used for generating random points. You can select any points in the generated set as start and goal positions.
% Use the 'ROI' name-value pair argument of helperSamplePositions function to specify one or more region-of-interests (ROIs) in the road network within which you want to define the start and goal positions. The ROIs can be circular, rectangular or a polygon region with any number of vertices. The value for ROI is an N-by-2 matrix specifying the spatial coordinates of a closed region. If not specified, the function generates random points across the entire road network.
% Use the 'Lanes' name-value pair argument of helperSamplePositions function to specify the lanes in which you want to define the start and the goal positions. To select a single lane, specify the lane number as a scalar value. In case of multiple lanes, the value for 'Lanes' name-value argument must be a vector containing the desired lane numbers. If not specified, the function selects the lanes randomly.
% Use the 'LongitudinalDistance' name-value pair argument of helperSamplePositions function to set the longitudinal distance between two consecutive points. If not specified, the function imposes at least 5 m distance between two consecutive points in the same lane. This implies that the longitudinal distance between two consecutive vehicles placed in the same lane is at least 5 m.
% During simulation, the vehicles spawn at the start points and then, travels to reach the goal points.
%% 1. Select Start Positions
% Specify the number of random points to be generated for selecting the start positions as 10. Specify a flag for setting the random number generator. Set the value for setSeed to 1 to specify the seed for random number generator. Pass the random generator settings as input to the helperSamplePositions function by using the 'Seed' name-value pair argument.
% The helperSamplePositions function outputs the 3-D spatial coordinates of the randomly selected points in the imported road network. The function also outputs the yaw angles relative to the selected points. The yaw angle obtained relative to a point defines the orientation for the vehicle to be placed at that point.

numPoints = 10;
setSeed = 1;
if setSeed == 1
    seed = 2;
    rng(seed);
    s = rng;
    [points,yaw] = helperSamplePositions(scenario,numPoints,'Seed',s);
else      
    [points,yaw] = helperSamplePositions(scenario,numPoints);
end

% Specify the number of vehicles to be placed in the scenario as 3. Select any three points in the generated set as the start positions for the vehicles.

numVehicles = 3;
startSet1 = [points(2,:);points(4,:);points(7,:)];
yaw1 = [yaw(2);yaw(4);yaw(7)];

% Place vehicles in the selected points by using vehicle function.

for idx = 1 : numVehicles
    vehicle(scenario,'Position',startSet1(idx,:),'Yaw',yaw1(idx),'ClassID',1);
end

% Generate another set of points by defining the ROIs. Compute the coordinates to specify a circular ROI.

xCor = 0;
yCor = 0;
radius = 50;
thetha = 0: pi/10: 2*pi;
roiCircular(:,1) = xCor+radius*cos(thetha);
roiCircular(:,2) = yCor+radius*sin(thetha);

% Specify the number of points to be generated within the ROI and the number of vehicles to place within the ROI as 3. Select all the points within the circular ROI as the start positions for the vehicles.

numPoints = 3;
numVehicles = numPoints;
[startSet2,yaw2] = helperSamplePositions(scenario,numPoints,'ROI',roiCircular);
for idx = 1 : size(startSet2,1)
    vehicle(scenario,'Position',startSet2(idx,:),'Yaw',yaw2(idx),'ClassID',1);
end

% Specify the coordinates for a rectangular ROI. Set the number of points to be generated within the ROI and the number of vehicles to place within the ROI as 3. Set the longitudinal distance between two consecutive points in the same lane as 30m. If the area of ROI is not large enough to accomodate the specified number of points at the specified longitudinal distance, then the helperSamplePositions function returns only those number of points that can be accommodated within the ROI. To get the desired number of points, you must either reduce the longitudinal distance or increase the area of the ROI.

roiRectangular = [0 0;100 100];
numPoints = 3;
[startSet3,yaw3] = helperSamplePositions(scenario,numPoints,'ROI',roiRectangular,'LongitudinalDistance',30);

% Place vehicles in the selected points by using the vehicle function.

for idx = 1 : size(startSet3,1)
    vehicle(scenario,'Position',startSet3(idx,:),'Yaw',yaw3(idx),'ClassID',1);
end

% Plot the generated sample points and the ROIs.

figScene = figure('Name','AutomaticScenarioGeneration');
set(figScene,'Position',[0, 0, 900,500]);

hPanel1 = uipanel(figScene,'Position',[0 0 0.5 1]);
hPlot1 = axes(hPanel1);
plot(scenario_in,'Parent',hPlot1);
title('Points for Selecting Start Positions')
hold on
plot(points(:,1),points(:,2),'ro','MarkerSize',5,'MarkerFaceColor','r');

plot(roiCircular(:,1),roiCircular(:,2),'LineWidth',1.2,'Color','k')
plot(startSet2(:,1),startSet2(:,2),'ko','MarkerSize',5,'MarkerFaceColor','k');

plot([roiRectangular(1,1);roiRectangular(1,1);roiRectangular(2,1);roiRectangular(2,1);roiRectangular(1,1)],...
     [roiRectangular(1,2);roiRectangular(2,2);roiRectangular(2,2);roiRectangular(1,2);roiRectangular(1,2)],...
     'LineWidth',1.2,'Color','b');
plot(startSet3(:,1),startSet3(:,2),'bo','MarkerSize',5,'MarkerFaceColor','b');
xlim([-50 190])
ylim([-85 330])
hold off

% Display the start positions and the vehicles in the scenario.

hPanel2 = uipanel(figScene,'Position',[0.5 0 0.5 1]);
hPlot2 = axes(hPanel2);
plot(scenario,'Parent',hPlot2);
title('Start Positions and Vehicle Placement')
hold on
plot(startSet1(:,1),startSet1(:,2),'rs','MarkerSize',15,'LineWidth',1.2);
plot(startSet2(:,1),startSet2(:,2),'ks','MarkerSize',15,'LineWidth',1.2);
plot(startSet3(:,1),startSet3(:,2),'bs','MarkerSize',15,'LineWidth',1.2);
xlim([-50 190])
ylim([-85 330])
hold off

% Merge all the start positions into a single matrix. The number of start positions implies the total number of vehicles in the driving scenario.

startPositions = [startSet1;startSet2;startSet3];

%% 2. Inspect Scenario Object
% Display the scenario object and inspect its properties. The Actors property of the scenario object is an 1-by-9 array and it stores information about the 9 vehicles that are added to the driving scenario. Access the details of each vehicle in Actors property by using dot indexing. Display the details about the first vehicle in the driving scenario. The Position property contains the start position of the vehicle.

scenario
scenario.Actors(1)

%
%% 3. Select Goal Positions
% Generate the goal positions for the vehicles in the scenario by using the helperSamplePositions function. The total number of goal positions must be same as the total number of start positions.

numGoalPositions = length(startPositions)

% Specify the coordinates for a polygon ROI and find 5 random points within the polygon ROI. Select these points as the goal positions for the first 5 vehicles in the scenario.

roiPolygon = [-50  170;30  250;72 170;-50 170];
numPoints1 = 5;
goalSet1 = helperSamplePositions(scenario,numPoints1,'ROI',roiPolygon);

% Display the scenario and the selected goal positions.

figure
plot(scenario); 
title('Goal Positions')
hold on
plot(roiPolygon(:,1), roiPolygon(:,2),'LineWidth',1.2,'Color','r')
plot(goalSet1(:,1), goalSet1(:,2),'ro','MarkerSize',5,'MarkerFaceColor','r')

% Generate the remaining set of goal positions in such a way that they all lie in a specific lane. Use the 'Lanes' name-value pair argument to specify the lane number for goal positions.

numPoints2 = 4;
goalSet2 = helperSamplePositions(scenario,numPoints2,'Lanes',1);
plot(goalSet2(:,1),goalSet2(:,2),'bo','MarkerSize',5,'MarkerFaceColor','b')
xlim([-50 190])
ylim([-85 310])
hold off

% Merge all the goal positions into a single matrix.

goalPositions = [goalSet1;goalSet2];

% Display the start positions and the goal positions with respect to each vehicle in the scenario.

vehicleNum = 1:length(startPositions);
table(vehicleNum(:),startPositions,goalPositions,'VariableNames',{'Vehicle','Start positions','Goal positions'})

%
%% Generate Vehicle Trajectories
% Use the helperGenerateWaypoints function to compute waypoints that connect the start and the goal positions. The function returns a structure array that contains the road centers, computed waypoints, and yaw angle for each vehicle in the scenario. Read the vehicle information from the scenario object and specify random speed values for each vehicle. Use the trajectory function to generate the trajectories for each vehicle by using the computed waypoints and random speed values.

info = helperGenerateWaypoints(scenario,startPositions,goalPositions);
for indx = 1:length(startPositions)
    vehicleData = scenario.Actors(indx);
    speed = randi([10,25],1,1);
    waypts = info(indx).waypoints;
    trajectory(vehicleData,waypts,speed);
end

% Set the stop time for the scenario.

scenario.StopTime = 50;

% Create a custom figure and display the simulated driving scenario.

close all;
figScene = figure;
set(figScene,'Position',[0,0,600,600]);
movegui(figScene,'center');
hPanel = uipanel(figScene,'Position',[0 0 1 1]);
hPlot = axes(hPanel);
plot(scenario,'Parent',hPlot);
title('Generated Scenario')
% Run the simulation
while advance(scenario)
    pause(0.01)
end

% In the generated scenario all the vehicles traverse along their trajectories at a particular speed to reach their goal positions. You can also observe collision between two actors as they traverse along their trajectories. While you synthesize a scenario for testing driving algorithms, it is important that the vehicles in the scenario do not collide. To prevent collision, you must adjust the velocity of the vehicles so that they do not collide with each other while travelling along their paths.
%
%% Modify Speed Profile to Avoid Collision
% Use the Simulink model CollisionFreeSpeedManipulator to correct the velocity of the vehicles such that they do not collide as they traverse along their trajectories. The model uses non-linear time scaling to reactively accelerate or de-accelerate a vehicle without altering its trajectory [1].

% Open the Simulink system block
open_system('CollisionFreeSpeedManipulator');

% Pass the scenario object as input
set_param('CollisionFreeSpeedManipulator/VelocityUpdate',...
          'ScenarioName','scenario')
% Run the simulation and log the output
out = sim('CollisionFreeSpeedManipulator','StopTime','50');
% Clear all the temporary variables and close the Simulink block
bdclose

%% Simulate and Visualize Generated Scenario
% Use the helpergetCFSMScenario function to convert the Simulink model output to a driving scenario object. Simulate and display the driving scenario. You can see that the vehicles travel along the specified trajectories to reach their goal positions.

newScenario = helpergetCFSMScenario(out,scenario);
close all;
figScene = figure;
set(figScene,'Position',[0,0,600,600]);
movegui(figScene,'center');
hPanel = uipanel(figScene,'Position',[0 0 1 1]);
hPlot = axes(hPanel);
plot(newScenario,'Parent',hPlot);
title('Updated Scenario')
hold on
h1 = plot(goalPositions(:,1),goalPositions(:,2),'gs','MarkerSize',15,'LineWidth',1.2);
h2 = plot(startPositions(:,1),startPositions(:,2),'rs','MarkerSize',15,'LineWidth',1.2);
legend([h2 h1],{'Start Positions';'Goal Positions'},'Location','southoutside','Orientation','horizontal')
hold off
% Run the simulation
while advance(newScenario)
    pause(0.01)
end

% You can also export the scenario to DrivingScenarioDesigner app and run the simulation.
% drivingScenarioDesigner(newScenario)
%
%
%% Tips To Avoid Collisions
% The Simulink model CollisionFreeSpeedManipulator adjusts only the speed profile of the active vehicles. Once the vehicle reaches the goal position and becomes non-active in the scenario, it is not considered for checking collisions. If you want to generate a driving scenario with non-colliding vehicles, select points at less proximities and in different lanes as the start and the goal positions. If there is another vehicle whose goal position is close to the non-active vehicle and trajectory is same as the non-active vehicle's trajectory then there will be collision between these vehicles. Similarly, collision occurs when two vehicles travelling in same or different lanes come in close proximity at the road intersections. Also, the chances for collision is more if two or more goal positions lie in the same lane.
%                2.
% If you want to generate a driving scenario with non-colliding vehicles, select random points at less proximities and in different lanes as the start and the goal positions.
% Copyright 2020 The MathWorks, Inc.