tibi_dabo_questions_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_questions_alg_node.h"

TibiDaboQuestionsAlgNode::TibiDaboQuestionsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboQuestionsAlgorithm>(),
  voice_recognition_client_("voice_recognition", true),
  hri_client_("hri", true)
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]
  this->answer_="";
  this->status_=-1;
  this->lastStatus_=-1;
  
  this->questionsOn_ = true;
  this->hriOn_       = true;
  this->infoOn_      = true;
  
  this->hri_active_       = false;
  this->questions_active_ = false;
  
  hri_goal_.sequence_file   = std::vector<std::string>(NUM_CLIENTS, std::string(""));
  hri_goal_.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);
  
  this->public_node_handle_.getParam("path", this->alg_.path_);
  this->alg_.initialize();

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

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

void TibiDaboQuestionsAlgNode::mainNodeThread(void)
{

  if(this->questionsOn_)
  {
    this->alg_.lock();

    // Questions
    if(!this->questions_active_ && (this->answer_.size()==0))
    {
      //this->questionsOn_=false;
      ROS_INFO("TibiDaboQuestionsAlgNode::mainNodeThread: questions Request!");
      voice_recognitionMakeActionRequest();
    }

    // HRI
    if(this->hriOn_ && !this->hri_active_ && (this->answer_.size()!=0) && this->alg_.showingAnswer_)
    {
      ROS_INFO("TibiDaboQuestionsAlgNode::mainNodeThread: hri Request!");
      hriMakeActionRequest();
    }

    // Info
    if(this->infoOn_)
    {
      this->alg_.iteration(this->status_, this->answer_);
    }
    else
    {
      this->alg_.iteration(-1, ""); //irilogo
    }

    if(!this->hriOn_ && (this->answer_.size()!=0) && this->alg_.showingAnswer_)
    {
      this->alg_.showingAnswer_ = false;
      this->answer_.clear();
      ROS_INFO("TibiDaboQuestionsAlgNode::mainNodeThread: sleeping 3 seconds");
      sleep(3);
    }

    this->alg_.unlock();
  }
  else
  {
    this->alg_.iteration(-1, ""); //irilogo
    if(this->questions_active_)
    {
      voice_recognition_client_.cancelGoal();
    }
  }

  if(!hriOn_ && this->hri_active_)
  {
    hri_client_.cancelGoal();
  }

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

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboQuestionsAlgNode::voice_recognitionDone(const actionlib::SimpleClientGoalState& state,  const iri_perception_msgs::voiceRecognitionResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionDone: %s", state.toString().c_str()); 

  //copy & work with requested result
    if(questionsOn_)
    {
      this->alg_.lock();
      this->answer_ = result->answer;
      this->status_ = result->code;
      ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionDone: Result, code: \"%s\", %i", this->answer_.c_str(), this->status_);
      this->alg_.unlock();
    }
    this->questions_active_ = false;
}

void TibiDaboQuestionsAlgNode::voice_recognitionActive() 
{ 
  //ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionActive: Goal just went active!"); 
} 

void TibiDaboQuestionsAlgNode::voice_recognitionFeedback(const iri_perception_msgs::voiceRecognitionFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var;
  if(feedback->status != this->lastStatus_)
  {
    this->alg_.lock();
    ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionFeedback: new feedback->status: %i", feedback->status); 
    this->status_ = feedback->status;
    this->lastStatus_ = this->status_;
    this->alg_.unlock();
  }

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    voice_recognition_client_.cancelGoal(); 
  } 
}

void TibiDaboQuestionsAlgNode::hriDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiDaboQuestionsAlgNode::hriDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiDaboQuestionsAlgNode::hriDone: %s", state.toString().c_str()); 

  //copy & work with requested result
  this->alg_.lock();
  this->hri_active_ = false;
  this->alg_.showingAnswer_ = false;
  this->answer_.clear();
  ROS_INFO("TibiDaboQuestionsAlgNode::hriDone: sleeping 3 seconds");
  sleep(3);
  this->alg_.unlock();
} 

void TibiDaboQuestionsAlgNode::hriActive() 
{ 
  //ROS_INFO("TibiDaboQuestionsAlgNode::hriActive: Goal just went active!"); 
} 

void TibiDaboQuestionsAlgNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboQuestionsAlgNode::hriFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok || !hriOn_ || !questionsOn_ ) 
  { 
    hri_client_.cancelGoal();
    this->hri_active_=false;
    //ROS_INFO("TibiDaboQuestionsAlgNode::hriFeedback: Cancelling Action!"); 
  } 
}

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

  //wait for the action server to start 
  //will wait for infinite time 
  voice_recognition_client_.waitForServer();  
  ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //voice_recognition_goal_.data = my_desired_goal; 
  voice_recognition_client_.sendGoal(voice_recognition_goal_, 
              boost::bind(&TibiDaboQuestionsAlgNode::voice_recognitionDone,     this, _1, _2), 
              boost::bind(&TibiDaboQuestionsAlgNode::voice_recognitionActive,   this), 
              boost::bind(&TibiDaboQuestionsAlgNode::voice_recognitionFeedback, this, _1)); 
  ROS_INFO("TibiDaboQuestionsAlgNode::voice_recognitionMakeActionRequest: Goal Sent. Wait for Result!");
  this->questions_active_ = true;
}

void TibiDaboQuestionsAlgNode::hriMakeActionRequest() 
{ 
  ROS_INFO("TibiDaboQuestionsAlgNode::hriMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  hri_client_.waitForServer();  
  ROS_INFO("TibiDaboQuestionsAlgNode::hriMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //hri_goal_.data = my_desired_goal;
  hri_goal_.sequence_file[TTS_] = this->answer_;
  hri_client_.sendGoal(hri_goal_, 
              boost::bind(&TibiDaboQuestionsAlgNode::hriDone,     this, _1, _2), 
              boost::bind(&TibiDaboQuestionsAlgNode::hriActive,   this), 
              boost::bind(&TibiDaboQuestionsAlgNode::hriFeedback, this, _1)); 
  ROS_INFO("TibiDaboQuestionsAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!");
  ROS_INFO("TibiDaboQuestionsAlgNode::hriMakeActionRequest: Goal Sent. Answer: \"%s\"", hri_goal_.sequence_file[TTS_].c_str());
  this->hri_active_=true;
}

void TibiDaboQuestionsAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->questionsOn_ = config.recognition;
  this->hriOn_       = config.hri;
  this->infoOn_      = config.info;
  this->alg_.config_ = config;
  this->alg_.unlock();
}

void TibiDaboQuestionsAlgNode::addNodeDiagnostics(void)
{
}

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