jaco_angles_action.cpp

Go to the documentation of this file.

#include <kinova/KinovaTypes.h>

#include "jaco_driver/jaco_angles_action.h"

#include "jaco_driver/jaco_types.h"


namespace jaco
{

JacoAnglesActionServer::JacoAnglesActionServer(JacoComm &arm_comm, const ros::NodeHandle &nh)
    : arm_comm_(arm_comm),
      node_handle_(nh, "joint_angles"),
      action_server_(node_handle_, "arm_joint_angles",
                     boost::bind(&JacoAnglesActionServer::actionCallback, this, _1), false)
{
    double tolerance;
    node_handle_.param<double>("stall_interval_seconds", stall_interval_seconds_, 0.5);
    node_handle_.param<double>("stall_threshold", stall_threshold_, 1.0);
    node_handle_.param<double>("rate_hz", rate_hz_, 10.0);
    node_handle_.param<double>("tolerance", tolerance, 2.0);
    tolerance_ = (float)tolerance;

    action_server_.start();
}


JacoAnglesActionServer::~JacoAnglesActionServer()
{
}


void JacoAnglesActionServer::actionCallback(const jaco_msgs::ArmJointAnglesGoalConstPtr &goal)
{
    jaco_msgs::ArmJointAnglesFeedback feedback;
    jaco_msgs::ArmJointAnglesResult result;
    JacoAngles current_joint_angles;
    ros::Time current_time = ros::Time::now();

    try
    {
        arm_comm_.getJointAngles(current_joint_angles);

        if (arm_comm_.isStopped())
        {
            ROS_INFO("Could not complete joint angle action because the arm is 'stopped'.");
            result.angles = current_joint_angles.constructAnglesMsg();
            action_server_.setAborted(result);
            return;
        }

        last_nonstall_time_ = current_time;
        last_nonstall_angles_ = current_joint_angles;

        JacoAngles target(goal->angles);
        arm_comm_.setJointAngles(target);

        // Loop until the action completed, is preempted, or fails in some way.
        // timeout is left to the caller since the timeout may greatly depend on
        // the context of the movement.
        while (true)
        {
            ros::spinOnce();

            if (action_server_.isPreemptRequested() || !ros::ok())
            {
                result.angles = current_joint_angles.constructAnglesMsg();
                arm_comm_.stopAPI();
                arm_comm_.startAPI();
                action_server_.setPreempted(result);
                return;
            }
            else if (arm_comm_.isStopped())
            {
                result.angles = current_joint_angles.constructAnglesMsg();
                action_server_.setAborted(result);
                return;
            }

            arm_comm_.getJointAngles(current_joint_angles);
            current_time = ros::Time::now();
            feedback.angles = current_joint_angles.constructAnglesMsg();
            action_server_.publishFeedback(feedback);

            if (target.isCloseToOther(current_joint_angles, tolerance_))
            {
                // Check if the action has succeeeded
                result.angles = current_joint_angles.constructAnglesMsg();
                action_server_.setSucceeded(result);
                return;
            }
            else if (!last_nonstall_angles_.isCloseToOther(current_joint_angles, stall_threshold_))
            {
                // Check if we are outside of a potential stall condition
                last_nonstall_time_ = current_time;
                last_nonstall_angles_ = current_joint_angles;
            }
            else if ((current_time - last_nonstall_time_).toSec() > stall_interval_seconds_)
            {
                // Check if the full stall condition has been meet
                result.angles = current_joint_angles.constructAnglesMsg();
                arm_comm_.stopAPI();
                arm_comm_.startAPI();
                action_server_.setPreempted(result);
                return;
            }

            ros::Rate(rate_hz_).sleep();
        }
    }
    catch(const std::exception& e)
    {
        result.angles = current_joint_angles.constructAnglesMsg();
        ROS_ERROR_STREAM(e.what());
        action_server_.setAborted(result);
    }
}

}  // namespace jaco