zyonz_chlorophyll_meter_client_alg_node.cpp

Go to the documentation of this file.

#include "zyonz_chlorophyll_meter_client_alg_node.h"

ZyonzChlorophyllMeterClientAlgNode::ZyonzChlorophyllMeterClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ZyonzChlorophyllMeterClientAlgorithm>(),
  take_sample_client_("take_sample", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  this->action_done=true;
}

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

void ZyonzChlorophyllMeterClientAlgNode::mainNodeThread(void)
{
  static bool close=false;

  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
  if(action_done)
  {
    if(close)
    {
      this->take_sample_goal_.close=true;
      close=false;
    }
    else
    {
      this->take_sample_goal_.close=false;
      close=true;
    }
    take_sampleMakeActionRequest();  
  }

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void ZyonzChlorophyllMeterClientAlgNode::take_sampleDone(const actionlib::SimpleClientGoalState& state,  const zyonz_msgs::chlorophyll_sampleResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleDone: Goal Achieved!"); 
  else 
    ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleDone: %s", state.toString().c_str()); 
  this->action_done=true;
  //copy & work with requested result 
} 

void ZyonzChlorophyllMeterClientAlgNode::take_sampleActive() 
{ 
  //ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleActive: Goal just went active!"); 
} 

void ZyonzChlorophyllMeterClientAlgNode::take_sampleFeedback(const zyonz_msgs::chlorophyll_sampleFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 
  std::cout << "Current meter angle: " << feedback->angle << std::endl;
  std::cout << "Current meter effort: " << feedback->effort << std::endl;

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    take_sample_client_.cancelGoal(); 
    //ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void ZyonzChlorophyllMeterClientAlgNode::take_sampleMakeActionRequest() 
{ 
  ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  take_sample_client_.waitForServer();  
  ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //take_sample_goal_.data = my_desired_goal; 
  sleep(1);
  take_sample_client_.sendGoal(take_sample_goal_, 
              boost::bind(&ZyonzChlorophyllMeterClientAlgNode::take_sampleDone,     this, _1, _2), 
              boost::bind(&ZyonzChlorophyllMeterClientAlgNode::take_sampleActive,   this), 
              boost::bind(&ZyonzChlorophyllMeterClientAlgNode::take_sampleFeedback, this, _1)); 
  this->action_done=false;
  ROS_INFO("ZyonzChlorophyllMeterClientAlgNode::take_sampleMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void ZyonzChlorophyllMeterClientAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void ZyonzChlorophyllMeterClientAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<ZyonzChlorophyllMeterClientAlgNode>(argc, argv, "zyonz_chlorophyll_meter_client_alg_node");
}