dynamixel_gripper_driver_node.cpp

Go to the documentation of this file.

#include "dynamixel_gripper_driver_node.h"

DynamixelGripperDriverNode::DynamixelGripperDriverNode(ros::NodeHandle &nh) : 
    iri_base_driver::IriBaseNodeDriver<DynamixelGripperDriver>(nh),
    close_action__aserver_(public_node_handle_, "close_action_"),
    open_action__aserver_(public_node_handle_, "open_action_"),
    max_torque_it_(0),
    last_action_successful_(false)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  //
  // [init publishers]
  // [init subscribers]
  // [init services]
  // [init clients]
  // [init action servers]

    /* Close action IRI Action server */
    close_action__aserver_.registerStartCallback(boost::bind(&DynamixelGripperDriverNode::close_action_StartCallback, this, _1)); 
    close_action__aserver_.registerStopCallback(boost::bind(&DynamixelGripperDriverNode::close_action_StopCallback, this)); 
    close_action__aserver_.registerIsFinishedCallback(boost::bind(&DynamixelGripperDriverNode::close_action_IsFinishedCallback, this)); 
    close_action__aserver_.registerHasSucceedCallback(boost::bind(&DynamixelGripperDriverNode::close_action_HasSucceedCallback, this)); 
    close_action__aserver_.registerGetResultCallback(boost::bind(&DynamixelGripperDriverNode::close_action_GetResultCallback, this, _1)); 
    close_action__aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelGripperDriverNode::close_action_GetFeedbackCallback, this, _1));

    close_action__aserver_.start();

    /* Open action IRI action server */
    /* Some functions can be reused from close_action: Stop. Other can not be
     * shared since PtrParams are from different type */
    open_action__aserver_.registerStartCallback(boost::bind(&DynamixelGripperDriverNode::open_action_StartCallback, this, _1));
    open_action__aserver_.registerStopCallback(boost::bind(&DynamixelGripperDriverNode::close_action_StopCallback, this));
    open_action__aserver_.registerIsFinishedCallback(boost::bind(&DynamixelGripperDriverNode::open_action_IsFinishedCallback, this));
    open_action__aserver_.registerHasSucceedCallback(boost::bind(&DynamixelGripperDriverNode::close_action_HasSucceedCallback, this));
    open_action__aserver_.registerGetResultCallback(boost::bind(&DynamixelGripperDriverNode::open_action_GetResultCallback, this, _1));
    open_action__aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelGripperDriverNode::open_action_GetFeedbackCallback, this, _1));

    open_action__aserver_.start();
}

void
DynamixelGripperDriverNode::mainNodeThread(void)
{ }

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void
DynamixelGripperDriverNode::open_action_StartCallback(const iri_dynamixel_gripper::openGoalConstPtr& goal)
{
    driver_.lock();

    try{
    last_action_successful_ = true;

    if (! driver_.open_gripper()) {
        ROS_ERROR("Unable to open gripper");
        last_action_successful_ = false;
    }
    }catch(...){
        ROS_WARN("Uncatched exception");
    }

    driver_.unlock(); 
}

bool
DynamixelGripperDriverNode::open_action_IsFinishedCallback()
{
    driver_.lock();

    bool ret = false;

    if (! driver_.is_moving())
        ret = true;

    driver_.unlock();
    return ret;
}

void
DynamixelGripperDriverNode::open_action_GetResultCallback(iri_dynamixel_gripper::openResultPtr& result) 
{
    result->successful = last_action_successful_;
}

void
DynamixelGripperDriverNode::open_action_GetFeedbackCallback(iri_dynamixel_gripper::openFeedbackPtr& feedback) 
{
    driver_.lock();

    try{
    feedback->angle  = driver_.get_angle();
    feedback->effort = driver_.get_effort();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }
 
    driver_.unlock();
}


void
DynamixelGripperDriverNode::close_action_StartCallback(const iri_dynamixel_gripper::closeGoalConstPtr& goal)
{
    driver_.lock();

    try{
    max_torque_it_ = 0;
    last_action_successful_ = true;

    if (! driver_.close_gripper()) {
        ROS_ERROR("Unable to close gripper");
        last_action_successful_ = false;
    }
    }catch(...){
        ROS_WARN("Uncatched exception"); 
    }

    driver_.unlock(); 
}

void
DynamixelGripperDriverNode::close_action_StopCallback(void)
{
    driver_.lock();
    try{
    driver_.stop_gripper();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock();
}

bool
DynamixelGripperDriverNode::close_action_IsFinishedCallback(void) 
{
    driver_.lock();

    if (! driver_.is_moving()) {
      driver_.unlock();
      return true;
    }

    try{
    float current_effort = driver_.get_effort();

    if (fabs(current_effort) > (0.9 * driver_.get_max_effort())) {
        max_torque_it_++;

        if (max_torque_it_ < 5) {
            driver_.unlock();
            return false;
        }

        /* Commenting the following line will make the gripper to apply 
         * continuos force while closing */
        // driver_.stop_gripper();
        driver_.unlock();
        return true;
    }
    }catch(...){
        ROS_WARN("Uncatched exception");
    }

    driver_.unlock();
    return false;
}

bool
DynamixelGripperDriverNode::close_action_HasSucceedCallback(void) 
{
    return true;
}

void
DynamixelGripperDriverNode::close_action_GetResultCallback(iri_dynamixel_gripper::closeResultPtr& result) 
{
    result->successful = last_action_successful_;
}

void
DynamixelGripperDriverNode::close_action_GetFeedbackCallback(iri_dynamixel_gripper::closeFeedbackPtr& feedback) 
{
    driver_.lock();

    try{
    feedback->angle  = driver_.get_angle();
    feedback->effort = driver_.get_effort();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }

    driver_.unlock();
}

/*  [action requests] */

void DynamixelGripperDriverNode::postNodeOpenHook(void)
{
}

void DynamixelGripperDriverNode::addNodeDiagnostics(void)
{
}

void DynamixelGripperDriverNode::addNodeOpenedTests(void)
{
}

void DynamixelGripperDriverNode::addNodeStoppedTests(void)
{
}

void DynamixelGripperDriverNode::addNodeRunningTests(void)
{
}

void DynamixelGripperDriverNode::reconfigureNodeHook(int level)
{
}

DynamixelGripperDriverNode::~DynamixelGripperDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<DynamixelGripperDriverNode>(argc, argv, "dynamixel_gripper_driver_node");
}