jaco_fingers_action.cpp

Go to the documentation of this file.

#include "jaco_driver/jaco_fingers_action.h"
#include <jaco_driver/KinovaTypes.h>
#include "jaco_driver/jaco_types.h"

namespace jaco
{

JacoFingersActionServer::JacoFingersActionServer(JacoComm &arm_comm, ros::NodeHandle &n) : 
    arm(arm_comm), 
    as_(n, "finger_joint_angles", boost::bind(&JacoFingersActionServer::ActionCallback, this, _1), false)
{
    as_.start();
}

JacoFingersActionServer::~JacoFingersActionServer()
{

}

void JacoFingersActionServer::ActionCallback(const jaco_driver::SetFingersPositionGoalConstPtr &goal)
{
        jaco_driver::SetFingersPositionFeedback feedback;
        jaco_driver::SetFingersPositionResult result;

        ROS_INFO("Got a finger goal for the arm");

        FingerAngles fingers;           //holds the current position of the fingers

        if (arm.Stopped())
        {
                arm.GetFingers(fingers);
                result.fingers = fingers.Fingers();
                as_.setAborted(result);
                return;
        }

        FingerAngles target(goal->fingers);
        arm.SetFingers(target);

        ros::Rate r(10);
 
        const float tolerance = 2.0;    //dead zone for position


        //while we have not timed out
        while (true)
        {
                ros::spinOnce();
                if (as_.isPreemptRequested() || !ros::ok())
                {
                        arm.Stop();
                        arm.Start();
                        as_.setPreempted();
                        return;
                }

                arm.GetFingers(fingers);
                feedback.fingers = fingers.Fingers();

                if (arm.Stopped())
                {
                        result.fingers = fingers.Fingers();
                        as_.setAborted(result);
                        return;
                }

                as_.publishFeedback(feedback);

                if (target.Compare(fingers, tolerance))
                {
                        ROS_INFO("Finger Control Complete.");

                        result.fingers = fingers.Fingers();
                        as_.setSucceeded(result);
                        return;
                }

                r.sleep();
        }
}

}