%% Vehicle Path Planning and Metrics Computation in a 2-D Costmap Environment
% Plan a vehicle path through a parking lot using the RRT* algorithm. Compute and visualize the smoothness, clearance, and validity of the planned path.
%
%% Load and Assign Map to State Validator
% Load a costmap of a parking lot. Plot the costmap to see the parking lot and the inflated areas that the vehicle should avoid.

load parkingLotCostmap.mat;
costmap = parkingLotCostmap;
plot(costmap)
xlabel('X (meters)')
ylabel('Y (meters)')

% Create a stateSpaceDubins object and increase the minimum turing radius to 4 meters.

statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % meters

% Create a validatorVehicleCostmap object using the created state space.

statevalidator = validatorVehicleCostmap(statespace);

% Assign the parking lot costmap to the state validator object.

statevalidator.Map = costmap;

%
%% Plan Path
% 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 = [5, 5, 90]; % [meters, meters, degrees]
goalPose = [40, 38, 180]; % [meters, meters, degrees]

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

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

% Interpolate along the path at every one meter. Convert the orientation angles from degrees to radians.

poses = zeros(size(refPath.PathSegments,2)+1,3);
poses(1,:) = refPath.StartPose;
for i = 1:size(refPath.PathSegments,2) 
    poses(i+1,:) = refPath.PathSegments(i).GoalPose; 
end
poses(:,3) = deg2rad(poses(:,3));

% Create a navPath object using the Dubins state space object and the states specified by poses.

path = navPath(statespace,poses);

%
%% Compute and Visualize Path Metrics
% Create a pathmetrics object.

pathMetricsObj = pathmetrics(path,statevalidator);

% Check path validity. The result is 1 (true) if the planned path is obstacle free. 0 (false) indicates an invalid path.

isPathValid(pathMetricsObj)

% Compute and visualize the minimum clearance of the path.

clearance(pathMetricsObj)
show(pathMetricsObj)
legend('Inflated Areas','Planned Path','Minimum Clearance')
xlabel('X (meters)')
ylabel('Y (meters)')

% Compute and visualize the smoothness of the path. Values close to 0 indicate a smoother path. Straight-line paths return a value of 0.

smoothness(pathMetricsObj)
show(pathMetricsObj,'Metrics',{'Smoothness'})
legend('Inflated Areas','Path Smoothness')
xlabel('X (meters)')
ylabel('Y (meters)')

% Visualize the clearance for each state of the path.

show(pathMetricsObj,'Metrics',{'StatesClearance'})
legend('Inflated Areas','Planned Path','Clearance of Path States')
xlabel('X (meters)')
ylabel('Y (meters)')

% Copyright 2019 The MathWorks, Inc.