%% Highway Lane Following with RoadRunner Scene
% This example shows how to configure and simulate a highway lane following
% application using a scene created in RoadRunner&trade; 3D scene editing
% tool. This example closely follows the
% <docid:driving_ug#mw_574c4bc8-4369-4da6-a4d3-aa48e7e31f92 Highway Lane
% Following> example.
%
%% Introduction
% A highway lane following system steers a vehicle to travel within a
% marked lane. It also maintains a set velocity or safe distance to a
% preceding vehicle in the same lane. The system typically includes vision
% processing, sensor fusion, decision logic, and controls components. To
% ensure functional safety and interoperability, this system requires
% testing on a variety of road conditions. For example, testing on a scene
% that has varying shadows, lane marking types, and road materials can
% make it easier to identify edge cases.
%
% <https://www.mathworks.com/products/roadrunner.html RoadRunner> is an
% interactive editor that enables you to design 3D scenes for simulating
% and testing automated driving systems. You can use RoadRunner to create
% roads, lane markings, road signs, vegetation, and scenes with varied
% complexities in a 3D environment.
%
% The <docid:driving_ug#mw_574c4bc8-4369-4da6-a4d3-aa48e7e31f92 Highway
% Lane Following> example that this example is based on shows how to
% simulate scenarios for curved and straight road scenes. This example
% demonstrates how to simulate scenarios with a scene created in
% RoadRunner. The scene contains variations in shadows, lane marking types,
% and road materials designed to test the impact of the vision processing
% on system functionality. In this example, you will:
%
% # *Review scene:* Explore the scene and road segments that were created
% in RoadRunner.
% # *Integrate scene into driving scenario:* Export the road network from
% the RoadRunner scene to an OpenDRIVE&reg; file, and then import this file
% into a driving scenario. Then, add a vehicle to the scenario and simulate
% the scenario.
% # *Integrate scene into Unreal Engine scenario:* Export the RoadRunner
% scene to an Unreal Engine&reg; game and connect a Simulink&reg; model to
% this scene. The imported driving scenario specifies vehicle poses. Add
% sensors to the vehicle and the simulate the scenario.
% # *Integrate scene into lane following application:* Using the techniques
% described in the previous sections, you integrate the RoadRunner scene
% into a scenario for highway lane following. Then, you add additional
% target vehicles to the scenario and simulate the system in a scenario
% that transitions from no shadows to shadows.
% # *Explore additional scenarios:* Simulate additional scenarios for lane
% marking and road type variations. Apply these techniques to your own
% design.
%
% You can use the modeling patterns and techniques used in this example to
% import your own scenes and test your algorithms. 
%
% In this example, you enable system-level simulation through integration
% with the Unreal Engine from Epic Games&reg;. This simulation environment
% requires a Windows&reg; 64-bit platform.
%%
if ~ispc
    error(['3D Simulation is only supported on Microsoft', char(174),...
          ' Windows', char(174), '.']);
end
 
%%
% To ensure reproducibility of the simulation results, set the random seed.
rng(0);

%%
% This example also requires you to download the Automated Driving
% Toolbox&trade; Interface for Unreal Engine 4 Projects support package.
%%
pathToUnrealExe = fullfile(...
    matlabshared.supportpkg.getSupportPackageRoot,...
    "toolbox","shared","sim3dprojects","driving","RoadRunnerScenes",....
    "WindowsPackage", "RRScene.exe");
if (~exist(pathToUnrealExe, 'file'))
    error('This example requires you to download and install Automated Driving Toolbox Interface for Unreal Engine 4 Projects support package'); 
end

%% Review Scene
% This example includes a scene (|RRHighway.rrscene|) that was designed in
% RoadRunner. If you have RoadRunner installed, you can follow the workflow
% in the <docid:roadrunner_ug#Scenes Scenes> RoadRunner topic to open the
% |RRHighway.rrscene| scene.
%
% This scene is designed to pose challenges to the lane following system.
% This image shows that the road network is divided into six sections.
% Each section incrementally adds variations to the scene as follows:
%
% * Section 1 contains dashed lanes without guard rails.
% * Section 2 contains dashed lanes with guard rails.
% * Section 3 adds trees that do cast shadows on the lane markings.
% * Section 4 contains trees that do not cast shadows on lane markings.
% * Section 5 contains solid lane markings instead of dashed lane
% markings.
% * Section 6 road material with lighter texture in comparison with other
% sections of the road.
%
% <<../RRScenario.png>>
%
% For example, the following image shows the transition between Section-3
% (with trees that do not cast shadows on the lane markings) and Section-4
% (with trees that do cast shadows on lane markings).
%
% <<../RRNoShadowsToShadows.png>>
%
% The following image shows the transition between Section-4 (with dashed
% lane markings) and Section-5 (with solid lane markings).
%
% <<../RRDashedToSolid.png>>
%
% The following image shows the transition between Section-5 (with darker
% road material) and Section-6 (with lighter road material).
%
% <<../RRRoadMaterialDifference.png>>
%
%% Integrate Scene into Driving Scenario
% A common motivation for importing scenes into a driving scenario is to
% enable the addition of vehicles and trajectories, either interactively or
% programmatically. You can integrate a road network from a RoadRunner
% scene into a driving scenario by using these steps:
%
% # Export OpenDRIVE file from RoadRunner. 
% # Import OpenDRIVE file into driving scenario.
% # Add vehicle and trajectory to driving scenario.
% # Simulate driving scenario.
%
% *Export OpenDRIVE File from RoadRunner* 
%
% RoadRunner enables you to export scenes to many file formats, include
% OpenDRIVE&reg;. To learn more about the OpenDRIVE export workflow, see
% <docid:roadrunner_ug#Exporting-to-OpenDRIVE Export to OpenDRIVE>. This
% example includes an OpenDRIVE file (|RRHighway.xodr|) that was exported
% from the RoadRunner scene (|RRHighway.rrscene|) using the process
% described in that topic.
%
% *Import OpenDRIVE File into Driving Scenario*
%
% OpenDRIVE files can be imported into cuboid driving scenarios. To learn
% more about this workflow, see
% <docid:driving_ug#mw_4b0b0843-18fc-4c8b-9041-2e3e9838df97 Import
% OpenDRIVE Roads into Driving Scenario>.
%
% Create a driving scenario and import the OpenDRIVE road network. For the
% purposes of this example, turn off the warnings from the OpenDRIVE
% importer.
%%
scenario = drivingScenario;
warning('off','driving:scenario:OpenDRIVEWarnings');
roadNetwork(scenario,"OpenDrive","RRHighway.xodr");

%%
% *Add Vehicle and Trajectory to Driving Scenario*
%
% You can add vehicles to a scenario either programmatically or
% interactively. This example shows the programmatic workflow. To add
% vehicles interactively, use the *Driving Scenario Designer* app.
%
% Add a vehicle to the road network using a set of predefined waypoints.
% These waypoints are attached as a supporting file, |manualwaypoints.mat|.
%%
v = vehicle(scenario); 
load("manualWaypoints.mat","waypoints");
speed = 20; % m/s 
trajectory(v, waypoints, speed);

%%
% *Simulate Driving Scenario*
%
% Plot the driving scenario in world coordinates. Also plot the scenario
% from the vehicle perspective by using a chase plot.

hFigScenario = figure;
p1 = uipanel('Position',[0 0 0.5 1]); 
h1 = axes("Parent",p1); 
plot(scenario,"Waypoints","On","Parent",h1); 
p2 = uipanel('Position',[0.5 0 0.5 1]); 
h2 = axes("Parent",p2); 
chasePlot(v,"Parent",h2);
%%
% Set the visibility of the figure to off.
%%
set(hFigScenario, 'Visible', 'Off');
%%
% You can optionally continue to explore, simulate, and edit the scenario
% in <docid:driving_ref#mw_07e6310f-b9c9-4f4c-b2f9-51e31d407766 Driving
% Scenario Designer> |drivingScenarioDesigner(scenario)|.

%% Integrate Scene into Unreal Engine Scenario
% A common motivation for importing scenes into Unreal Engine is to enable
% simulation systems with camera, radar, and lidar sensor models. You can
% integrate a RoadRunner scene with an Unreal Engine driving scenario
% simulation using these steps:
%
% # Export Unreal Engine scene from RoadRunner. 
% # Configure Unreal Engine scene.
% # Create test bench model.
% # Simulate test bench model. 
%
% *Export Unreal Engine Scene from RoadRunner*
%
% RoadRunner enables exporting to Unreal Engine scene. To learn more about
% this workflow, see <docid:roadrunner_ug#Exporting-to-Unreal Exporting to
% Unreal>. The workflow includes exporting Filmbox (.fbx) and XML files,
% which can be imported into the Unreal Editor. After you open a scene in
% the Unreal Engine editor, you might want to adjust other scene aspects
% such as lighting.
%
% This example uses an Unreal Engine scene (|RRHighway|) that was
% exported from the RoadRunner scene (|RRHighway.rrscene|).
%
% *Configure Unreal Engine Scene*
%
% The Unreal Engine scene can co-simulate with Simulink using a
% |MathWorksSimulation| plugin from the
% <https://www.mathworks.com/matlabcentral/fileexchange/74555-automated-driving-toolbox-interface-for-unreal-engine-4-projects
% Automated Driving Toolbox Interface for Unreal Engine 4 Projects> support
% package. To install the support package, follow the steps in
% <docid:driving_ug#mw_75c4b3ea-e08f-4af3-b6d6-e0a11e5fd6b4 Install Support
% Package for Customizing Scenes>.
%
% The Unreal Engine scene (|RRHighway|) used in this example has been
% compiled with the |MathWorksSimulation| plugin.
%
% *Create Test Bench Model*
%
% You can connect a Simulink model to the Unreal Engine for co-simulation.
% To learn more about this workflow, see
% <docid:driving_ug#mw_84f3846e-9919-41d6-9d5e-d193ef8d3698 Simulate a
% Simple Driving Scenario and Sensor in 3D Environment>. The open-loop test
% bench model (|RRHighwayTestBench.slx|) uses this workflow to connect to
% the Unreal Engine scene (|RRHighway|).
%
% Open the test bench model for the scenario.
%%
open_system("RRHighwayTestBench");

%%
% Opening this model runs the |helperSLRRHighwaySetup| script. This script
% configures the Simulation 3D Scene Configuration block in the
% |RRHighwayTestBench| model.

%%
% This model contains blocks that enable simulating a driving
% scenario with Unreal Engine.
%
% * <docid:driving_ref#mw_0abe0f52-f25a-4829-babb-d9bafe8fdbf3 Scenario
% Reader> reads the driving scenario from the base workspace and outputs
% the ego vehicle pose.
% * <docid:driving_ref#mw_96a521fd-316f-497b-bc01-b2c5f4083563 Simulation
% 3D Scene Configuration> enables connection to the |RRHighway| scene.
% * <docid:driving_ref#mw_32cd8e72-2d69-4c3e-98b0-5b918db383a4 Simulation
% 3D Vehicle with Ground Following> controls the pose of a vehicle in the
% scene
% * <docid:driving_ref#mw_27640406-e319-4cd8-8d2c-6231f1a6a233 Vehicle To
% World> converts actor poses from the coordinates of the input ego vehicle
% to the world coordinates of the scenario.
% * <docid:driving_ref#mw_e9491451-3198-4988-8ef1-6a3878d29155 Simulation
% 3D Camera> synthesizes camera images.
% * <docid:driving_ref#mw_a657f4e8-85c3-41ea-8d9c-1146805720f0 Simulation
% 3D Probabilistic Radar> synthesizes radar detections.
% * <docid:driving_ref#mw_f17d49dd-d3d9-40fb-b1cc-2e35dfdf5302 Simulation
% 3D Lidar> synthesizes lidar point cloud data.
%
% The model also contains blocks to visualize the camera and lidar sensors.
% You can use the Bird's-Eye Scope to visualize radar and vision detections
% from a bird's-eye view. To learn how to configure this scope, see
% <docid:driving_ug#mw_a5e20cb3-45b7-4d10-92d6-2bf27f7a26ba Visualize 3D
% Simulation Sensor Coverages and Detections>.
%
% *Simulate Test Bench Model*
%
% Simulate the model. The vehicle follows the trajectory defined
% in the driving scenario.
%%
sim("RRHighwayTestBench");

%%
% <<../RROpenLoopOutput.png>>
%% Integrate Scene into Lane Following Application
% You can reuse the techniques described in the previous sections to
% simulate and assess a closed-loop system such as the highway lane
% following application. This section reuses models, helper functions, and
% techniques described in the
% <docid:driving_ug#mw_574c4bc8-4369-4da6-a4d3-aa48e7e31f92 Highway Lane
% Following> example. Open and configure the system test bench model.
%%
open_system("HighwayLaneFollowingTestBench"); 
scenarioFcnName = "scenario_RRHighway_01_NoShadowToShadow"; 
visionVariant = "VisionProcessingAlgorithm"; 
helperSLHighwayLaneFollowingSetup(scenarioFcnName,visionVariant);

%%
% *Integrate Scene into Driving Scenario*
%
% The first argument to the |helperSLHighwayLaneFollowingSetup| is the name
% of a function that creates a driving scenario that is compatible
% with the |HighwayLaneFollowingTestBench|. The
% |scenario_RRHighway_01_NoShadowToShadow| function creates this driving
% scenario. It imports the |RRHighway.xodr| OpenDRIVE file and adds
% vehicles around the road segment that transitions from no shadow to
% shadow. You can explore this function to learn more about programmatic
% techniques for creating scenarios. It leverages several helper functions
% that you can use to simplify adding vehicle and their trajectories to the
% imported road network. Explore the
% |scenario_RRHighway_01_NoShadowToShadow| to know more about the helper
% functions and their usage.
%
% The |helperSLHighwayLaneFollowingSetup| function creates a variable
% scenario in the base workspace. Plot this scenario and notice the region
% of the scene that will be simulated.
%%
hFigScenario = figure;
p1 = uipanel('Position',[0 0 0.5 1]); 
h1 = axes("Parent",p1); 
plot(scenario,"Waypoints","On","Parent",h1); 
p2 = uipanel('Position',[0.5 0 0.5 1]); 
h2 = axes("Parent",p2); 
chasePlot(scenario.Actors(1),"Parent",h2)
%%
% Set the visibility of the figure to off.
%%
set(hFigScenario, 'Visible', 'Off');
%%
% *Integrate Scene into Unreal Engine Scenario*
%
% The |HighwayLaneFollowingTestBench| model contains algorithm components
% for vision detection, forward vehicle sensor fusion, and controls. The
% Simulation 3D Scenario subsystem integrates the model with the driving
% scenario and the corresponding Unreal Engine game.
%
% Open the Simulation 3D Scenario subsystem.
open_system("HighwayLaneFollowingTestBench/Simulation 3D Scenario") 

%%
% Notice that this subsystem reuses the modeling techniques and blocks
% described previously in this example. 
% 
% The primary differences are:
%
% * The ego vehicle is under closed-loop control. 
% * Target vehicles are added to the scenario. 
% * The lane following example does not use a lidar sensor.
%
% Simulate the model to see the ego vehicle behavior for the scenario.
%%
mpcverbosity('off');
sim("HighwayLaneFollowingTestBench");

%%
% The system is able to detect and follow lanes in the conditions where
% shadows are present. For more details on how to analyze the simulation
% results, refer to the
% <docid:driving_ug#mw_574c4bc8-4369-4da6-a4d3-aa48e7e31f92 Highway Lane
% Following> example.
%%
% <<../RRClosedLoopOutput.png>>
%%
% Close the figure.
%%
close(hFigScenario);
%% Explore Additional Scenarios
% This example provides additional scenarios that you can use to test the
% system behavior.
%
% * |scenario_RRHighway_02_DashedToSolidMarkings| function configures the
% test scenario such that the ego vehicle navigates from Section 4 to
% Section 5 of the scene. This enables testing the lane following
% application for the transition from dashed lane markings to solid lane
% markings.
% * |scenario_RRHighway_03_DarkToLightRoadMaterial| function configures the
% test scenario such that the ego vehicle navigates from Section 5 to
% Section 6 of the scene. This enables testing the lane following
% application for the transition from darker textured road material to
% lighter textured road material.
%
% You can configure the model and workspace with these scenarios by using
% the |helperSLHighwayLaneFollowingSetup| function. For example, this code
% configures the test bench to simulate a scenario in the region where the
% road material changes.
%%
helperSLHighwayLaneFollowingSetup(... 
    "scenario_RRHighway_03_DarkToLightRoadMaterial",... 
    "VisionProcessingAlgorithm");

%%
% You can apply these techniques to integrate RoadRunner scenes into
% driving scenarios for simulation and testing of your systems.
%%
% Enable the Model Predictive Controller update messages and turn back on
% warnings from the OpenDRIVE importer.
mpcverbosity('on');
warning('on','driving:scenario:OpenDRIVEWarnings');
%%
% Copyright 2020 The MathWorks, Inc.