%% Smooth a Planned Path
% Smooth a path that was planned by an RRT* path planner.
% Load and plot a costmap of a parking lot.

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

%
% Define start and goal poses for the vehicle as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the Θ orientation angles are in degrees.

startPose = [4,4,90]; % [meters, meters, degrees]
goalPose = [30,13,0];

%
% Use a pathPlannerRRT object to plan a path from the start pose to the goal pose.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

%
% Plot and zoom in on the planned path. The path is composed of a sequence of Dubins curves. These curves include abrupt changes in curvature that are not suitable for driving with passengers.

hold on
plot(refPath,'Vehicle','off','DisplayName','Reference path')
xlim([3 31])
ylim([3 18])

%
% Interpolate the transition poses of the path. Use these poses as the reference poses for interpolating the smooth path. Also return the motion directions at each pose.

[refPoses,refDirections] = interpolate(refPath);

%
% Specify the number of poses to return in the smooth path. Return poses spaced about 0.1 meters apart, along the entire length of the path.

approxSeparation = 0.1; % meters
numSmoothPoses = round(refPath.Length / approxSeparation);

%
% Generate the smooth path by fitting a cubic spline to the reference poses. smoothPathSpline returns the specified number of discretized poses along the smooth path.

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses);

%
% Plot the smooth path. The more abrupt changes in curvature that were present in the reference path are now smoothed out.

plot(poses(:,1),poses(:,2),'LineWidth',2,'DisplayName','Smooth path')
hold off

% Copyright 2018 The MathWorks, Inc.