dynamixel_nc_gripper_driver_node.cpp

Go to the documentation of this file.

#include "dynamixel_nc_gripper_driver_node.h"

DynamixelNcGripperDriverNode::DynamixelNcGripperDriverNode(ros::NodeHandle &nh) : 
    iri_base_driver::IriBaseNodeDriver<DynamixelNcGripperDriver>(nh),
    is_opened_(false),
    is_closed_(false),
    tool_open_action_aserver_(public_node_handle_, "tool_open_action"),
    tool_close_action_aserver_(public_node_handle_, "tool_close_action")
{
    //init class attributes if necessary
    //this->loop_rate_ = 2;//in [Hz]

    // [init publishers]

    // [init subscribers]

    // [init services]

    // [init clients]

    // [init action servers]
    tool_open_action_aserver_.registerStartCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionStartCallback, this, _1)); 
    tool_open_action_aserver_.registerStopCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionStopCallback, this)); 
    tool_open_action_aserver_.registerIsFinishedCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionIsFinishedCallback, this)); 
    tool_open_action_aserver_.registerHasSucceedCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionHasSucceedCallback, this)); 
    tool_open_action_aserver_.registerGetResultCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionGetResultCallback, this, _1)); 
    tool_open_action_aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_open_actionGetFeedbackCallback, this, _1)); 
    tool_open_action_aserver_.start();

    tool_close_action_aserver_.registerStartCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionStartCallback, this, _1)); 
    tool_close_action_aserver_.registerStopCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionStopCallback, this)); 
    tool_close_action_aserver_.registerIsFinishedCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionIsFinishedCallback, this)); 
    tool_close_action_aserver_.registerHasSucceedCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionHasSucceedCallback, this)); 
    tool_close_action_aserver_.registerGetResultCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionGetResultCallback, this, _1)); 
    tool_close_action_aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelNcGripperDriverNode::tool_close_actionGetFeedbackCallback, this, _1)); 
    tool_close_action_aserver_.start();

    // [init action clients]
}

void DynamixelNcGripperDriverNode::mainNodeThread(void)
{
    //lock access to driver if necessary
    this->driver_.lock();

    // [fill msg Header if necessary]
    //<publisher_name>.header.stamp = ros::Time::now();
    //<publisher_name>.header.frame_id = "<publisher_topic_name>";

    // [fill msg structures]

    // [fill srv structure and make request to the server]

    // [fill action structure and make request to the action server]

    // [publish messages]

    //unlock access to driver if previously blocked
    this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void DynamixelNcGripperDriverNode::tool_open_actionStartCallback(const iri_common_drivers_msgs::tool_openGoalConstPtr& goal)
{ 
    driver_.lock(); 

    try
    {
        is_opened_ = true;

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

    driver_.unlock(); 
} 

void DynamixelNcGripperDriverNode::tool_open_actionStopCallback(void) 
{ 
    driver_.lock(); 
    //stop action 
    try{
        driver_.stop_gripper();
        driver_.disable_gripper();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock(); 
} 

bool DynamixelNcGripperDriverNode::tool_open_actionIsFinishedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 

    if (driver_.is_at_max_angle())
    {
        ret = true; 
    }

    driver_.unlock(); 

    return ret; 
} 

bool DynamixelNcGripperDriverNode::tool_open_actionHasSucceedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 
    //if goal was accomplished 
    ret = true; 
    driver_.unlock(); 

    return ret; 
} 

void DynamixelNcGripperDriverNode::tool_open_actionGetResultCallback(iri_common_drivers_msgs::tool_openResultPtr& result) 
{ 
    driver_.lock(); 
    //update result data to be sent to client 
    result->successful = is_opened_;
    driver_.unlock(); 
} 

void DynamixelNcGripperDriverNode::tool_open_actionGetFeedbackCallback(iri_common_drivers_msgs::tool_openFeedbackPtr& feedback) 
{ 
    driver_.lock(); 
    try
    {
        feedback->angle = driver_.get_current_angle();
        feedback->effort = driver_.get_current_effort();
        ROS_INFO("[angle]: %f, [effort]: %f", feedback->angle, feedback->effort); 
    }
    catch(...)
    {
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock(); 
}

void DynamixelNcGripperDriverNode::tool_close_actionStartCallback(const iri_common_drivers_msgs::tool_closeGoalConstPtr& goal)
{ 
    driver_.lock(); 

    try
    {
        is_closed_ = true;

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

    driver_.unlock(); 
} 

void DynamixelNcGripperDriverNode::tool_close_actionStopCallback(void) 
{ 
    driver_.lock(); 
    //stop action 
    try{
        driver_.stop_gripper();
        driver_.disable_gripper();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock(); 
} 

bool DynamixelNcGripperDriverNode::tool_close_actionIsFinishedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 

    if (driver_.is_at_min_angle())
    {
        ret = true; 
    }

    driver_.unlock(); 

    return ret; 
} 

bool DynamixelNcGripperDriverNode::tool_close_actionHasSucceedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 
    //if goal was accomplished 
    driver_.disable_gripper(); 
    ret = true; 
    driver_.unlock(); 

    return ret; 
} 

void DynamixelNcGripperDriverNode::tool_close_actionGetResultCallback(iri_common_drivers_msgs::tool_closeResultPtr& result) 
{ 
    driver_.lock(); 
    //update result data to be sent to client 
    result->successful = is_closed_; 
    driver_.unlock(); 
} 

void DynamixelNcGripperDriverNode::tool_close_actionGetFeedbackCallback(iri_common_drivers_msgs::tool_closeFeedbackPtr& feedback) 
{ 
    driver_.lock(); 
    try
    {
        feedback->angle = driver_.get_current_angle();
        feedback->effort = driver_.get_current_effort();
        ROS_INFO("[angle]: %f, [effort]: %f", feedback->angle, feedback->effort); 
    }
    catch(...)
    {
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock(); 
}

/*  [action requests] */

void DynamixelNcGripperDriverNode::postNodeOpenHook(void)
{
}

void DynamixelNcGripperDriverNode::addNodeDiagnostics(void)
{
}

void DynamixelNcGripperDriverNode::addNodeOpenedTests(void)
{
}

void DynamixelNcGripperDriverNode::addNodeStoppedTests(void)
{
}

void DynamixelNcGripperDriverNode::addNodeRunningTests(void)
{
}

void DynamixelNcGripperDriverNode::reconfigureNodeHook(int level)
{
}

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

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