%% Plan Path and Interpolate Along Path
% Plan a vehicle path through a parking lot by using the rapidly exploring random tree (RRT*) algorithm. Interpolate the poses of the vehicle at points along the path.
% Load a costmap of a parking lot. Plot the costmap to see the parking lot and inflated areas for the vehicle to avoid.

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);

%
% Interpolate the vehicle poses every 1 meter along the entire path.

lengths = 0 : 1 : refPath.Length;
poses = interpolate(refPath,lengths);

%
% Plot the interpolated poses on the costmap.

plot(costmap)
hold on
scatter(poses(:,1),poses(:,2),'DisplayName','Interpolated Poses')
hold off

% Copyright 2018 The MathWorks, Inc.