%% 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 and the state bounds to the state validator.

stateValidator.Map = map;
stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];

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

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

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

planner = trajectoryOptimalFrenet(refPath,stateValidator);

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

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

% Specify the deviation offset value close to the left lateral terminal state to prioritize left lane changes.

planner.DeviationOffset = 5;

% Trajectory Planning
% Initial cartesian state of vehicle.

initCartState = [0 25 pi/9 0 0 0];

% Convert cartesian state of vehicle to Frenet state.

initFrenetState = cart2frenet(planner,initCartState);

% 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.