%% Highway Lane Change
% This example shows how to simulate an automated lane change maneuver
% system for highway driving scenario.
% 
%% Introduction
% An automated lane change maneuver (LCM) system enables the ego vehicle to
% automatically move from one lane to another lane. The LCM system models
% the longitudinal and lateral control dynamics for automated lane change.
% An LCM system senses the environment for most important objects (MIOs)
% using on-board sensors, identifies an optimal trajectory that avoids
% these objects, and steers ego vehicle along this trajectory.
%
% This example shows how to design and test the planner and controller
% components of an LCM system. In this example, the lane change planner 
% uses ground truth information from the scenario to detect MIOs. It then  
% generates a feasible trajectory to negotiate a lane change that is 
% executed by the lane change controller. In this example, you:
%
% # *Explore the test bench model:* The model contains planning, controls, 
% vehicle dynamics, sensors, and metrics to assess functionality.
% # *Model the lane change planner:* The reference model
% contains a simple behavior layer and a motion planner. The behavior layer
% configures the motion planner to generate an optimal trajectory by
% considering MIO information.
% # *Model the lane change controller:* The reference model generates
% control commands for the ego vehicle based on the generated trajectory.
% # *Simulate and visualize system behavior:* The test bench model is
% configured to test the integration of planning and controls to perform
% lane change maneuver on a straight road with multiple vehicles.
% # *Explore other scenarios:* These scenarios test the system under
% additional conditions.
% 
% You can apply the modeling patterns used in this example to test your own
% planner and controller components of an LCM system.

%% Explore Test Bench Model
% In this example, you use a system-level simulation test bench model
% to explore the behavior of the planner and controller components for a
% lane change maneuver system. Open the system-level simulation test bench
% model.
open_system('HighwayLaneChangeTestBench')

%%
% Opening this model runs the |helperSLHighwayLaneChangeSetup| script that
% initializes the road scenario using the <docid:driving_ref#bvm8jbf
% drivingScenario> object in the base workspace. It also configures the
% controller design parameters, vehicle model parameters, and the
% Simulink&reg; bus signals required for defining the inputs and outputs
% for the |HighwayLaneChangeTestBench| model.
%% 
% The test bench model contains the following subsystems.
%
% # *Sensors and Environment* specifies the road, vehicles, and sensors
% used for simulation.
% # *Lane Change Planner* specifies behavior and trajectory planning for
% the ego vehicle.
% # *Lane Change Controller* specifies the path following controller that
% generates control commands to steer the ego vehicle along the generated
% trajectory.
% # *Vehicle Dynamics* models the ego-vehicle using a
% <docid:driving_ref#mw_9569b0a8-23c5-4aaf-ba41-e3ebe9ef7971 Bicycle
% Model> and updates its state using commands received from the *Lane
% Change Controller*.
% # *Metrics Assessment* specifies metrics to assess system level behavior.
% 
% The *Vehicle Dynamics* subsystem is based on the subsystem used in the
% <docid:driving_ug#mw_574c4bc8-4369-4da6-a4d3-aa48e7e31f92 Highway
% Lane Following> example. This example focuses on the *Lane Change
% Planner* and *Lane Change Controller* reference models.
%
% The *Sensors and Environment* subsystem uses
% <docid:driving_ref#mw_0abe0f52-f25a-4829-babb-d9bafe8fdbf3 Scenario
% Reader> and <docid:driving_ref#mw_2fa86dfd-52db-4f59-8781-7d954ae3e8dc
% Vision Detection Generator> blocks to provide the road network, vehicle
% ground truth positions, and lane detections required for the lane change
% maneuver system. Open the *Sensors and Environment* subsystem. This
% subsystem also outputs the reference path required by the lane change
% planner.
open_system('HighwayLaneChangeTestBench/Sensors and Environment')

%% 
% The <docid:driving_ref#mw_0abe0f52-f25a-4829-babb-d9bafe8fdbf3 Scenario
% Reader> block is configured to read the <docid:driving_ref#bvm8jbf
% drivingScenario> object from the base workspace. It uses this object to
% read the actor data and lane information. It takes in ego vehicle
% information to perform a closed-loop simulation. This block outputs
% ground truth information of lanes and actors in ego vehicle coordinates.
% 
% The <docid:driving_ref#mw_2fa86dfd-52db-4f59-8781-7d954ae3e8dc Vision
% Detection Generator> block provides lane detections with respect to the
% ego vehicle which help in identifying vehicles present in the ego lane
% and adjacent lanes.
%
% The *Lane Change Planner* reference model uses lane detections from the
% <docid:driving_ref#mw_2fa86dfd-52db-4f59-8781-7d954ae3e8dc Vision
% Detection Generator> block and ground-truth actor or vehicle poses from
% the <docid:driving_ref#mw_0abe0f52-f25a-4829-babb-d9bafe8fdbf3 Scenario
% Reader> to perform trajectory planning for the automated lane change
% maneuver.
%
%% Model Lane Change Planner
% The *Lane Change Planner* reference model uses a *Simple Behavior Layer*
% to configure the *Motion Planner*. *Motion Planner* is responsible for
% generating the trajectory for a lane change maneuver. Open the *Lane
% Change Planner* reference model.
open_system('LaneChangePlanner')

%% 
% # The *Find MIOs* subsystem finds the most important objects with respect
% to the current state of the ego vehicle.
% # The *Simple Behavior Layer* subsystem specifies the planner behavior
% for the Motion planner.
% # The *Motion Planner* reference model uses
% <docid:nav_ref#object_trajectoryoptimalfrenet trajectoryOptimalFrenet>
% and MIO information to perform trajectory planning.

%% 
% The *Find MIOs* subsystem uses ground-truth vehicle poses to compute MIO
% information with respect to the ego vehicle. The vehicles present in the
% front or rear of the ego vehicle are considered as MIOs. The MIOs can
% also be in adjacent lanes as shown in the following figure.
%
% <<../MIOs.png>>
%
% The *Simple Behavior Layer* subsystem configures planner behavior by
% computing the terminal states, weights, and deviation values using the
% current ego vehicle state information. Open the *Simple Behavior Layer*
% subsystem.
open_system('LaneChangePlanner/Simple Behavior Layer')

%%  
% * The *EstimateCurrentEgoState* block uses a System object&trade;,
% |HelperEstimateCurrentEgoState|, to compute the lateral and longitudinal
% positions of the ego vehicle. This computation is done in Frenet space
% using the current state of the ego vehicle and lane detections. It also
% computes information about adjacent lane availability (NoLeftLane,
% NoRightLane) with respect to ego vehicle position and deviation offset
% from the reference path.
%
% * The *LongitudinalSampler* block defines the longitudinal planning
% horizon, as shown in the following figure. It configures the possible
% longitudinal terminal state for defining the planner behavior based on
% the distance to the input longitudinal position. The input longitudinal
% position is calculated based on the distance to the goal point.
%
% <<../LongutidinalTerminalState.gif>>
%% 
% * The *LateralSampler* block defines the lateral planning horizon, as
% shown in the following figure. It configures the possible lateral
% terminal states for defining the planner behavior based on adjacent lane
% availability (NoLeftLane, NoRightLane).
%
% <<../LateralTerminalStates.gif>>
%% 
% * The *VelocitySampler* block configures the possible velocity terminal
% states for defining the planner behavior. It is set to a default
% configuration in this model.
%
% * The *AccelerationSampler* block configures the possible acceleration
% terminal states for defining the planner behavior. It is set to a default
% configuration in this model.
%
% * The *Weights* bus defines the attributes that can be configured to
% vary the trajectory profile generated by the planner. These are set to
% default values in this model.
%
% The *Motion Planner* reference model generates a trajectory by using the
% planner behavior, scenario information, and MIO information. Open the
% *Motion Planner* reference model.
%
open_system('MotionPlanner')

%% 
% * The *Pulse Generator* block defines a re-plan period for the *Trajectory
% Generator* block. The default value is set to 1 second.
%
% * The *Trajectory Generator* block generates an optimal trajectory based
% on MIOs, the reference path, and the specified planner behavior. This
% block uses a system object, |HelperTrajectoryGenerator|, to generate the
% required trajectory. This System object is implemented using
% <docid:nav_ref#object_trajectoryoptimalfrenet trajectoryOptimalFrenet>.
% It uses MIO information updated using a state validator. Multiple
% trajectory samples are validated for dynamic collision using the state
% validator and the optimal trajectory is identified.
%
% <<../trajectoryGenerator.png>>
%
% * The *Extract States* block extracts path information from the
% generated trajectory.
%
% * The *Path Analyzer* block estimates the heading angle and finds the
% appropriate point on the path to follow. The generated path must conform
% to the road shape. This reference point on the path is used by the *Lane
% Change Controller* reference model.
%
% <<../pathAnalyzer.png>>
%

%% Model Lane Change Controller
% The *Lane Change Controller* reference model simulates a path following
% control mechanism that keeps the ego vehicle traveling along the
% generated trajectory while tracking a set velocity. To do so, the
% controller adjusts both the longitudinal acceleration and front steering
% angle of the ego vehicle. The controller computes optimal control actions
% while satisfying velocity, acceleration, and steering angle constraints
% using adaptive model predictive control (MPC). Open the *Lane Change
% Controller* reference model.
open_system('LaneChangeController')

%%
% * The *Virtual Lane Center* subsystem creates a virtual lane from the
% path point. The virtual lane matches the format required by the *Path
% Following Controller* block.
% * The *Preview Curvature* subsystem converts trajectory to curvature
% input required by *Path Following Controller*.
% * The *Path Following Controller* block uses the
% <docid:mpc_ref#mw_78ac0ef3-0622-4d5f-907b-52f6c7bebff6 Path Following
% Control System> block from the Model Predictive Control Toolbox&trade;.
%% 
% The *Path Following Controller* block keeps the vehicle traveling within
% a marked lane of a highway while maintaining a user-set velocity. This
% controller includes combined longitudinal and lateral control of the ego
% vehicle:
%% 
% * Longitudinal control maintains a user-set velocity of the ego vehicle.
% * Lateral control keeps the ego vehicle traveling along the center line
% of its lane by adjusting the steering of the ego vehicle.
%
%% Explore Metrics Assessment
% The *Metrics Assessment* subsystem assesses system level behavior of the
% LCM system using the metrics mentioned below. Open the *Metrics
% Assessment* subsystem.
open_system('HighwayLaneChangeTestBench/Metrics Assessment')

%% 
% * The *DetectCollision* block detects the collision of the ego vehicle
% with other vehicles and halts the simulation if a collision is detected.
% * The *DetectLeadVehicle* block computes the headway between the ego and
% lead vehicles, which is used for computing the *TimeGap*.
% * The *TimeGap* is calculated using the distance to the lead vehicle
% (headway) and the longitudinal velocity of the ego vehicle, and it is
% evaluated against prescribed limits.
% * The *LongitudinalJerk* is calculated using the longitudinal velocity
% and evaluated against prescribed limits.
% * The *LateralJerk* value is calculated using the lateral velocity
% evaluated against prescribed limits.
%
%% Simulate and Visualize System Behavior
% Set up and run the |HighwayLaneChangeTestBench| simulation model to
% visualize the behavior of the system during a lane change.
% The *Visualization* block in the model creates a bird's eye plot that
% displays the lane information, ego vehicle, ego trajectory, and other
% vehicles in the scenario. Configure the |HighwayLaneChangeTestBench|
% model to use |scenario_LC_06_DoubleLaneChange| scenario.
helperSLHighwayLaneChangeSetup("scenario_LC_06_DoubleLaneChange")

%%
% Simulate the model for 5 seconds. To reduce command-window output, first
% turn off the MPC update messages.
mpcverbosity('off');
sim("HighwayLaneChangeTestBench","StopTime","5");

%% 
% Run the simulation for 8 seconds. A trajectory is calculated to navigate
% around a slower lead vehicle.
sim("HighwayLaneChangeTestBench","StopTime","8");

%% 
% Run the simulation for 13 seconds. The vehicle continues straight ahead
% in the left lane.
sim("HighwayLaneChangeTestBench","StopTime","13");

%% Explore Other Scenarios
% In the previous section, you explored the system behavior for the
% |scenario_LC_06_DoubleLaneChange| scenario. Below is a list of scenarios
% that are compatible with the |HighwayLaneChangeTestBench| model.
%
%    scenario_LC_01_SlowMoving
%    scenario_LC_02_SlowMovingWithPassingCar
%    scenario_LC_03_DisabledCar
%    scenario_LC_04_CutInWithBrake
%    scenario_LC_05_SingleLaneChange
%    scenario_LC_06_DoubleLaneChange [Default]
%    scenario_LC_07_RightLaneChange
%    scenario_LC_08_SlowmovingCar_Curved
%    scenario_LC_09_CutInWithBreak_Curved
%    scenario_LC_10_SingleLaneChange_Curved
%
% These scenarios are created using the
% <docid:driving_ref#mw_07e6310f-b9c9-4f4c-b2f9-51e31d407766 Driving
% Scenario Designer> and are exported to a scenario file. Examine the
% comments in each file for more details on the road and vehicles in each
% scenario. You can configure the |HighwayLaneChangeTestBench| and
% workspace to simulate these scenarios using the
% |helperSLHighwayLaneChangeSetup| function. For example, you can configure
% the simulation for a curved road scenario.
helperSLHighwayLaneChangeSetup("scenario_LC_10_SingleLaneChange_Curved")

%% Conclusion
% This example shows how to simulate a highway lane change maneuver using
% ideal vehicle positions and lane detections.

%%
% Enable the MPC update messages again.
mpcverbosity('on');

%%
% Copyright 2019-2020 The MathWorks, Inc.
