jaco_angles_action.cpp

Go to the documentation of this file.

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

namespace jaco
{

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

JacoAnglesActionServer::~JacoAnglesActionServer()
{

}

void JacoAnglesActionServer::ActionCallback(const jaco_driver::ArmJointAnglesGoalConstPtr &goal)
{
        jaco_driver::ArmJointAnglesFeedback feedback;
        jaco_driver::ArmJointAnglesResult result;

        ROS_INFO("Got an angular goal for the arm");

        JacoAngles cur_position;                //holds the current position of the arm

        if (arm.Stopped())
        {
                arm.GetAngles(cur_position);
                result.angles = cur_position.Angles();

                as_.setAborted(result);
                return;
        }

        JacoAngles target(goal->angles);
        arm.SetAngles(target);

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

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

                arm.GetAngles(cur_position);
                feedback.angles = cur_position.Angles();

                if (arm.Stopped())
                {
                        result.angles = cur_position.Angles();
                        as_.setAborted(result);
                        return;
                }

                as_.publishFeedback(feedback);

                if (target.Compare(cur_position, tolerance))
                {
                        ROS_INFO("Angular Control Complete.");

                        result.angles = cur_position.Angles();
                        as_.setSucceeded(result);
                        return;
                }

                r.sleep();
        }
}

}