qnode_ecmr.cpp

Go to the documentation of this file.

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <sstream>
#include "../include/tibi_dabo_gui/qnode_ecmr.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace tibi_dabo_gui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
  init_argc(argc),
  init_argv(argv),
  hri_action("hri",true),
  haod_action("haod",true),
  xml_files(NUM_CLIENTS, std::string(""))
  {}

QNode::~QNode() {
  if(ros::isStarted()) {
    ros::shutdown(); // explicitly needed since we use ros::start();
    ros::waitForShutdown();
  }
  wait();
}

bool QNode::initialize()
{
  if ( ! ros::master::check() ) {
      return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;

  joy_publisher          = n.advertise<sensor_msgs::Joy>("joy", 1000);
  image_subscriber       = n.subscribe("image", 1000, &QNode::imageCallback, this);
  imageDetect_subscriber = n.subscribe("imageDetect", 1000, &QNode::imageDetectCallback, this);
  faces_subscriber       = n.subscribe("faces", 1000, &QNode::facesCallback, this);

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

  this->loadQuestions();
  this->imageReady=false;
  this->imageDetectReady=false;
  this->hri_active=false;
  this->haod_active=false;
  this->assistance=false;

  start();
  return true;
}

void QNode::imageCallback(const sensor_msgs::Image::ConstPtr &msg)
{
    this->image = *msg;
    this->imageReady = true;
}

void QNode::imageDetectCallback(const sensor_msgs::Image::ConstPtr &msg)
{
    this->imageDetect = *msg;
    this->imageDetectReady = true;
}

void QNode::facesCallback(const face_detector_mono::RectArray::ConstPtr &msg)
{
  this->mutex.lock();
  //save msg in rects, to be drawn on image by main_window
  this->rects.clear();
  for(unsigned int i=0; i<msg->rects.size(); i++)
  {
    std::vector<int> rect;
    rect.push_back(msg->rects[i].x);
    rect.push_back(msg->rects[i].y);
    rect.push_back(msg->rects[i].width);
    rect.push_back(msg->rects[i].height);
    this->rects.push_back(rect);
  }
  this->mutex.unlock();
}

void QNode::run() {
  ros::Rate loop_rate(100);
  int count = 0;
  while ( ros::ok() )
  {
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
  Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

// void QNode::buttonPressed(int button)
// {
//   ROS_INFO("QNode::buttonPressed: %d", button);
//   sensor_msgs::Joy msg;
//   msg.buttons.resize(11);
//   msg.buttons[button] = 1;
//   joy_publisher.publish(msg);
// }

bool QNode::getImage(uchar* & data, int & width, int & height, std::string & encoding)
{
  if(imageReady)
  {
    data     = &this->image.data[0];
    width    = this->image.width;
    height   = this->image.height;
    encoding = this->image.encoding;
  }
  return imageReady;
}

bool QNode::getImageDetect(uchar* & data, int & width, int & height, std::string & encoding)
{
  if(imageDetectReady)
  {
    data     = &this->imageDetect.data[0];
    width    = this->imageDetect.width;
    height   = this->imageDetect.height;
    encoding = this->imageDetect.encoding;
  }
  return imageDetectReady;
}

std::string QNode::processAnswer(int question)
{
  if(hri_active)
  {
    ROS_INFO("QNode::processAnswer: Hri active. Cancelling goal.");
    hri_action.cancelGoal();
    std::string error(":(");
    return error;
  }
  else
  {
    if(unsigned(question)<this->qas.size())
    {
      ROS_INFO("QNode::processAnswer: %s", (this->hri_prefix+this->qas[question].answer_hri[language]).c_str());
      xml_files[TTS_]       = this->hri_prefix+this->qas[question].answer_hri[language];
      xml_files[LEDS_]      = "";
      xml_files[HEAD_]      = "head_home.xml";
      xml_files[RIGHT_ARM_] = "right_arm_home.xml";
      xml_files[LEFT_ARM_]  = "left_arm_home.xml";
      hriMakeActionRequest(xml_files);
      return this->qas[question].answer[language];
    }
    else
    {
      ROS_ERROR("QNode::processAnswer: question index out of bounds");
      std::string error(":(");
      return error;
    }
  }
}

//hri action client functions
void QNode::hriDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result)
{
  if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("QNode::HRIDone: Goal Achieved!");
  else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
    ROS_INFO("QNode::HRIDone: Goal aborted!");
  else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
    ROS_INFO("QNode::HRIDone: Goal preempted!");
  else
    ROS_INFO("QNode::HRIDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
  this->hri_active=false;
}

void QNode::hriActive()
{
  ROS_INFO("QNode::hriActive: Goal just went active!"); 
  this->hri_active=true;
}

void QNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback)
{
  //ROS_INFO("QNode::hriFeedback: feedback received!");
  bool feedback_is_ok = true;
  if( !feedback_is_ok )
    hri_action.cancelGoal();
}

void QNode::hriMakeActionRequest(std::vector<std::string> &xml_files) 
{
   hri_action.cancelGoal();
   ROS_INFO("QNode::hriMakeActionRequest: Starting New Request. Wait for server!"); 
   hri_action.waitForServer();
   this->hri_goal.sequence_file[TTS_]       = xml_files[TTS_];
   this->hri_goal.sequence_file[LEDS_]      = xml_files[LEDS_];
   this->hri_goal.sequence_file[HEAD_]      = xml_files[HEAD_];
   this->hri_goal.sequence_file[RIGHT_ARM_] = xml_files[RIGHT_ARM_];
   this->hri_goal.sequence_file[LEFT_ARM_]  = xml_files[LEFT_ARM_];
   hri_action.sendGoal(this->hri_goal, 
               boost::bind(&QNode::hriDone,     this, _1, _2), 
               boost::bind(&QNode::hriActive,   this), 
               boost::bind(&QNode::hriFeedback, this, _1)); 
   ROS_INFO("QNode::hriMakeActionRequest: Goal Sent. Wait for Result!"); 
}


//haod action client functions
void QNode::haodDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::haodResultConstPtr& result)
{
  if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("QNode::HRIDone: Goal Achieved!");
  else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
    ROS_INFO("QNode::HRIDone: Goal aborted!");
  else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
    ROS_INFO("QNode::HRIDone: Goal preempted!");
  else
    ROS_INFO("QNode::HRIDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
  this->haod_active=false;
  this->assistance=false;
}

void QNode::haodActive()
{
  ROS_INFO("QNode::haodActive: Goal just went active!"); 
  this->haod_active=true;
}

void QNode::haodFeedback(const tibi_dabo_msgs::haodFeedbackConstPtr& feedback)
{
  //ROS_INFO("QNode::haodFeedback: feedback received!");
  bool feedback_is_ok = true;
  this->assistance=feedback->assistance;
  if( !feedback_is_ok )
    haod_action.cancelGoal();
}

void QNode::haodMakeActionRequest(std::vector<int> &point) 
{
   ROS_INFO("QNode::haodMakeActionRequest: Starting New Request. Wait for server!"); 
   //haod_action.waitForServer();
   this->haod_goal.x=point[0];
   this->haod_goal.y=point[1];
   haod_action.sendGoal(this->haod_goal, 
               boost::bind(&QNode::haodDone,     this, _1, _2), 
               boost::bind(&QNode::haodActive,   this), 
               boost::bind(&QNode::haodFeedback, this, _1)); 
   ROS_INFO("QNode::haodMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void QNode::setImagePoint(std::vector<int> point)
{
  ROS_INFO("QNode::setImagePoint: x,y = %d, %d", point[0], point[1]);
  //start haod action with xy coordinates
  haodMakeActionRequest(point);
  this->imageDetectReady=false;
}

void QNode::haodButtonPressed(int button)
{
  ROS_INFO("QNode::haodButtonPressed: %d", button);
  sensor_msgs::Joy msg;
  msg.buttons.resize(11);
  msg.buttons[button] = 1;
  joy_publisher.publish(msg);
}

bool QNode::needAssistance()
{
  return assistance;
}

void QNode::cancelHri()
{
  if(this->hri_active)
    hri_action.cancelGoal();
}

bool QNode::haodRunning()
{
  return this->haod_active;
}

std::vector<std::vector<int> > QNode::getRects()
{
  std::vector<std::vector<int> > myRects;
  this->mutex.lock();
  myRects=this->rects;
  this->mutex.unlock();
  return myRects;
}

void QNode::setLanguage(int index)
{
  this->language=index;
  this->setHriPrefix(index);

  //publish Joy with index+1 (1,2,3) on button PLUS to tell Language change
  sensor_msgs::Joy msg;
  msg.buttons.resize(11);
  msg.buttons[wiimote::State::MSG_BTN_PLUS] = index+1;
  joy_publisher.publish(msg);
}

void QNode::setHriPrefix(int index)
{
  if(index==0)
  {
    this->hri_prefix="\\language=Catalan \\voice=Montserrat ";
  }
  else if(index==1)
  {
    this->hri_prefix="\\language=English \\voice=Kate ";
  }
  else if(index==2)
  {
    this->hri_prefix="\\language=Spanish \\voice=Leonor ";
  }
}

void QNode::loadQuestions()
{
  qa_data my_qa;

  my_qa.question.push_back("Qui ets?");
  my_qa.question.push_back("Who are you?");
  my_qa.question.push_back("¿Quién eres?");  
  my_qa.answer.push_back("Sóc la Tibi, un robot social i urbà.");
  my_qa.answer.push_back("I am Tibi, an urban and social robot.");
  my_qa.answer.push_back("Soy Tibi, un robot social y urbano.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Soc la Tibi, un robot social i urba.");
  my_qa.answer_hri.push_back(this->hri_prefix +"I am Tibi, an urban and social robot.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Soy Tibi, un robot social y urbano.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();

  my_qa.question.push_back("Què fas?");
  my_qa.question.push_back("What do you do?");
  my_qa.question.push_back("¿Qué haces?");  
  my_qa.answer.push_back("Interacciono amb gent i em puc moure per entorns urbans.");
  my_qa.answer.push_back("I interact with people and i can move through urban environments.");
  my_qa.answer.push_back("Interacciono con personas y me muevo por entornos urbanos.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Interacciono amb gent i em puc moure per entorns urbans.");
  my_qa.answer_hri.push_back(this->hri_prefix +"I interact with people and i can move through urban environments.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Interacciono con personas y me muevo por entornos urbanos.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();

  my_qa.question.push_back("D'on vens?");
  my_qa.question.push_back("Where do you come from?");
  my_qa.question.push_back("¿De dónde vienes?");  
  my_qa.answer.push_back("Vinc del Institut de Robòtica de la UPC i CSIC.");
  my_qa.answer.push_back("I come from the Robotics Institute of the UPC and CSIC.");
  my_qa.answer.push_back("Vengo del Instituto de Robótica de la UPC y CSIC.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Vinc del Institut de Robotica de la UPC i C SIC.");
  my_qa.answer_hri.push_back(this->hri_prefix +"I come from the Robotics Institut of the UPC and C SIC.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Vengo del Instituto de Robótica de la UPC y C SIC.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();
  
  my_qa.question.push_back("On i quan és el sopar de gala?");
  my_qa.question.push_back("Where and when is the gala dinner? ");
  my_qa.question.push_back("¿Dónde y cúando es la cena de gala?");  
  my_qa.answer.push_back("El sopar de gala serà el dijous a les 20 hores a l'Hotel España.");
  my_qa.answer.push_back("The gala dinner will be held on Thursday at 20 o'clock hours in Hotel España.");
  my_qa.answer.push_back("La cena de gala será el jueves a las 20 horas en el Hotel España.");
  my_qa.answer_hri.push_back(this->hri_prefix +"El sopar de gala serà el dijous a les 20 hores al Hotel España.");
  my_qa.answer_hri.push_back(this->hri_prefix +"The gala dinner will be held on Thursday at 20 o'clock hours in Hotel España.");
  my_qa.answer_hri.push_back(this->hri_prefix +"La cena de gala será el jueves a las 20 horas en el Hotel España.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();
  
  my_qa.question.push_back("On i quan és la visita cultural?");
  my_qa.question.push_back("Where and when is the cultural visit?");
  my_qa.question.push_back("¿Dónde y cúando es la visita cultural?");  
  my_qa.answer.push_back("La visita cultural serà el divendres a les 20h en el Museu Nacional d'Art de Catalunya.");
  my_qa.answer.push_back("The cultural visit will be held on Friday at 20 o'clock in the National Art Museum of Catalonia.");
  my_qa.answer.push_back("La visita cultural será el viernes a las 20h en el Museo Nacional de Arte de Cataluña.");
  my_qa.answer_hri.push_back(this->hri_prefix +"La visita cultural serà el divendres a les 20 hores en el Museu Nacional d'Art de Catalunya.");
  my_qa.answer_hri.push_back(this->hri_prefix +"The cultural visit will be held on Friday at 20 o'clock in the National Art Museum of Catalonia.");
  my_qa.answer_hri.push_back(this->hri_prefix +"La visita cultural será el viernes a las 20 horas en el Museo Nacional de Arte de Cataluña.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();

  my_qa.question.push_back("Hola!");
  my_qa.question.push_back("Hello!");
  my_qa.question.push_back("Hola!");  
  my_qa.answer.push_back("Hola. Benvinguts a la Conferència Europea de Robots Mòbils.");
  my_qa.answer.push_back("Hello. Welcome to the European Conference on Mobile Robots");
  my_qa.answer.push_back("Hola. Bienvenidos a la Conferencia Europea de Robots Móviles.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Hola. Benvinguts a la Conferència Europea de Robots Mòbils.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Hello. Welcome to the European Conference on Mobile Robots.");
  my_qa.answer_hri.push_back(this->hri_prefix +"Hola. Bienvenidos a la Conferencia Europea de Robóts Móviles.");
  this->qas.push_back(my_qa);

  my_qa.question.clear();
  my_qa.answer.clear();
  my_qa.answer_hri.clear();
}

}  // namespace tibi_dabo_gui