voice_recognition_client_alg_node.cpp

Go to the documentation of this file.

#include "voice_recognition_client_alg_node.h"

VoiceRecognitionClientAlgNode::VoiceRecognitionClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<VoiceRecognitionClientAlgorithm>(),
  hri_client_("hri", true),
  voice_recognition_client_("voice_recognition", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  //this->code_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("code", 100);
  this->code_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::voiceRecognitionAnswer>("answerCode", 100);
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  this->voice_recognition_action_in_progress = false;
  this->voice_recognition_done               = false;
  this->hri_action_in_progress               = false;
  this->lastStatus_                          = 0;
  this->code_                                = 0;
  this->lastCode_                            = 0;

  hri_goal_.sequence_file   = std::vector<std::string>(NUM_CLIENTS, std::string(""));
  hri_goal_.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);
}

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

void VoiceRecognitionClientAlgNode::mainNodeThread(void)
{
  //ROS_INFO("VoiceRecognitionClientAlgNode::this->code_: %i", this->code_);
  // [fill msg structures]
  //this->String_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server] 
  
  // Start voice_recognition action request
  bool run = true;
  if(run && !this->voice_recognition_action_in_progress && !this->hri_action_in_progress && !this->voice_recognition_done)
  {
    this->voice_recognition_action_in_progress = true;
    voice_recognitionMakeActionRequest();
    this->voice_recognition_done = true;    
  }
  // Start hri action request if voice_recognition action is done
  if(this->voice_recognition_done && !this->hri_action_in_progress)
  {
    if(this->answer_.size()!=0)
    {
      //ROS_INFO("VoiceRecognitionClientAlgNode::Selected answer is: %s", this->answer_.c_str());
      this->hri_action_in_progress = true;
      //ROS_INFO("VoiceRecognitionClientAlgNode::Supposed hriMakeActionRequest and succeeded, to continue!");
      hriMakeActionRequest();
      this->hri_action_in_progress = false;
      //this->answer_.clear();
    }
    this->voice_recognition_done = false;
  }

  // [publish messages]
  // Publish message with code and answer (if code has changed)
  if(this->code_ != this->lastCode_)
  {
    //std::ostringstream osstream;
    //osstream << this->code_;
    //std::string aux = osstream.str();  
    //this->String_msg_.data = aux;
    this->voiceRecognitionAnswer_msg_.code   = this->code_;
    this->voiceRecognitionAnswer_msg_.answer = this->answer_;
    this->code_publisher_.publish(this->voiceRecognitionAnswer_msg_);
    this->lastCode_ = this->code_;
    
    if(this->answer_.size()!=0)
    {
      if(this->code_<100) //normal answer
      {
        sleep(3);
      }
      else //location answer
      {
        sleep(4);
      }
    }
      //std::cin.get(); //wait for key
    this->answer_.clear();
  }
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

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

  //copy & work with requested result
  this->hri_action_in_progress = false;
} 

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

void VoiceRecognitionClientAlgNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("VoiceRecognitionClientAlgNode::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 ) 
  { 
    hri_client_.cancelGoal(); 
    //ROS_INFO("VoiceRecognitionClientAlgNode::hriFeedback: Cancelling Action!"); 
  } 
}

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

  //copy & work with requested result
  this->answer_ = result->answer;
  this->code_   = result->code;
  ROS_INFO("VoiceRecognitionClientAlgNode:: result, code: %s, %i", this->answer_.c_str(), this->code_);
  this->voice_recognition_action_in_progress = false;
  this->voice_recognition_done = true;
} 

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

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

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var;
  if(feedback->status != this->lastStatus_)
  {
    this->code_ = feedback->status;
    ROS_INFO("VoiceRecognitionClientAlgNode:: New status: %i", feedback->status);
    this->lastStatus_ = feedback->status;
    
  }

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

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

  //wait for the action server to start 
  //will wait for infinite time 
  hri_client_.waitForServer();  
  ROS_INFO("VoiceRecognitionClientAlgNode::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(&VoiceRecognitionClientAlgNode::hriDone,     this, _1, _2), 
              boost::bind(&VoiceRecognitionClientAlgNode::hriActive,   this), 
              boost::bind(&VoiceRecognitionClientAlgNode::hriFeedback, this, _1)); 
  ROS_INFO("VoiceRecognitionClientAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!");
  this->hri_action_in_progress = true;
}

void VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest() 
{ 
  ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  voice_recognition_client_.waitForServer();  
  ROS_INFO("VoiceRecognitionClientAlgNode::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(&VoiceRecognitionClientAlgNode::voice_recognitionDone,     this, _1, _2), 
              boost::bind(&VoiceRecognitionClientAlgNode::voice_recognitionActive,   this), 
              boost::bind(&VoiceRecognitionClientAlgNode::voice_recognitionFeedback, this, _1)); 
  ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest: Goal Sent. Wait for Result!");
  this->voice_recognition_action_in_progress = true;  
}

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

  this->alg_.unlock();
}

void VoiceRecognitionClientAlgNode::addNodeDiagnostics(void)
{
}

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