%% Plan Path Using Reeds-Shepp 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 Reeds-Shepp 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 = [48.75, 29.75, 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 'Reeds-Shepp'.

planner = pathPlannerRRT(costmap);
planner.ConnectionMethod = 'Reeds-Shepp';
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 requires at least one reverse motion before sliding into the parking spot at the goal position. In such scenarios where a reverse motion is required to reach the goal pose, Reeds-Shepp path segments are useful as Dubins path segments do not allow reverse motion.
%
% Copyright 2021 The MathWorks, Inc.
%