tibi_dabo_conversation_alg_node.cpp

Go to the documentation of this file.

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

TibiDaboConversationAlgNode::TibiDaboConversationAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboConversationAlgorithm>(),
  hri_client_("hri", true),
  robot(""),
  sequences(NUM_CLIENTS, std::string("")),
  hri_action_in_progress_(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);

  public_node_handle_.getParam("robot", this->robot);

  this->loadSentences();
  this->index=-1;

  // [init publishers]

  // [init subscribers]
  this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 100, &TibiDaboConversationAlgNode::joy_callback, this);

  // [init services]
  this->conversation_server_ = this->public_node_handle_.advertiseService("conversation_server", &TibiDaboConversationAlgNode::conversationCallback, this);

  // [init clients]
  conversation_client_ = this->public_node_handle_.serviceClient<tibi_dabo_conversation::conversation>("conversation_client");

  // [init action servers]

  // [init action clients]
}

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

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

  // [fill srv structure and make request to the server]
/*  if(this->hri_action_in_progress_)
  {
    //When hri succeed, call service so the other robot responds //TODO
    this->conversation_srv_.request.type  = this->buttonIndex;
    this->index++;
    this->conversation_srv_.request.index = this->index;
    ROS_INFO("TibiDaboConversationAlgNode:: Sending New Service Request!");
    if (conversation_client_.call(conversation_srv_))
    {
      ROS_INFO("TibiDaboConversationAlgNode:: Conversation Service called");
    }
    else
    {
      ROS_INFO("TibiDaboConversationAlgNode:: Failed to Call Conversation Server");
    }
    this->hri_action_in_progress_=false; //TODO
  }
*/
  /*
  conversation_srv_.request.type = 0;
  conversation_srv_.request.index = 0;
  ROS_INFO("TibiDaboConversationAlgNode:: Sending New Request!");
  if (conversation_client_.call(conversation_srv_))
  {
    //ROS_INFO("TibiDaboConversationAlgNode:: Response: %s", conversation_srv_.response.result);
  }
  else
  {
    //ROS_INFO("TibiDaboConversationAlgNode:: Failed to Call Server on topic conversation ");
  }
  */

  // [fill action structure and make request to the action server]
  //hriMakeActionRequest(sequences);

  // [publish messages]
}

/*  [subscriber callbacks] */
void TibiDaboConversationAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& joy_msg)
{
  //ROS_INFO("TibiDaboConversationAlgNode::joy_callback: New Message Received");

  //use appropiate mutex to shared variables if necessary
  this->alg_.lock();
  //this->joy_mutex_.enter();

  //std::cout << msg->data << std::endl;
  bool execute_action=false;
  static bool first=true;

  if(first)
  {
    this->prev_buttons_.resize(joy_msg->buttons.size());
    first=false;
  }

  for(unsigned int i=0; i<joy_msg->buttons.size(); i++)
  {
    if(joy_msg->buttons[i]==1 && this->prev_buttons_[i]==0)
    {
      this->buttonIndex= this->useExtraButton(i);
      if(this->buttonIndex!=-1)
      {
        ROS_INFO("TibiDaboConversationAlgNode::joy_callback: Received Button %i", this->buttonIndex);
        execute_action=true;
        this->index = 0; //initialize conversation
      }
    }
  }

  if(this->hri_action_in_progress_ && execute_action)
  {
    ROS_INFO("TibiDaboHriTeleopAlgNode::joy_callback: hri_client_.cangelGoal()!");
    this->hri_client_.cancelGoal();
  }
  // Request Action
  if(!this->hri_action_in_progress_ && execute_action && unsigned(this->buttonIndex)<this->sentences.size() && unsigned(this->index)<this->sentences[this->buttonIndex].size())
  {
    this->sequences[TTS_]       = this->sentences[this->buttonIndex][this->index];
    this->sequences[LEDS_]      = "";
    this->sequences[HEAD_]      = "";
    this->sequences[RIGHT_ARM_] = "";
    this->sequences[LEFT_ARM_]  = "";
    this->hriMakeActionRequest(this->sequences);
    //hriMakeActionRequest();
  }
  this->prev_buttons_ = joy_msg->buttons;

  //unlock previously blocked shared variables
  this->alg_.unlock();
  //this->joy_mutex_.exit();
}

/*  [service callbacks] */
bool TibiDaboConversationAlgNode::conversationCallback(tibi_dabo_conversation::conversation::Request &req, tibi_dabo_conversation::conversation::Response &res)
{
  //ROS_INFO("TibiDaboConversationAlgNode::conversationCallback: New Request Received!");

  //use appropiate mutex to shared variables if necessary
  this->alg_.lock();
  //this->conversation_mutex_.enter();

  //ROS_INFO("TibiDaboConversationAlgNode::conversationCallback: Processin New Request!");
  //do operations with req and output on res
  //res.data2 = req.data1 + my_var;
  this->buttonIndex = req.type;
  this->index       = req.index;
  if(!this->hri_action_in_progress_ && unsigned(this->buttonIndex)<this->sentences.size() && unsigned(this->index)<this->sentences[this->buttonIndex].size())
  {

    //ROS_INFO("TibiDaboConversationAlgNode::conversationCallback: conversation continued!");

    this->sequences[TTS_]       = sentences[this->buttonIndex][this->index];
    this->sequences[LEDS_]      = "";
    this->sequences[HEAD_]      = "";
    this->sequences[RIGHT_ARM_] = "";
    this->sequences[LEFT_ARM_]  = "";
    this->hriMakeActionRequest(this->sequences);
    //hriMakeActionRequest();

  }
  else
  {
    //ROS_INFO("TibiDaboConversationAlgNode::conversationCallback: conversation NOT continued or finished (hri busy/end of sentneces!");
    this->buttonIndex = -1;
    this->index       = -1;
  }

  //unlock previously blocked shared variables
  this->alg_.unlock();
  //this->conversation_mutex_.exit();

  return true;
}

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

    //When hri succeed, call service so the other robot responds
    this->conversation_srv_.request.type  = this->buttonIndex;
    this->index++;
    this->conversation_srv_.request.index = this->index;
    //ROS_INFO("TibiDaboConversationAlgNode:: Sending New Service Request!");
    if (conversation_client_.call(conversation_srv_))
    {
      //ROS_INFO("TibiDaboConversationAlgNode:: Conversation Service called");
    }
    else
    {
      ROS_INFO("TibiDaboConversationAlgNode:: Failed to Call Conversation Server");
    }

  }
  else
    ROS_INFO("TibiDaboConversationAlgNode::hriDone: %s", state.toString().c_str());

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

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

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

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

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

  //set goal
  hri_goal_.sequence_file[TTS_]       = this->sequences[TTS_];
  hri_goal_.sequence_file[LEDS_]      = this->sequences[LEDS_];
  hri_goal_.sequence_file[HEAD_]      = this->sequences[HEAD_];
  hri_goal_.sequence_file[RIGHT_ARM_] = this->sequences[RIGHT_ARM_];
  hri_goal_.sequence_file[LEFT_ARM_]  = this->sequences[LEFT_ARM_];

  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("TibiDaboConversationAlgNode::hriMakeActionRequest: %s}", goal_str.c_str());

  //send a goal to the action
  //hri_goal_.data = my_desired_goal;
  hri_client_.sendGoal(hri_goal_,
              boost::bind(&TibiDaboConversationAlgNode::hriDone,     this, _1, _2),
              boost::bind(&TibiDaboConversationAlgNode::hriActive,   this),
              boost::bind(&TibiDaboConversationAlgNode::hriFeedback, this, _1));
  //ROS_INFO("TibiDaboConversationAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!");
  this->hri_action_in_progress_ = true;
}

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

  this->alg_.unlock();
}

void TibiDaboConversationAlgNode::addNodeDiagnostics(void)
{
}

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

int TibiDaboConversationAlgNode::useExtraButton(const unsigned int & index)
{
  ROS_DEBUG("Tibi Dabo wiimote use extra Button");

  switch(index)
  {
    case wiimote::State::MSG_BTN_1:
      ROS_DEBUG("Button 1!");
      return 0;
      break;

    case wiimote::State::MSG_BTN_2:
      ROS_DEBUG("Button 2!");
      return 1;
      break;

    case wiimote::State::MSG_BTN_B:
      ROS_DEBUG("Button B!");
      return 2;
      break;

    case wiimote::State::MSG_BTN_PLUS:
      ROS_DEBUG("Button (+)!");
      return 3;
      break;

    case wiimote::State::MSG_BTN_MINUS:
      ROS_DEBUG("Button (-)!");
      return 4;
      break;

    case wiimote::State::MSG_BTN_HOME:
      ROS_DEBUG("Button Home!");
      return 5;
      break;

    default:
      ROS_DEBUG("Button UNDEFINED!");
      return -1;
      break;
  }

}

void TibiDaboConversationAlgNode::loadSentences()
{
  // Button 1
  std::vector<std::string> sentencesList;
  sentencesList.push_back("Hola!");
  sentencesList.push_back("Ey, hola!");
  sentencesList.push_back("Què tal?");
  sentencesList.push_back("Be, i tu?");
  sentencesList.push_back("Molt be també!");
  sentencesList.push_back("Me n'alegro");
  this->sentences.push_back(sentencesList);

  //Button 2
  sentencesList.clear();
  sentencesList.push_back("Per cert");
  sentencesList.push_back("Diguem");
  sentencesList.push_back("Saps que?");
  sentencesList.push_back("Que");
  sentencesList.push_back("Mec!");
  sentencesList.push_back("Interesant...");
  sentencesList.push_back("\\item=laugh");
  this->sentences.push_back(sentencesList);

  // Button B
  sentencesList.clear();
  sentencesList.push_back("Com et dius?");
  sentencesList.push_back("Jo? Em dic "+this->robot+", i tu?");
  sentencesList.push_back("Jo soc "+this->robot+"-------------------------------------");
  sentencesList.push_back("Un plaer --------------------------------------");
  sentencesList.push_back("Igualment -------------------------------------");
  this->sentences.push_back(sentencesList);

  // Button +
  sentencesList.clear();
  sentencesList.push_back("Som -----------------------");
  sentencesList.push_back(this->robot+ "-----------------------------------------------");
  sentencesList.push_back("i "+ this->robot + " ---------------------------------------");
  sentencesList.push_back("Una parella de robots -----------------------");
  sentencesList.push_back("Socials i urbans -----------------------");
  this->sentences.push_back(sentencesList);

  // Button -
  sentencesList.clear();
  sentencesList.push_back("Bé, me n'he d'anar -----------------------");
  sentencesList.push_back("Val, fins un altr ---------------------------");
  sentencesList.push_back("Adeu! ---------------------------------------");
  this->sentences.push_back(sentencesList);

  // Button Home
  sentencesList.clear();
  sentencesList.push_back("\\item=throat ---------------------------");
  sentencesList.push_back("\\item=throat, \\item=throat-------------");
  this->sentences.push_back(sentencesList);
}