dabo_intro_alg_node.cpp

Go to the documentation of this file.

#include "dabo_intro_alg_node.h"
#include <wiimote/State.h>

DaboIntroAlgNode::DaboIntroAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DaboIntroAlgorithm>(),
  hri_client_client_("hri_client", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 1, &DaboIntroAlgNode::joy_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  // [init class attributes]
  this->hriActive       =false;
  this->trigger_forward =false;
  this->trigger_backward=false;
  this->current_language=0;
  this->robot=0; //dabo
  this->hri_client_goal_.sequence_file.resize(5);

  std::locale::global(std::locale(""));
  this->loadSequences();
}

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

void DaboIntroAlgNode::mainNodeThread(void)
{
  static int state=1;
  //static int timeout=0;
  // if(!this->trigger_forward)
  // {
  //   ROS_INFO("trigger_forward");
  //   this->trigger_forward=true;
  // }

  alg_.lock(); 
  switch(state)
  {
    //intro
    case 0:
      if(this->trigger_forward)
      {
//        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state++;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state=5;
      }
      break;
   //tibidabo
   case 1:
      if(this->trigger_forward)
      {
        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state=2;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state=6;
      }
      break;
    //teo
    case 2:
      if(this->trigger_forward)
      {
        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state=4;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state=1;
      }
      break;
    //humanoides
    case 3:
      if(this->trigger_forward)
      {
//        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state++;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state--;
      }
      break;
    //kinton
    case 4:
      if(this->trigger_forward)
      {
        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state=6;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state=2;
      }
      break;
    //preguntes
    case 5:
      if(this->trigger_forward)
      {
//        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state++;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state--;
      }
      break;
    //gracies
    case 6:
      if(this->trigger_forward)
      {
        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state=1;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state=4;
      }
      break;
    //adeu
    case 7:
      if(this->trigger_forward)
      {
//        if(this->doHri(state))
        {
          this->trigger_forward=false;
          state=0;
        }
      }
      else if(this->trigger_backward)
      {
        this->trigger_backward=false;
        state--;
      }
      break;
  }
  //this->current_state=state;
  this->alg_.unlock();

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

  // [publish messages]
}

bool DaboIntroAlgNode::doHri(int state)
{
  bool done=false;
  static int index = 0;
  int number = this->speech[state].size();

  if(!hriActive)
  {
    if(index<number)
    {
      this->fillHriGoal(state,index);
      hri_clientMakeActionRequest();
      ROS_INFO("DaboIntroAlgNode:doHri: speech[%d][%d][%d]: %s", state, index, this->current_language, this->hri_client_goal_.sequence_file[0].c_str());
      index++;
    }
    else
    {
      done=true;
      index=0;
    }
  }
  return done;
}

void DaboIntroAlgNode::fillHriGoal(int state, int index)
{
  this->hri_client_goal_.sequence_file[0] = this->hri_prefix[this->robot][this->current_language] + this->speech[state][index][this->current_language];
  this->hri_client_goal_.sequence_file[1] = "";
  this->hri_client_goal_.sequence_file[2] = this->head_sequence[state][index][this->current_language];
  this->hri_client_goal_.sequence_file[4] = this->left_arm_sequence[state][index][this->current_language];
  this->hri_client_goal_.sequence_file[3] = this->right_arm_sequence[state][index][this->current_language];
}

/*  [subscriber callbacks] */
void DaboIntroAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg) 
{ 
  //ROS_INFO("DaboIntroAlgNode::joy_callback: New Message Received");
  static std::vector<int> prev_buttons(msg->buttons);

  this->alg_.lock(); 

  if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==1 && prev_buttons[wiimote::State::MSG_BTN_PLUS]==0)
  {
    ROS_INFO("DaboIntroAlgNode::joy_callback: trigger forward"); 
    this->trigger_forward =true;
    this->trigger_backward=false;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_MINUS]==1 && prev_buttons[wiimote::State::MSG_BTN_MINUS]==0)
  {
    ROS_INFO("DaboIntroAlgNode::joy_callback: trigger backward"); 
    this->trigger_backward=true;
    this->trigger_forward =false;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1 && prev_buttons[wiimote::State::MSG_BTN_HOME]==0)
  {
   ROS_INFO("TibiDaboIntroAlgNode::joy_callback: cancel hri");
   this->hri_client_client_.cancelGoal();
  }
  prev_buttons=msg->buttons;
  this->alg_.unlock(); 
}

/*  [service callbacks] */

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

  //copy & work with requested result 
  this->hriActive=false;
  this->alg_.unlock();
} 

void DaboIntroAlgNode::hri_clientActive() 
{ 
  this->alg_.lock();
  //ROS_INFO("DaboIntroAlgNode::hri_clientActive: Goal just went active!"); 
  this->hriActive=true;
  this->alg_.unlock();
} 

void DaboIntroAlgNode::hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  this->alg_.lock();
  //ROS_INFO("DaboIntroAlgNode::hri_clientFeedback: 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_client_.cancelGoal(); 
    //ROS_INFO("DaboIntroAlgNode::hri_clientFeedback: Cancelling Action!"); 
  } 
  this->alg_.unlock();
}

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

  //wait for the action server to start 
  //will wait for infinite time 
  ROS_INFO("DaboIntroAlgNode::hri_clientMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //hri_client_goal_.data = my_desired_goal; 
  hri_client_client_.sendGoal(hri_client_goal_, 
              boost::bind(&DaboIntroAlgNode::hri_clientDone,     this, _1, _2), 
              boost::bind(&DaboIntroAlgNode::hri_clientActive,   this), 
              boost::bind(&DaboIntroAlgNode::hri_clientFeedback, this, _1)); 
  ROS_INFO("DaboIntroAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void DaboIntroAlgNode::loadSequences()
{
  std::vector < std::vector < std::string > > sequence;
  std::vector < std::string > sentence;

  //speech[0]: intro
  sentence.push_back("Hola, bienvenidos al laboratorio de robótica móvil.");
  sentence.push_back("Hola, benvinguts al laboratori de robòtica mòbil.");
  sentence.push_back("Hello, welcome to the mobile robotics laboratory.");
  sequence.push_back(sentence);
  sentence.clear();
  sentence.push_back("Ahora os presentaremos algunos de los robots que tenemos por aquí.");
  sentence.push_back("Ara us presentarem alguns dels robots que tenim per aquí.");
  sentence.push_back("Now we will present some of the robots we have here.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[1]: tibidabo
  sentence.push_back("Yo soy Dabo. Un robot de servicio, social, y urbano.");
  sentence.push_back("Jo sóc Dàbo. Un robot de servei, social, i urbà.");
  sentence.push_back("I am Dabo. A service, social, and urban robot.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[2]: teo
  sentence.push_back("Continuemos. Por aquí tenemos a Teo. Es un robot todo terreno de cuatro ruedas, que puede hacer mapas 3 d.");
  sentence.push_back("Continuem. Per aquí tenim el Téóh. És un robot tot terreny de quatre rodes, que pot fer mapes 3 d.");
  sentence.push_back("Let's continue. Here we have Teo. It's an all terrain robot with four wheels.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[3]: humanoides
  sentence.push_back("Ahora os enseñaremos los humanoides.");
  sentence.push_back("Ara us ensenyarem els humanoides.");
  sentence.push_back("Now we will show you the humanoids.");
  sequence.push_back(sentence);
  sentence.clear();
  sentence.push_back("Son unos pequeños robots con piernas y brazos que pueden caminar, subir escaleras, jugar a sumo, e incluso jugar a fútbol.");
  sentence.push_back("Són uns petits robots amb cames i braços que poden caminar, pujar escales, jugar a sumo, i fins i tot jugar a futbol.");
  sentence.push_back("They are little robots with legs and arms who can walk, climb stairs, play sumo, and even soccer.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[4]: kinton
  sentence.push_back("Y aquí veis a Kinton, nuestro robot volador. Es como un helicóptero con cuatro hélices.");
  sentence.push_back("I aquí veieu el kíntoun, el nostre robot volador. És com un helicòpter amb quatre hèlices.");
  sentence.push_back("And here you can see Kinton, our flying robot. Is like a helicopter with four helixs.");
  sequence.push_back(sentence);
  sentence.clear();
  sentence.push_back("Aunque no puede volar aquí, lo utilizamos en investigación de vehículos aéreos, y vuelo autònomo.");
  sentence.push_back("Encara que aquí no pot volar, el fem servir en investigació de vehicles aeris i, vol autònom.");
  sentence.push_back("Although it can not fly here, we use it for air vehicle research and autonomous flight.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[5]: preguntas
  sentence.push_back("Si teneis preguntas, ahora es el momento. No dudeis en preguntar, intentaremos responderlas todas.");
  sentence.push_back("Si teniu preguntes, ara és el moment. No dubteu en demanar-les, intentarem resoldre-les totes.");
  sentence.push_back("If you have questions, now is the moment. Don't doubt to ask them to us, we will try to solve them all.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[6]: gracias
  sentence.push_back("Muchas gracias por haber venido. Esperamos que os haya gustado esta visita virtual.");
  sentence.push_back("Moltes gràcies per haver vingut. Esperem que us hagi agradat aquesta visita virtual.");
  sentence.push_back("Thank you for coming, We hope you have enjoyed this virtual visit.");
  sequence.push_back(sentence);
  sentence.clear();
  this->speech.push_back(sequence);
  sequence.clear();

  //speech[7]: adios
  sentence.push_back("Ha sido un placer. Hasta la próxima. Adiós!");
  sentence.push_back("Ha estat un plaer. Fins la propera. Adéu!");
  sentence.push_back("It's been a pleasure. See you next time. Bye!");
  sequence.push_back(sentence);
  sentence.clear();
  speech.push_back(sequence);
  sequence.clear();

  int max_sentences = 2; //max sentences per speech level
  //head_sequence[0]
  sentence.resize(3,"head_home.xml");
  sequence.resize(max_sentences,sentence);
  sentence.clear();
  this->head_sequence.resize(8,sequence);
  sequence.clear();
 
  //left_arm_sequence[0]
  sentence.resize(3,"left_arm_home.xml");
  sequence.resize(max_sentences,sentence);
  sentence.clear();
  this->left_arm_sequence.resize(8,sequence);
  sequence.clear();

  //right_arm_sequence[0]
  sentence.resize(3,"right_arm_home.xml");
  sequence.resize(max_sentences,sentence);
  sentence.clear();
  this->right_arm_sequence.resize(8,sequence);
  sequence.clear();

  //head and arms movements 
  //intro
  this->head_sequence[0][0][0]     ="head_hi.xml";
  this->left_arm_sequence[0][0][0] ="left_arm_home.xml";
  this->right_arm_sequence[0][0][0]="right_arm_home.xml";

  this->head_sequence[0][1][0]     ="head_home.xml";
  this->left_arm_sequence[0][1][0] ="left_arm_home.xml";
  this->right_arm_sequence[0][1][0]="right_arm_home.xml";
  //tibidabo 
  this->head_sequence[1][0][0]     ="head_talk_circle_6s.xml";
  this->left_arm_sequence[1][0][0] ="left_arm_talk.xml";
  this->right_arm_sequence[1][0][0]="right_arm_talk.xml";
 
  this->head_sequence[1][1][0]     ="head_talk_l_4s.xml";
  //this->left_arm_sequence[1][1][0] ="left_arm_talk_2.xml";
  //this->right_arm_sequence[1][1][0]="right_arm_talk_2.xml";
  //teo
  this->head_sequence[2][0][0] = "head_down.xml";
  //humanoides
  this->head_sequence[3][0][0]     ="head_talk_right_6s.xml";
  this->left_arm_sequence[3][0][0] ="left_arm_right.xml";
  this->right_arm_sequence[3][0][0]="right_arm_right.xml";
  //kinton
  this->head_sequence[4][0][0] = "head_talk_l_4s.xml";
  //preguntes
  this->head_sequence[5][0][0] = "head_yes_4s.xml";
  //gracies
  this->head_sequence[6][0][0] = "head_roll.xml";
  //adeu
  this->head_sequence[7][0][0] = "head_side.xml";
  
  //left_arm_sequence[0]
  sentence.resize(3,"left_arm_home.xml");
  sequence.resize(max_sentences,sentence);
  sentence.clear();
  this->left_arm_sequence.resize(8,sequence);
  sequence.clear();

  //right_arm_sequence[0]
  sentence.resize(3,"right_arm_home.xml");
  sequence.resize(max_sentences,sentence);
  sentence.clear();
  this->right_arm_sequence.resize(8,sequence);
  sequence.clear();

  std::vector < std::string > prefix;
  prefix.push_back("\\language=Spanish \\voice=Leonor ");
  prefix.push_back("\\language=Catalan \\voice=Montserrat ");
  prefix.push_back("\\language=English \\voice=Kate ");
  this->hri_prefix.push_back(prefix);
  prefix.clear();
  prefix.push_back("\\language=Spanish \\voice=Jorge ");
  prefix.push_back("\\language=Catalan \\voice=Jordi ");
  prefix.push_back("\\language=English \\voice=Simon ");
  this->hri_prefix.push_back(prefix);
  prefix.clear();
}

void DaboIntroAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  if(config.trigger_forward)
  {
    this->trigger_forward=config.trigger_forward;
    config.trigger_forward=false;
  }
  else if(config.trigger_backward)
  {
    this->trigger_backward=config.trigger_backward;
    config.trigger_backward=false;
  }
  this->current_language=config.language;
  if(config.robot=="tibi")
  {
    this->robot=0;
  }
  else if(config.robot=="dabo")
  {
    this->robot=1;
  }
  else
  {
    ROS_ERROR("TibiDaboIntroAlgNode::node_config_update: wrong parameter robot, must be tibi or dabo");
    config.robot="tibi";
  }
  if(config.stop_hri)
  {
    this->hri_client_client_.cancelGoal();
    config.stop_hri=false;
  }
  this->alg_.unlock();
}

void DaboIntroAlgNode::addNodeDiagnostics(void)
{
}

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