%% Code Generation for Path Planning and Vehicle Control
% This example shows how to modify a Simulink(R) model of a path planning and 
% vehicle control algorithm, generate C++ code, and verify the generated code  
% using software-in-the-loop (SIL) simulation.
%
% Copyright 2018-2019 The MathWorks, Inc.

%% Introduction
% Developing a path planning and vehicle control algorithm often involves 
% designing and simulating an algorithm model in Simulink, implementing the 
% algorithm in C++ code, and integrating the algorithm code into an external 
% software environment for deployment into a vehicle. Automatically 
% generating and verifying code from the algorithm model ensures functional 
% equivalence between the simulation and implementation.
% 
% The <docid:driving_ug#mw_b8684291-90c7-415b-97db-725062fe2674 Automated Parking Valet in Simulink>
% example showed how to design a path planning and vehicle control algorithm.  
% This example shows how to modify the design for implementation in C++. This 
% steps in this workflow are:
% 
% # Partition the design into algorithm and test bench models.
% # Modify the algorithm model to support code generation.
% # Generate C++ code from the algorithm model.
% # Verify the behavior of the generated code using SIL simulation.
% 
% You can then integrate the generated code into an external software project 
% for further testing in a vehicle.

%% Partition the Algorithm and Test Bench
% The original model from the <docid:driving_ug#mw_b8684291-90c7-415b-97db-725062fe2674 Automated Parking Valet in Simulink>
% example has already been partitioned into separate algorithm and test bench models. 
% 
% * *Algorithm Model*: |AutomatedParkingValetAlgorithm| specifies the path 
% planning and vehicle control functionality to be implemented in C++. 
% * *Test Bench Model*: |AutomatedParkingValetTestBench| specifies the 
% stimulus and environment to test the algorithm model.

%% Simulate Test Bench Model
% The |AutomatedParkingValetTestBench| model specifies the stimulus and 
% environment to test the |AutomatedParkingValetAlgorithm| model. The main 
% components of the |AutomatedParkingValetTestBench| include:
% 
% * *Algorithm Model Reference*: The |AutomatedParkingValetAlgorithm| model 
% block is referenced by a Model block. The Model block and supports simulating 
% the referenced model in different modes of simulation including normal 
% and SIL modes. To learn more about the Model block, refer to <docid:simulink_ug#f4-141756 Reference Existing Models>.
% * *Costmap*: The Costmap Creator block creates the costmap of the 
% environment and outputs it as a bus signal.
% * *Behavior Planner*: The Behavior Planner block triggers a sequence of 
% navigation tasks based on the global route plan by providing an intermediate goal and configuration.
% * *Vehicle Model*: To demonstrate the performance of the algorithm, the 
% parking valet controller is applied to the Vehicle Model block, which 
% contains a Vehicle Body 3DOF block.
%
% The |AutomatedParkingValetTestBench| model is also configured to log the  
% pose(|CurrPose|) and longitudinal velocity (|CurrVelocity|) of the vehicle  
% and the status of whether the goal from the behavioral planner was reached 
% (|GoalReached|). These signals are logged to the workspace variable |logsout|. 

%%
% Simulate the test bench model with the algorithm in normal mode.
open_system('AutomatedParkingValetTestBench')
set_param('AutomatedParkingValetTestBench/AutomatedParkingValetAlgorithm','SimulationMode','Normal');
sim('AutomatedParkingValetTestBench')
helperPlotSimulationSignals(logsout)
%%
% The first figure displays the path that the vehicle traversed from the 
% parking lot input to the final parking space. The second figure plots the 
% velocity and goal-reached signals. Notice that the vehicle velocity is 
% smooth and continuous when transitioning between goals.

%% Modify Algorithm Model to Support Code Generation
% The |AutomatedParkingValetAlgorithm| model specifies the functionality to 
% be implemented in C++. The main components of the |AutomatedParkingValetAlgorithm| model are:
% 
% * *Path Planner*: Plans a feasible path through the environment map using 
% a <docid:driving_ref#mw_3e6efb93-e83e-4c4b-9e7b-0bebb1c1179c pathPlannerRRT> object.
% * *Trajectory Generator*: Smooths the reference path by fitting splines 
% and converts the smoothed path into a trajectory by generating a speed profile.
% * *Vehicle Controller*: Controls the steering and velocity of the vehicle 
% to follow the generated path and speed profile.
% 
% Open and update the algorithm model.
open_system('AutomatedParkingValetAlgorithm')
set_param('AutomatedParkingValetAlgorithm','SimulationCommand','Update');

%%
% The |AutomatedValetParking| model includes several modifications from the 
% <docid:driving_ug#mw_b8684291-90c7-415b-97db-725062fe2674 Automated Parking Valet in Simulink> 
% example to support code generation. The most significant modifications 
% are specifying fixed-size component interfaces and explicit rate transitions.
% 
% Variable-size component interfaces have been replaced with fixed-size 
% interface to enable generating and verifying C++ code with SIL simulation. 
% 
% * The variable-size |Poses| signal has been split into a fixed-size outport 
% (|RefPosesArray|) with an additional outport specifying the size (|RefPosesSize|).
% * The |costmapBus| bus associated with the |Costmap| input port contains only 
% fixed-size elements, since the costmap does not change size in this example.
% 
% The |AutomatedValetParking| model contains multiple rates. The color of the 
% blocks represents different sample times. Path planning and trajectory 
% generation is performed at a 0.1s sample time and is colored green. 
% Vehicle control is performed at a 0.05s sample time and is colored red. 
% To learn more about displaying sample time colors, refer to 
% <docid:simulink_ug#bry66ow View Sample Time Information>.
% 
% Explicit rate transition blocks have been inserted into the model to 
% treat each rate as a separate task. 
% 
% * A Rate Transition block has been inserted to the fixed-size |CurrPose|
% signal.
% * A helper Varsize Rows Rate Transition block (named RT) has been inserted to 
% variable-size signals that connect blocks of different rates.
% 
% Treating each rate as a specific task enables generating a C++ class with 
% separate method entry points for each rate. Generating separate methods 
% for each rate simplifies integration into multi-tasking software 
% schedulers or operating systems in the vehicle. To learn more about 
% treating rates as separate tasks, refer to <docid:ecoder_ug#bumxn1_ Modeling for Multitasking Execution>.

%% Configure and Generate Code from Algorithm Model
% Configuring the |AutomatedParkingValetAlgorithm| model to generate code includes setting parameters to:
% 
% * Generate C++ code with entry points for each rate.
% * Apply common optimizations.
% * Generate a report to facilitate exploring the generated code.
% 
% Set and view model parameters to enable C++ code generation. 
helperSetModelParametersForCodeGeneration('AutomatedParkingValetAlgorithm')

%%
% Generate code and the code generation report from the algorithm model.
rtwbuild('AutomatedParkingValetAlgorithm');

%%
% Use the Code Generation Report to explore the generated code. To learn 
% more about the Code Generation Report, refer to <docid:rtw_ug#btcfnxv Reports for Code Generation>. 
% Use the Code Interface Report link in the Code Generation Report to 
% explore these generated methods:
% 
% * |initialize|: Call once on initialization.
% * |step0|: Call periodically every 0.05s to execute trajectory
% generation and vehicle control.
% * |step1|: Call periodically every 0.1s seconds to execute path planning.
% * |terminate|: Call once on termination.
% 
% Additional get and set methods for signal interface are declared in 
% |AutomatedParkingValetAlgorithm.h| and defined in |AutomatedParkingValetAlgorithm.c|.

%% Verify Implementation with SIL Simulation
% Software-in-the-loop (SIL) simulation provides early insight into the 
% behavior of a deployed application. To learn more about SIL simulation, 
% refer to <docid:ecoder_ug#br90rb7 SIL and PIL Simulations>.
%  
% SIL simulation enables you to:
% * Verify that the compiled generated code on the host is functionally 
% equivalent to the normal mode.
% * Log execution times of generated code on the host computer. These times  
% can be an early indicator of performance of the generated code. For 
% accurate execution time measurements, profile the generated 
% code when it is integrated into the external environment or 
% when using with processor-in-the-loop(PIL) simulation. To learn more about SIL profiling, 
% refer to <docid:ecoder_ug#bst4d8a Code Execution Profiling with SIL and PIL>.
% 
% Configure algorithm and test bench model parameters to support SIL 
% simulation and log execution profiling information.
helperSetModelParametersForSIL('AutomatedParkingValetAlgorithm');
helperSetModelParametersForSIL('AutomatedParkingValetTestBench');

%%
% Simulate the test bench model with the algorithm in SIL mode and plot
% the results.
open_system('AutomatedParkingValetTestBench')
set_param('AutomatedParkingValetTestBench/AutomatedParkingValetAlgorithm','SimulationMode','Software-in-the-loop (SIL)');
sim('AutomatedParkingValetTestBench');
%%
helperPlotSimulationSignals(logsout, executionProfile)
%%
% The execution time for the |step0| and |step1| methods are shown in the lower 
% plot. The plots indicate that the maximum execution time is required at 
% the lower rate (|step1|) after a goal pose is achieved. This lower rate 
% is expected because it corresponds to the time when a new path is planned. 

%% Conclusion
% This example demonstrated a workflow to generate and verify C++ code for 
% a path planner and vehicle control algorithm. Compiling and verifying the code 
% with SIL simulation established confidence that the generated code is 
% functionally correct before integrating into an external software environment. 
% The workflow was demonstrated as an extension of the 
% <docid:driving_ug#mw_b8684291-90c7-415b-97db-725062fe2674 Automated Parking Valet in Simulink>
% example and is generally applicable to designing and implementing path 
% planning applications.