%% Visualize Sensor Data from Unreal Engine Simulation Environment
% This example shows how to visualize sensor coverages and detections
% obtained from high-fidelity radar and lidar sensors in a 3D simulation
% environment. In this example, you learn how to:
%
% # Configure Simulink(R) models to simulate within the 3D environment.
% This environment is rendered using the Unreal Engine(R) from Epic
% Games(R).
% # Read ground truth data and vehicle trajectories from a scenario
% authored using the
% *<docid:driving_ref#mw_07e6310f-b9c9-4f4c-b2f9-51e31d407766 Driving
% Scenario Designer>* app, and then recreate this scenario in the Simulink
% model.
% # Add radar and lidar sensors to these models by using
% <docid:driving_ref#mw_a657f4e8-85c3-41ea-8d9c-1146805720f0 Simulation 3D
% Probabilistic Radar> and
% <docid:driving_ref#mw_f17d49dd-d3d9-40fb-b1cc-2e35dfdf5302 Simulation 3D
% Lidar> blocks.
% # Visualize the driving scenario and generated sensor data in the
% *<docid:driving_ref#mw_59742eb7-dce8-4938-9c2e-44d34c7b8891 Bird's-Eye
% Scope>*.
%
% You can use these visualizations and sensor data to test and improve your
% automated driving algorithms. You can also extend this example to fuse
% detections and visualize object tracking results, as shown in the
% <docid:driving_ug#mw_8ae0eb21-efca-4a6d-9a4a-0415a638c554 Sensor
% Fusion Using Synthetic Radar and Vision Data in Simulink> example.
%

% Copyright 2019 The MathWorks, Inc.

%% Inspect Cuboid Driving Scenario
% In this example, the ground truth (roads, lanes, and actors) and vehicle
% trajectories come from a scenario that was authored in the
% *<docid:driving_ref#mw_07e6310f-b9c9-4f4c-b2f9-51e31d407766 Driving
% Scenario Designer>* app. In this app, vehicles and other actors are
% represented as simple box shapes called _cuboids_. For more details about
% authoring cuboid scenarios, see the
% <docid:driving_ug#mw_e49e404a-0301-4634-b5c2-c8a6da2db9f6 Build a Driving
% Scenario and Generate Synthetic Detections> example.
%
% Open the cuboid driving scenario file in the app.
%
%   drivingScenarioDesigner('StraightRoadScenario.mat')
%
% In the app, run the scenario simulation. In this scenario, the ego
% vehicle (a blue car) travels north along a straight road at a constant
% speed. In the adjacent lane, an orange car travels north at a slightly
% higher constant speed. In the opposite lane, a yellow truck drives south
% at a constant speed.
%
% <<../StraightRoadScenario.png>>
%
% When authoring driving scenarios that you later recreate in the 3D
% simulation environment, you must use a road network identical to one from
% the default 3D scenes. Otherwise, in the recreated scenario, the
% positions of vehicles and sensors are inaccurate. This driving scenario
% uses a recreation of the
% <docid:driving_ref#mw_1f96887a-fedc-496c-bf4f-7696c4cba03b Straight Road>
% scene. To select a different cuboid version of a 3D scene, on the app
% toolstrip, select *Open > Prebuilt Scenario > Simulation3D* and choose
% from the available scenes. Not all 3D scenes have corresponding versions
% in the app.
%
% * For a list of supported scenes and additional details about each scene,
% see <docid:driving_ug#mw_be43977b-1cd6-4647-b590-a2fd40bfbe3e Cuboid
% Versions of 3D Simulation Scenes in Driving Scenario Designer>.
% * To generate vehicle trajectories for scenes that are not available in
% the app, use the process described in the
% <docid:driving_ug#mw_f780d508-aca7-46bb-b440-40ec9e6ff0af Select
% Waypoints for 3D Simulation> example instead.
%
% The dimensions of vehicles in the cuboid scenarios must also match the
% dimensions of one of the predefined 3D simulation vehicle types. On the
% app toolstrip, under *3D Display*, the *Use 3D Simulation Actor
% Dimensions* selection sets each cuboid vehicle to have the dimensions of
% a 3D vehicle type. In this scenario, the vehicles have these 3D display
% types and corresponding vehicle dimensions.
%
% * |Ego Vehicle| &mdash;
% <docid:driving_ref#mw_57a6d568-b260-4e54-b25e-76a58f16b3ce Sedan> vehicle
% dimensions
% * |Vehicle in Adjacent Lane| &mdash;
% <docid:driving_ref#mw_a9b33d5d-29cd-4db4-8bb5-2027444de2d4 Muscle Car>
% vehicle dimensions
% * |Vehicle in Opposite Lane| &mdash;
% <docid:driving_ref#mw_d89a0b78-1915-49fc-9951-16a72d715c33 Box Truck>
% vehicle dimensions
%
% To change a vehicle to a different display type, on the *Actors* tab in
% the left pane of the app, update the *3D Display Type* parameter for that
% vehicle. To change the color of a vehicle, select the color patch next to
% the selected vehicle and choose a new color.
%
% To preview how the vehicles display in the 3D environment, use the
% 3D display window available from the app. On the app toolstrip, select
% *3D Display > View Simulation in 3D Display* and rerun the simulation.
%
% <<../StraightRoadScenario3DDisplay.png>>
%
%% Open 3D Simulation Model
% The model used in this example recreates the cuboid driving scenario. The
% model also defines high-fidelity sensors that generate synthetic
% detections from the environment. Open the model.
%
open_system('Visualize3DSimulationSensorCoveragesDetections')
%% Inspect Scene Configuration
% The <docid:driving_ref#mw_96a521fd-316f-497b-bc01-b2c5f4083563 Simulation
% 3D Scene Configuration> block configures the model to simulate in the 3D
% environment.
%
% * The *Scene name* parameter is set to the default |Straight road| scene.
% This scene corresponds to the cuboid version defined in the app scenario
% file.
% * The *Scene view* parameter is set to |Ego Vehicle|. During simulation,
% the 3D simulation window displays the scene from behind the ego vehicle.
%
% The <docid:driving_ref#mw_0abe0f52-f25a-4829-babb-d9bafe8fdbf3 Scenario
% Reader> block reads the ground truth data (road boundaries, lane
% markings, and actor poses) from the app scenario file. The *Bird's-Eye
% Scope* visualizes this ground truth data, not the ground truth data of
% the 3D simulation environment. To use the same scene for the cuboid and
% 3D simulation environments, the ground truth data for both environments
% must match. If you are creating a new scenario, you can generate a
% Scenario Reader block that reads data from your scenario file. First,
% open the scenario file in the *Driving Scenario Designer* app. Then, on
% the app toolstrip, select *Export > Export Simulink Model*. If you update
% the scenario, you do not need to generate a new Scenario Reader block.
%
% The Simulation 3D Scene Configuration block and Scenario Reader block
% both have their *Sample time* parameter set to |0.1|. In addition, all
% other 3D simulation vehicle and sensor blocks inherit their sample time
% from the Simulation 3D Scene Configuration block. By setting a single
% sample time across the entire model, the *Bird's-Eye Scope* displays data
% from all blocks at a constant rate. If the ground truth and sensor data
% have different sample times, then the scope visualizes them at different
% time intervals. This process causes the ground truth and sensor data
% visualizations to flicker.
%% Inspect Vehicle Configuration
% The <docid:driving_ref#mw_32cd8e72-2d69-4c3e-98b0-5b918db383a4 Simulation
% 3D Vehicle with Ground Following> blocks specify the appearances and
% trajectories of the vehicles in the 3D simulation environment. Each
% vehicle is a direct counterpart to one of the vehicles defined in the
% *Driving Scenario Designer* app scenario file.
%
% In the 3D environment, vehicle positions are in world coordinates.
% However, the Scenario Reader block outputs the poses of non-ego actors in
% ego vehicle coordinates. A
% <docid:driving_ref#mw_27640406-e319-4cd8-8d2c-6231f1a6a233 Vehicle To
% World> block converts these non-ego actor poses into world coordinates.
% Because the ego vehicle is output in world coordinates, this conversion
% is not necessary for the ego vehicle. For more details about the vehicle
% and world coordinate systems, see <docid:driving_ug#bvgu_ya-2 Coordinate
% Systems in Automated Driving Toolbox>.
%
% Locations of vehicle origins differ between cuboid and 3D scenarios.
%
% * In cuboid scenarios, the vehicle origin is on the ground, at the center
% of the rear axle.
% * In 3D scenarios, the vehicle origin is on ground, at the geometric
% center of the vehicle.
%
% The <docid:driving_ref#mw_da39544a-b88f-41dc-a641-9457988cebe9 Cuboid To
% 3D Simulation> blocks convert the cuboid origin positions to the 3D
% simulation origin positions. In the *ActorID used for conversion*
% parameters of these blocks, the specified |ActorID| of each vehicle
% determines which vehicle origin to convert. The Scenario Reader block
% outputs |ActorID| values in its *Actors* output port. In the *Driving
% Scenario Designer* app, you can find the corresponding |ActorID| values
% on the *Actors* tab, in the actor selection list. The |ActorID| for each
% vehicle is the value that precedes the colon.
%
% Each Cuboid To 3D Simulation block outputs *X*, *Y*, and *Yaw* values
% that feed directly into their corresponding vehicle blocks. In the 3D
% simulation environment, the ground terrain of the 3D scene determines the
% _Z_-position (elevation), roll angle, and pitch angle of the vehicles.
%
% In each Simulation 3D Vehicle with Ground Following block, the *Type*
% parameter corresponds to the *3D Display Type* selected for that vehicle
% in the app. In addition, the *Color* parameter corresponds to the vehicle
% color specified in the app. To maintain similar vehicle visualizations
% between the *Bird's-Eye Scope* and the 3D simulation window, the
% specified type and color must match. To change the color of a vehicle in
% the app, on the *Actors* tab, click the color patch to the right of the
% actor name in the actor selection list. Choose the color that most closely
% matches the colors available in the *Color* parameter of the Simulation
% 3D Vehicle with Ground Following block.
%% Inspect Sensor Configuration
% The model includes two sensor blocks with default parameter settings.
% These blocks generate detections from the 3D simulation environment.
%
% * The <docid:driving_ref#mw_a657f4e8-85c3-41ea-8d9c-1146805720f0
% Simulation 3D Probabilistic Radar> sensor block generates object
% detections based on a statistical model. This sensor is mounted to the
% front bumper of the ego vehicle.
% * The <docid:driving_ref#mw_f17d49dd-d3d9-40fb-b1cc-2e35dfdf5302
% Simulation 3D Lidar> sensor block generates detections in the form of a
% point cloud. This sensor is mounted to the center of the roof of the ego
% vehicle.
%
% Although you can specify sensors in the *Driving Scenario Designer* app
% and export them to Simulink, the exported blocks are not compatible with
% the 3D simulation environment. You must specify 3D simulation sensors in
% the model directly.
%
%% Simulate and Visualize Scenario
% During simulation, you can visualize the scenario in both the 3D
% simulation window and the
% *<docid:driving_ref#mw_59742eb7-dce8-4938-9c2e-44d34c7b8891 Bird's-Eye
% Scope>*.
%
% First, open the scope. On the Simulink toolstrip, under *Review Results*,
% click *Bird's-Eye Scope*. Then, to find signals that the scope can
% display, click *Find Signals*.
%
% To run the simulation, click *Run* in either the model or scope. When the
% simulation begins, it can take a few seconds for the 3D simulation window
% to initialize, especially when you run it for the first time in a
% Simulink session. When this window opens, it displays the scenario with
% high-fidelity graphics but does not display detections or sensor
% coverages.
%
% <<../Visualize3DSimulationSensorsUnrealEngine.png>>
%
% The *Bird's-Eye Scope* displays detections and sensor coverages by
% using a cuboid representation. The radar coverage area and
% detections are in red. The lidar coverage area is in gray, and its point
% cloud detections display as a parula colormap.
%
% <<../Visualize3DSimulationSensorsBirdsEyeScope.png>>
%
% The model runs the simulation at a pace of 0.5 seconds per wall-clock
% second. To adjust the pacing, from the Simulink toolstrip, select *Run >
% Simulation Pacing*, and then move the slider to increase or decrease the
% speed of the simulation.
%% Modify the Driving Scenario
% When modifying your driving scenario, you might need to update the
% scenario in the *Driving Scenario Designer* app, the Simulink model, or
% in both places, depending on what you change.
%
% * *Modify the road network* &mdash; In the app, select a new prebuilt scene from
% the *Simulation3D* folder. Do not modify these road networks or the roads
% will not match the roads in the selected 3D scene. In the model, in the
% Simulation 3D Scene Configuration block, select the corresponding scene
% in the *Scene name* parameter.
% * *Modify vehicle trajectories* &mdash; In the app, modify the vehicle
% trajectories and resave the scenario. In the model, you do not need to
% update anything to account for this change. The Scenario Reader block
% automatically picks up these changes.
% * *Modify vehicle appearances* &mdash; In the app, update the color and
% *3D Display Type* parameter of the vehicles. Also make sure that the
% *3D Display > Use 3D Simulation Actor Dimensions* option is
% selected. In the model, update the *Color* and *Type* parameters of the
% corresponding Simulation 3D Vehicle with Ground Following blocks.
% * *Add a new vehicle* &mdash; In the app, create a new vehicle and specify a
% trajectory, color, and 3D display type. In the model, add a new
% Simulation 3D Vehicle with Ground Following block and corresponding
% Cuboid To 3D Simulation block. Set up these blocks similar to how the
% existing non-ego vehicles are set up. In the Cuboid To 3D Simulation
% block, set the |ActorID| of the new vehicle.
% * *Set a new ego vehicle* &mdash; In the app, on the *Actors* tab, select
% the vehicle that you want to set as the ego vehicle and click *Set As Ego
% Vehicle*. In the model, in the Cuboid To 3D Simulation blocks, update the
% *ActorID used for conversion* parameters to account for which vehicle is
% the new ego vehicle. In the sensor blocks, set the *Parent name*
% parameters such that the sensors are mounted to the new ego vehicle.
% * *Modify or add sensors* &mdash; In the app, you do not need to make any
% changes. In the model, modify or add sensor blocks. When adding sensor
% blocks, set the *Parent name* of all sensors to the ego vehicle.
%
% To visualize any updated scenario in the *Bird's-Eye Scope*, you must
% click *Find Signals* again. If you modify a scenario or are interested in
% only visualizing sensor data, consider turning off the 3D window during
% simulation. In the Simulation 3D Scene Configuration block, clear the
% *Display 3D simulation window* parameter.