%% Plan Path to Parking Spot
% Plan a vehicle path to a parking spot by using the RRT* algorithm.
%
% 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('parkingLotCostmapReducedInflation.mat');
costmap = data.parkingLotCostmapReducedInflation;
plot(costmap)

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

startPose = [11, 10, 0]; % [meters, meters, degrees]
goalPose  = [31.5, 17, 90];

%
% Create an RRT* path planner to plan a path from the start pose to the goal pose.

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

%
% Plot the planned path.

plot(planner)