dynamixel_no_gripper_driver_node.cpp

Go to the documentation of this file.

#include "dynamixel_no_gripper_driver_node.h"

DynamixelNoGripperDriverNode::DynamixelNoGripperDriverNode(ros::NodeHandle &nh) : 
    iri_base_driver::IriBaseNodeDriver<DynamixelNoGripperDriver>(nh),
    is_opened_(false),
    is_closed_(false),
    tool_open_aserver_(public_node_handle_, "tool_open_action"),
    tool_close_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_aserver_.registerStartCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openStartCallback, this, _1)); 
    tool_open_aserver_.registerStopCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openStopCallback, this)); 
    tool_open_aserver_.registerIsFinishedCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openIsFinishedCallback, this)); 
    tool_open_aserver_.registerHasSucceedCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openHasSucceedCallback, this)); 
    tool_open_aserver_.registerGetResultCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openGetResultCallback, this, _1)); 
    tool_open_aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_openGetFeedbackCallback, this, _1)); 
    tool_open_aserver_.start();

    tool_close_aserver_.registerStartCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeStartCallback, this, _1)); 
    tool_close_aserver_.registerStopCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeStopCallback, this)); 
    tool_close_aserver_.registerIsFinishedCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeIsFinishedCallback, this)); 
    tool_close_aserver_.registerHasSucceedCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeHasSucceedCallback, this)); 
    tool_close_aserver_.registerGetResultCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeGetResultCallback, this, _1)); 
    tool_close_aserver_.registerGetFeedbackCallback(boost::bind(&DynamixelNoGripperDriverNode::tool_closeGetFeedbackCallback, this, _1)); 
    tool_close_aserver_.start();

    // [init action clients]
}

void DynamixelNoGripperDriverNode::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 DynamixelNoGripperDriverNode::tool_openStartCallback(const iri_common_drivers_msgs::tool_openGoalConstPtr& goal)
{ 
    driver_.lock(); 

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

    driver_.unlock(); 
} 

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

bool DynamixelNoGripperDriverNode::tool_openIsFinishedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 

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

    driver_.unlock(); 

    return ret; 
} 

bool DynamixelNoGripperDriverNode::tool_openHasSucceedCallback(void) 
{ 
    bool ret = false; 

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

    return ret; 
} 

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

void DynamixelNoGripperDriverNode::tool_openGetFeedbackCallback(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 DynamixelNoGripperDriverNode::tool_closeStartCallback(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 DynamixelNoGripperDriverNode::tool_closeStopCallback(void) 
{ 
    driver_.lock(); 
    //stop action 
    try{
        driver_.stop_gripper();
    }catch(...){
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock(); 
} 

bool DynamixelNoGripperDriverNode::tool_closeIsFinishedCallback(void) 
{ 
    bool ret = false; 

    driver_.lock(); 

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

    driver_.unlock(); 

    return ret; 
} 

bool DynamixelNoGripperDriverNode::tool_closeHasSucceedCallback(void) 
{ 
    bool ret = false; 

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

    return ret; 
} 

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

void DynamixelNoGripperDriverNode::tool_closeGetFeedbackCallback(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 DynamixelNoGripperDriverNode::postNodeOpenHook(void)
{
}

void DynamixelNoGripperDriverNode::addNodeDiagnostics(void)
{
}

void DynamixelNoGripperDriverNode::addNodeOpenedTests(void)
{
}

void DynamixelNoGripperDriverNode::addNodeStoppedTests(void)
{
}

void DynamixelNoGripperDriverNode::addNodeRunningTests(void)
{
}

void DynamixelNoGripperDriverNode::reconfigureNodeHook(int level)
{
}

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

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