function exampleHelperROS2ValetParkManeuver(msg, planning, valet)
%% Parking Maneuver
% Now that the vehicle is near the parking spot, a specialized parking
% maneuver is used to park the vehicle in the final parking spot. This
% maneuver requires passing through a narrow corridor flanked by the edges
% of the parking spot on both ends. Such a maneuver is typically
% accompanied with ultrasound sensors or laser scanners continuously
% checking for obstacles.
if msg.data
    return;
end

currentVelMsg = planning.CurVelSub.LatestMessage;
currentVel = currentVelMsg.data;
    
valet.ccConfig = valet.costmap.CollisionChecker;
valet.ccConfig.NumCircles = 4;
valet.costmap.CollisionChecker = valet.ccConfig;

% Set up the pathPlannerRRT to use the updated costmap
valet.parkMotionPlanner = pathPlannerRRT(valet.costmap, 'MinIterations', 1000);

valet.preParkPose = [planning.CurPoseSub.LatestMessage.x, ...
    planning.CurPoseSub.LatestMessage.y, ...
    planning.CurPoseSub.LatestMessage.theta];

% Compute the required parking maneuver
refPath = plan(valet.parkMotionPlanner, valet.preParkPose, valet.parkPose);

%keyboard;
%%
% Once the maneuver is found, repeat the previous process to determine a
% complete plan: smooth the path, generate a speed profile and follow the
% trajectory using the feedback controller.

% Retrieve transition poses and directions from the planned path 
[transitionPoses, directions] = interpolate(refPath);
    
% Smooth the path
numSmoothPoses   = round(refPath.Length / valet.approxSeparation);
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);
smoothpathMsg = planning.PathPub.ros2message;
smoothpathMsg.data = reshape(refPoses, 1, numel(refPoses));
planning.PathPub.send(smoothpathMsg);

directionsMsg = ros2message(planning.DirPub);
directionsMsg.data = directions;
send(planning.DirPub, directionsMsg);

% Set up the velocity profile generator to stop at the end of the trajectory,
% with a speed limit of 5 mph
refVelocities = exampleHelperROSValetGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 2.2352);
velprofileMsg = ros2message(planning.VelPub);
velprofileMsg.data = refVelocities;
send(planning.VelPub, velprofileMsg);
    
end