function exampleHelperROS2ValetControlCallback(msg, control, valet)
% exampleHelperROS2ValetControlCallback 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.ros2message;
    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 = ros2message(control.AccelPub);
    accelMsg.data = accelCmd;
    send(control.AccelPub, accelMsg);
    decelMsg = ros2message(control.DecelPub);
    decelMsg.data = decelCmd;
    send(control.DecelPub, decelMsg);
    
    steeringMsg = ros2message(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 = ros2message(control.VehGoalReachPub);
        reachMsg.data = true;
        send(control.VehGoalReachPub, reachMsg);
    end
end
clear('control');
end