function exampleHelperSendPR2GripperCommand(side,position,maxEffort,wait)
%exampleHelperSendPR2GripperCommand Control PR2 gripper 
%   Input arguments: 
%   - side
%   - gripper position
%   - max effort 
%   - a Boolean flag that decides whether to wait for motion complete

% Copyright 2016 The MathWorks, Inc.

validateattributes(side,{'char'},{});
side = validatestring(side, {'left','right'});
if nargin < 4
    wait = false;
end
if strcmp(side,'left')
    channel = '/l_gripper_controller/command';
    subChannel = '/l_gripper_controller/state';
else
    channel = '/r_gripper_controller/command';
    subChannel = '/r_gripper_controller/state';
end
if wait
    sub = rossubscriber(subChannel);
end
msg = rosmessage('pr2_controllers_msgs/Pr2GripperCommand');
msg.Position = position;
msg.MaxEffort = maxEffort;
rospublisher(channel,msg);

if wait
    waiting = true;
    count = 0;
    while waiting
        if abs(sub.receive.ProcessValueDot) < 1e-3
            count = count + 1;
        end
        pause(0.1);
        waiting = count < 10;
    end
end

