%% Optimal Trajectory Planning in Frenet Space
% This example shows how to plan an optimal trajectory using a trajectoryOptimalFrenet object.
%% Create and Assign Map to State Validator
% Create a state validator object for collision checking.

stateValidator = validatorOccupancyMap;

% Create an obstacle grid map.

grid = zeros(50,100);
grid(24:26,48:53) = 1;

% Create a binaryOccupancyMap with the grid map.

map = binaryOccupancyMap(grid);

% Assign the map to the state validator.

stateValidator.Map = map;

%% Plan and Visualize Trajectory
% Create a reference path for the planner to follow.

refPath = [0,25;100,25];

% Declare the cost function handle to prioritize left lane changes.

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

% Initialize the planner object with the reference path, the state validator, and the custom cost function.

planner = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

% Assign longitudinal terminal state, lateral deviation, and maximum acceleration values.

planner.TerminalStates.Longitudinal = 100;
planner.TerminalStates.Lateral = -10:10:10;
planner.FeasibilityParameters.MaxAcceleration = 10;

% Trajectory Planning
% Initial Frenet state of vehicle.

initFrenetState = zeros(1,6);

% Plan a trajectory from initial Frenet state.

plan(planner,initFrenetState);

% Trajectory Visualization
% Visualize the map and the trajectories.

show(map)
hold on
show(planner,'Trajectory','all')

% Copyright 2019 The MathWorks, Inc.