tibi_dabo_hri_client_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_hri_client_alg_node.h"

TibiDaboHriClientAlgNode::TibiDaboHriClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboHriClientAlgorithm>(),

  hri_client_("hri", true),
  sequences(NUM_CLIENTS, std::string("")),
  hri_active(false),
  sendRequest(false),
  stopRequest(false)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  hri_goal_.sequence_file   = std::vector<std::string>(NUM_CLIENTS, std::string(""));
  hri_goal_.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);

  // [init publishers]

  // [init subscribers]

  // [init services]

  // [init clients]

  // [init action servers]

  // [init action clients]
}

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

void TibiDaboHriClientAlgNode::mainNodeThread(void)
{
  // [fill msg structures]

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

  // [fill action structure and make request to the action server]
  if(this->sendRequest && !this->hri_active )
  {
    hriMakeActionRequest(sequences);
    this->sendRequest = false;
  }

  if(this->stopRequest && this->hri_active )
  {
    ROS_INFO("TibiDaboHriClientAlgNode::mainNodeThread: Cancelling Action!");
    this->hri_client_.cancelGoal();
    this->stopRequest = false;
  }

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

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

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

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

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

/*  [action requests] */
void TibiDaboHriClientAlgNode::hriMakeActionRequest(std::vector<std::string> &sequences)
{
  ROS_INFO("TibiDaboHriClientAlgNode::hriMakeActionRequest: Starting New Request!");

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

  //send a goal to the action
  //hri_goal_.data = my_desired_goal;
  hri_goal_.sequence_file[TTS_]       = sequences[TTS_];
  hri_goal_.sequence_file[LEDS_]      = sequences[LEDS_];
  hri_goal_.sequence_file[HEAD_]      = sequences[HEAD_];
  hri_goal_.sequence_file[RIGHT_ARM_] = sequences[RIGHT_ARM_];
  hri_goal_.sequence_file[LEFT_ARM_]  = sequences[LEFT_ARM_];

  hri_client_.sendGoal(hri_goal_,
              boost::bind(&TibiDaboHriClientAlgNode::hriDone,     this, _1, _2),
              boost::bind(&TibiDaboHriClientAlgNode::hriActive,   this),
              boost::bind(&TibiDaboHriClientAlgNode::hriFeedback, this, _1));
  ROS_INFO("TibiDaboHriClientAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!");

  std::string goal_str("goal : {");
  for(unsigned int ii=0; ii<hri_goal_.sequence_file.size(); ii++)
  {
    goal_str += hri_goal_.sequence_file[ii] + ", ";
  }
  ROS_INFO("TibiDaboHriClientAlgNode::hriMakeActionRequest: %s}", goal_str.c_str());

  this->hri_active = true;

}

void TibiDaboHriClientAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  ROS_INFO("TibiDaboHriClientAlgNode::node_config_update!");
  if(config.Request) //Send request with sequences
  {
    if(!this->sendRequest)
    {
      ROS_INFO("TibiDaboHriClientAlgNode::node_config_update: new request!");

      std::string voice;
      if(config.Genre == "Dabo")
      {
       if(config.Language == "English")
         voice="Simon";
       else if(config.Language == "Catalan")
         voice="Jordi";
       else if(config.Language == "Spanish")
         voice="Jorge";
       else
         voice="Simon";
      }
      else if(config.Genre == "Tibi")
      {
       if(config.Language == "English")
         voice="Kate";
       else if(config.Language == "Catalan")
         voice="Montserrat";
       else if(config.Language == "Spanish")
         voice="Leonor";
       else
         voice="Kate";
      }
      else
        voice="Kate";

      sequences[TTS_]       = "\\language="+config.Language + " \\voice=" + voice + " \\volume=" + boost::to_string(config.Volume) + " " + config.tts;
      sequences[LEDS_]      = config.leds;
      sequences[HEAD_]      = config.head;
      sequences[RIGHT_ARM_] = config.right_arm;
      sequences[LEFT_ARM_]  = config.left_arm;
      this->sendRequest=true;
    }
    else //Only once
    {
      this->sendRequest=false;
    }
  }
  this->stopRequest = config.Stop;
  this->alg_.unlock();
}

void TibiDaboHriClientAlgNode::addNodeDiagnostics(void)
{
}

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