%% Plan Path Using Dubins Path Segments
% Plan a vehicle path through a parking lot by using the optimal rapidly exploring random tree (RRT*) algorithm. The planned path is composed of a sequence of Dubins path segments. Check that the path is valid, and then plot the transition poses 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('parkingLotCostmapReducedInflation.mat');
costmap = data.parkingLotCostmapReducedInflation;
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 = [53.3, 19, 90];

%
% Use a pathPlannerRRT object to plan a path from the start pose to the goal pose. Set the ConnectionMethod property of the pathPlannerRRT object to 'Dubins'.

planner = pathPlannerRRT(costmap);
planner.ConnectionMethod = 'Dubins';
refPath = plan(planner,startPose,goalPose);

%
% Check that the path is valid.

isPathValid = checkPathValidity(refPath,costmap)

%
%  Interpolate the transition poses along the path.

transitionPoses = interpolate(refPath);

%
% Plot the planned path and the transition poses on the costmap.

hold on
plot(refPath,'DisplayName','Planned Path')
scatter(transitionPoses(:,1),transitionPoses(:,2),[],'filled', ...
   'DisplayName','Transition Poses')
hold off

% Notice that the path from the start pose to the goal pose does not require a reverse motion. Hence, the planned path is valid. In scenarios where a reverse motion is required to reach the goal pose, use Reeds-Shepp path segments as Dubins path segments do not allow reverse motion.
%
%
%
% Copyright 2021 The MathWorks, Inc.
%