function exampleHelperROSValetControlCallback(msg, control, valet)
% exampleHelperROSValetControlCallback callback for control

% Copyright 2019 The MathWorks, Inc.
refVelocities = msg.Data;

smoothPathMsg = control.PathSub.LatestMessage;
refPoses = reshape(smoothPathMsg.Data, numel(smoothPathMsg.Data)/3,3);

directionsMsg = control.DirSub.LatestMessage;
directions = directionsMsg.Data;

speedMsg = control.SpeedSub.LatestMessage;
speedConfig.StartSpeed = speedMsg.Data(1);
speedConfig.EndSpeed = speedMsg.Data(2);
speedConfig.MaxSpeed = speedMsg.Data(3);

% Configure path analyzer
valet.pathAnalyzer.RefPoses     = refPoses;
valet.pathAnalyzer.Directions   = directions;
valet.pathAnalyzer.VelocityProfile = refVelocities;

% Reset longitudinal controller
reset(valet.lonController);

reachGoal = false;

nextGoalMsg = control.NextGoalSub.LatestMessage;
nextGoal(1) = nextGoalMsg.X;
nextGoal(2) = nextGoalMsg.Y;
nextGoal(3) = nextGoalMsg.Theta;

% Execute control loop
while ~reachGoal
    curPoseMsg = control.CurPoseSub.LatestMessage;
    currentPose = [curPoseMsg.X, curPoseMsg.Y, curPoseMsg.Theta];
    
    currentVelMsg = control.CurVelSub.LatestMessage;
    currentVel = currentVelMsg.Data;
    
    % Find the reference pose on the path and the corresponding velocity
    [refPose, refVel, direction] = valet.pathAnalyzer(currentPose, currentVel);
    dirMsg = control.VehDirPub.rosmessage;
    dirMsg.Data = direction;
    send(control.VehDirPub, dirMsg);
    
    % Compute steering command
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        'Direction', direction, 'Wheelbase', valet.vehicleDims.Wheelbase);
    
    % Compute acceleration and deceleration commands
    valet.lonController.Direction = direction;
    [accelCmd, decelCmd] = valet.lonController(refVel, currentVel);
    
    accelMsg = rosmessage(control.AccelPub);
    accelMsg.Data = accelCmd;
    send(control.AccelPub, accelMsg);
    decelMsg = rosmessage(control.DecelPub);
    decelMsg.Data = decelCmd;
    send(control.DecelPub, decelMsg);
    
    steeringMsg = rosmessage(control.SteeringPub);
    steeringMsg.Data = steeringAngle;
    send(control.SteeringPub, steeringMsg);
    
    % Check if the vehicle reaches the goal
    reachGoal = exampleHelperROSValetGoalChecker(nextGoal, currentPose, currentVel, speedConfig.EndSpeed, direction);
    
    % Wait for fixed-rate execution
    waitfor(valet.controlRate);
    
    if reachGoal
        reachMsg = rosmessage(control.VehGoalReachPub);
        reachMsg.Data = true;
        send(control.VehGoalReachPub, reachMsg);
    end
end
clear('control');
end