%% Interpolate Poses For Dubins Path
% Create a dubinsConnection object.

dubConnObj = dubinsConnection;

%
% Define start and goal poses as [x y theta] vectors.

startPose = [0 0 0];
goalPose = [1 1 pi];

%
% Calculate a valid path segment to connect the poses.

[pathSegObj,pathCosts] = connect(dubConnObj,startPose,goalPose);

%
% Show the generated path.

show(pathSegObj{1})

%
% Interpolate poses along the path. Get a pose every 0.2 meters, including the transitions between turns.

length = pathSegObj{1}.Length;
poses = interpolate(pathSegObj{1},0:0.2:length)

%
% Use the quiver function to plot these poses.

quiver(poses(:,1),poses(:,2),cos(poses(:,3)),sin(poses(:,3)),0.5)

% Copyright 2019 The MathWorks, Inc.