zyonz_chlorophyll_meter_driver_node.cpp

Go to the documentation of this file.

#include "zyonz_chlorophyll_meter_driver_node.h"

ZyonzChlorophyllMeterDriverNode::ZyonzChlorophyllMeterDriverNode(ros::NodeHandle &nh) : 
    iri_base_driver::IriBaseNodeDriver<ZyonzChlorophyllMeterDriver>(nh),
    last_action_successful_(false),
    take_sample_aserver_(public_node_handle_, "take_sample")
{
    //init class attributes if necessary
    //this->loop_rate_ = 2;//in [Hz]

    int baudrate(1000000);
    private_node_handle_.param<int>("baudrate", baudrate, 1000000);
    driver_.set_baudrate(baudrate);
    ROS_INFO("Baudrate: %d", baudrate);
    int bus_id(255);
    private_node_handle_.param<int>("bus_id", bus_id, 255);
    driver_.set_bus_id(bus_id);
    ROS_INFO("Bus ID.: %d", bus_id);
    int servo_id(0);
    private_node_handle_.param<int>("servo_id", servo_id, 0);
    driver_.set_servo_id((unsigned char)servo_id);
    ROS_INFO("Servo ID.: %d", servo_id);
    double max_angle(-25.0f);
    private_node_handle_.param<double>("max_angle", max_angle, -25.0);
    driver_.set_max_angle(max_angle);
    ROS_INFO("Max. Angle: %f", max_angle);
    double min_angle(-46.0f);
    private_node_handle_.param<double>("min_angle", min_angle, -46.0);
    driver_.set_min_angle(min_angle);
    ROS_INFO("Min. Angle: %f", min_angle);
    double effort(10.0f);
    private_node_handle_.param<double>("effort", effort, 10.0);
    driver_.set_effort(effort);
    ROS_INFO("Effort: %f", effort);

    // [init publishers]

    // [init subscribers]

    // [init services]

    // [init clients]

    // [init action servers]
    take_sample_aserver_.registerStartCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleStartCallback, this, _1)); 
    take_sample_aserver_.registerStopCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleStopCallback, this)); 
    take_sample_aserver_.registerIsFinishedCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleIsFinishedCallback, this)); 
    take_sample_aserver_.registerHasSucceedCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleHasSucceedCallback, this)); 
    take_sample_aserver_.registerGetResultCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleGetResultCallback, this, _1)); 
    take_sample_aserver_.registerGetFeedbackCallback(boost::bind(&ZyonzChlorophyllMeterDriverNode::take_sampleGetFeedbackCallback, this, _1)); 
    take_sample_aserver_.start(); 

    // [init action clients]
}

void ZyonzChlorophyllMeterDriverNode::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 ZyonzChlorophyllMeterDriverNode::take_sampleStartCallback(const zyonz_msgs::chlorophyll_sampleGoalConstPtr& goal)
{ 
    driver_.lock(); 
    std::cout << "Received a new take_sample action request" << std::endl;

    this->last_action_successful_ = false;

    //check goal 
    if(goal->close)
    {//execute close action
        this->last_action_successful_ = this->driver_.close_meter();
    }
    else
    {//execute open action
        this->last_action_successful_ = this->driver_.open_meter();
    }
    driver_.unlock(); 
} 

void ZyonzChlorophyllMeterDriverNode::take_sampleStopCallback(void) 
{ 
    driver_.lock(); 
    //stop action 
    this->driver_.stop_meter();
    driver_.unlock(); 
} 

bool ZyonzChlorophyllMeterDriverNode::take_sampleIsFinishedCallback(void) 
{ 
    ROS_DEBUG("take_sampleIsFinishedCallback");
    bool ret = false; 
    // wait for the motor to move
    usleep(200000);

    driver_.lock(); 
    //if action has finish for any reason 
    if (driver_.is_at_max_angle() || driver_.is_at_min_angle())
    {
        ret = true;
    }

    driver_.unlock(); 

    return ret; 
} 

bool ZyonzChlorophyllMeterDriverNode::take_sampleHasSucceedCallback(void) 
{ 
    bool ret = false; 

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

    return ret; 
} 

void ZyonzChlorophyllMeterDriverNode::take_sampleGetResultCallback(zyonz_msgs::chlorophyll_sampleResultPtr& result) 
{ 
    driver_.lock(); 
    //update result data to be sent to client 
    result->successful = last_action_successful_;
    driver_.unlock(); 
} 

void ZyonzChlorophyllMeterDriverNode::take_sampleGetFeedbackCallback(zyonz_msgs::chlorophyll_sampleFeedbackPtr& feedback) 
{
    ROS_DEBUG("take_sampleGetFeedbackCallback"); 
    driver_.lock(); 
    try
    {
        feedback->angle = driver_.get_current_angle();
        feedback->effort = driver_.get_current_effort();
        ROS_DEBUG("[angle]: %f, [effort]: %f", feedback->angle, feedback->effort); 
    }
    catch(...)
    {
        ROS_WARN("Uncatched exception");
    }
    driver_.unlock();
}

/*  [action requests] */

void ZyonzChlorophyllMeterDriverNode::postNodeOpenHook(void)
{
    take_sample_aserver_.start();
}

void ZyonzChlorophyllMeterDriverNode::addNodeDiagnostics(void)
{
}

void ZyonzChlorophyllMeterDriverNode::addNodeOpenedTests(void)
{
}

void ZyonzChlorophyllMeterDriverNode::addNodeStoppedTests(void)
{
}

void ZyonzChlorophyllMeterDriverNode::addNodeRunningTests(void)
{
}

void ZyonzChlorophyllMeterDriverNode::reconfigureNodeHook(int level)
{
}

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

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