00001
00009
00010
00011
00012
00013 #include <ros/ros.h>
00014 #include <ros/network.h>
00015 #include <string>
00016 #include <sstream>
00017 #include "../include/tibi_dabo_gui/qnode.hpp"
00018
00019
00020
00021
00022
00023 namespace tibi_dabo_gui {
00024
00025
00026
00027
00028
00029 QNode::QNode(int argc, char** argv ) :
00030 init_argc(argc),
00031 init_argv(argv),
00032 hri_action("hri",true),
00033 haod_action("haod",true),
00034 xml_files(NUM_CLIENTS, std::string(""))
00035 {}
00036
00037 QNode::~QNode() {
00038 if(ros::isStarted()) {
00039 ros::shutdown();
00040 ros::waitForShutdown();
00041 }
00042 wait();
00043 }
00044
00045 bool QNode::initialize()
00046 {
00047 if ( ! ros::master::check() ) {
00048 return false;
00049 }
00050 ros::start();
00051 ros::NodeHandle n;
00052
00053 joy_publisher = n.advertise<sensor_msgs::Joy>("joy", 1000);
00054 image_subscriber = n.subscribe("image", 1000, &QNode::imageCallback, this);
00055 imageDetect_subscriber = n.subscribe("imageDetect", 1000, &QNode::imageDetectCallback, this);
00056 faces_subscriber = n.subscribe("faces", 1000, &QNode::facesCallback, this);
00057
00058 hri_goal.sequence_file = std::vector<std::string>(NUM_CLIENTS, std::string(""));
00059 hri_goal.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);
00060
00061 this->loadQuestions();
00062 this->imageReady=false;
00063 this->imageDetectReady=false;
00064 this->hri_active=false;
00065 this->haod_active=false;
00066 this->assistance=false;
00067
00068 start();
00069 return true;
00070 }
00071
00072 void QNode::imageCallback(const sensor_msgs::Image::ConstPtr &msg)
00073 {
00074 this->image = *msg;
00075 this->imageReady = true;
00076 }
00077
00078 void QNode::imageDetectCallback(const sensor_msgs::Image::ConstPtr &msg)
00079 {
00080 this->imageDetect = *msg;
00081 this->imageDetectReady = true;
00082 }
00083
00084 void QNode::facesCallback(const face_detector_mono::RectArray::ConstPtr &msg)
00085 {
00086 this->mutex.lock();
00087
00088 this->rects.clear();
00089 for(unsigned int i=0; i<msg->rects.size(); i++)
00090 {
00091 std::vector<int> rect;
00092 rect.push_back(msg->rects[i].x);
00093 rect.push_back(msg->rects[i].y);
00094 rect.push_back(msg->rects[i].width);
00095 rect.push_back(msg->rects[i].height);
00096 this->rects.push_back(rect);
00097 }
00098 this->mutex.unlock();
00099 }
00100
00101 void QNode::run() {
00102 ros::Rate loop_rate(100);
00103 int count = 0;
00104 while ( ros::ok() )
00105 {
00106 ros::spinOnce();
00107 loop_rate.sleep();
00108 ++count;
00109 }
00110 std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00111 Q_EMIT rosShutdown();
00112 }
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 bool QNode::getImage(uchar* & data, int & width, int & height, std::string & encoding)
00124 {
00125 if(imageReady)
00126 {
00127 data = &this->image.data[0];
00128 width = this->image.width;
00129 height = this->image.height;
00130 encoding = this->image.encoding;
00131 }
00132 return imageReady;
00133 }
00134
00135 bool QNode::getImageDetect(uchar* & data, int & width, int & height, std::string & encoding)
00136 {
00137 if(imageDetectReady)
00138 {
00139 data = &this->imageDetect.data[0];
00140 width = this->imageDetect.width;
00141 height = this->imageDetect.height;
00142 encoding = this->imageDetect.encoding;
00143 }
00144 return imageDetectReady;
00145 }
00146
00147 std::string QNode::processAnswer(int question)
00148 {
00149 if(hri_active)
00150 {
00151 ROS_INFO("QNode::processAnswer: Hri active. Cancelling goal.");
00152 hri_action.cancelGoal();
00153 std::string error("...");
00154 return error;
00155 }
00156 else
00157 {
00158 if(unsigned(question)<this->qas.size())
00159 {
00160 ROS_INFO("QNode::processAnswer: %s", (this->hri_prefix+this->qas[question].answer_hri[language]).c_str());
00161 xml_files[TTS_] = this->hri_prefix+this->qas[question].answer_hri[language];
00162 xml_files[LEDS_] = "";
00163 xml_files[HEAD_] = "head_home.xml";
00164 xml_files[RIGHT_ARM_] = "right_arm_home.xml";
00165 xml_files[LEFT_ARM_] = "left_arm_home.xml";
00166 hriMakeActionRequest(xml_files);
00167 return this->qas[question].answer[language];
00168 }
00169 else
00170 {
00171 ROS_ERROR("QNode::processAnswer: question index out of bounds");
00172 std::string error(":(");
00173 return error;
00174 }
00175 }
00176 }
00177
00178
00179 void QNode::hriDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::sequenceResultConstPtr& result)
00180 {
00181 if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
00182 ROS_INFO("QNode::HRIDone: Goal Achieved!");
00183 else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
00184 ROS_INFO("QNode::HRIDone: Goal aborted!");
00185 else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
00186 ROS_INFO("QNode::HRIDone: Goal preempted!");
00187 else
00188 ROS_INFO("QNode::HRIDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
00189 this->hri_active=false;
00190 }
00191
00192 void QNode::hriActive()
00193 {
00194 ROS_INFO("QNode::hriActive: Goal just went active!");
00195 this->hri_active=true;
00196 }
00197
00198 void QNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback)
00199 {
00200
00201 bool feedback_is_ok = true;
00202 if( !feedback_is_ok )
00203 hri_action.cancelGoal();
00204 }
00205
00206 void QNode::hriMakeActionRequest(std::vector<std::string> &xml_files)
00207 {
00208 hri_action.cancelGoal();
00209 ROS_INFO("QNode::hriMakeActionRequest: Starting New Request. Wait for server!");
00210 hri_action.waitForServer();
00211 this->hri_goal.sequence_file[TTS_] = xml_files[TTS_];
00212 this->hri_goal.sequence_file[LEDS_] = xml_files[LEDS_];
00213 this->hri_goal.sequence_file[HEAD_] = xml_files[HEAD_];
00214 this->hri_goal.sequence_file[RIGHT_ARM_] = xml_files[RIGHT_ARM_];
00215 this->hri_goal.sequence_file[LEFT_ARM_] = xml_files[LEFT_ARM_];
00216 hri_action.sendGoal(this->hri_goal,
00217 boost::bind(&QNode::hriDone, this, _1, _2),
00218 boost::bind(&QNode::hriActive, this),
00219 boost::bind(&QNode::hriFeedback, this, _1));
00220 ROS_INFO("QNode::hriMakeActionRequest: Goal Sent. Wait for Result!");
00221 }
00222
00223
00224
00225 void QNode::haodDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::haodResultConstPtr& result)
00226 {
00227 if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
00228 ROS_INFO("QNode::HRIDone: Goal Achieved!");
00229 else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
00230 ROS_INFO("QNode::HRIDone: Goal aborted!");
00231 else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
00232 ROS_INFO("QNode::HRIDone: Goal preempted!");
00233 else
00234 ROS_INFO("QNode::HRIDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
00235 this->haod_active=false;
00236 this->assistance=false;
00237 }
00238
00239 void QNode::haodActive()
00240 {
00241 ROS_INFO("QNode::haodActive: Goal just went active!");
00242 this->haod_active=true;
00243 }
00244
00245 void QNode::haodFeedback(const tibi_dabo_msgs::haodFeedbackConstPtr& feedback)
00246 {
00247
00248 bool feedback_is_ok = true;
00249 this->assistance=feedback->assistance;
00250 if( !feedback_is_ok )
00251 haod_action.cancelGoal();
00252 }
00253
00254 void QNode::haodMakeActionRequest(std::vector<int> &point)
00255 {
00256 ROS_INFO("QNode::haodMakeActionRequest: Starting New Request. Wait for server!");
00257
00258 this->haod_goal.x=point[0];
00259 this->haod_goal.y=point[1];
00260 haod_action.sendGoal(this->haod_goal,
00261 boost::bind(&QNode::haodDone, this, _1, _2),
00262 boost::bind(&QNode::haodActive, this),
00263 boost::bind(&QNode::haodFeedback, this, _1));
00264 ROS_INFO("QNode::haodMakeActionRequest: Goal Sent. Wait for Result!");
00265 }
00266
00267 void QNode::setImagePoint(std::vector<int> point)
00268 {
00269 ROS_INFO("QNode::setImagePoint: x,y = %d, %d", point[0], point[1]);
00270
00271 haodMakeActionRequest(point);
00272 this->imageDetectReady=false;
00273 }
00274
00275 void QNode::haodButtonPressed(int button)
00276 {
00277 ROS_INFO("QNode::haodButtonPressed: %d", button);
00278 sensor_msgs::Joy msg;
00279 msg.buttons.resize(11);
00280 msg.buttons[button] = 1;
00281 joy_publisher.publish(msg);
00282 }
00283
00284 bool QNode::needAssistance()
00285 {
00286 return assistance;
00287 }
00288
00289 void QNode::cancelHri()
00290 {
00291 if(this->hri_active)
00292 hri_action.cancelGoal();
00293 }
00294
00295 bool QNode::haodRunning()
00296 {
00297 return this->haod_active;
00298 }
00299
00300 std::vector<std::vector<int> > QNode::getRects()
00301 {
00302 std::vector<std::vector<int> > myRects;
00303 this->mutex.lock();
00304 myRects=this->rects;
00305 this->mutex.unlock();
00306 return myRects;
00307 }
00308
00309 void QNode::setLanguage(int index)
00310 {
00311 this->language=index;
00312 this->setHriPrefix(index);
00313
00314
00315 sensor_msgs::Joy msg;
00316 msg.buttons.resize(11);
00317 msg.buttons[wiimote::State::MSG_BTN_PLUS] = index+1;
00318 joy_publisher.publish(msg);
00319 }
00320
00321 void QNode::setHriPrefix(int index)
00322 {
00323 if(index==0)
00324 {
00325 this->hri_prefix="\\language=Catalan \\voice=Montserrat ";
00326 }
00327 else if(index==1)
00328 {
00329 this->hri_prefix="\\language=English \\voice=Kate ";
00330 }
00331 else if(index==2)
00332 {
00333 this->hri_prefix="\\language=Spanish \\voice=Leonor ";
00334 }
00335 }
00336
00337 void QNode::loadQuestions()
00338 {
00339 qa_data my_qa;
00340
00341 my_qa.question.push_back("Qui ets?");
00342 my_qa.question.push_back("Who are you?");
00343 my_qa.question.push_back("¿Quién eres?");
00344 my_qa.answer.push_back("Sóc la Tibi, un robot de servei, social i urbà.");
00345 my_qa.answer.push_back("I am Tibi, a service urban and social robot.");
00346 my_qa.answer.push_back("Soy Tibi, un robot de servicio, social y urbano .");
00347 my_qa.answer_hri.push_back(this->hri_prefix +"Soc la Tibi, un robot de servei, social i urba.");
00348 my_qa.answer_hri.push_back(this->hri_prefix +"I am Tibi, a service, urban, and social robot.");
00349 my_qa.answer_hri.push_back(this->hri_prefix +"Soy Ti bi, un robot de servicio, social y urbano.");
00350 this->qas.push_back(my_qa);
00351
00352 my_qa.question.clear();
00353 my_qa.answer.clear();
00354 my_qa.answer_hri.clear();
00355
00356 my_qa.question.push_back("Què fas?");
00357 my_qa.question.push_back("What do you do?");
00358 my_qa.question.push_back("¿Qué haces?");
00359 my_qa.answer.push_back("Interacciono amb gent i em puc moure per entorns urbans.");
00360 my_qa.answer.push_back("I interact with people and I can move through urban environments.");
00361 my_qa.answer.push_back("Interacciono con personas y me muevo por entornos urbanos.");
00362 my_qa.answer_hri.push_back(this->hri_prefix +"Interacciono amb gent i em puc moure per entorns urbans.");
00363 my_qa.answer_hri.push_back(this->hri_prefix +"I interact with people and I can move through urban environments.");
00364 my_qa.answer_hri.push_back(this->hri_prefix +"Interacciono con personas y me muevo por entornos urbanos.");
00365 this->qas.push_back(my_qa);
00366
00367 my_qa.question.clear();
00368 my_qa.answer.clear();
00369 my_qa.answer_hri.clear();
00370
00371 my_qa.question.push_back("D'on vens?");
00372 my_qa.question.push_back("Where do you come from?");
00373 my_qa.question.push_back("¿De dónde vienes?");
00374 my_qa.answer.push_back("Vinc del Institut de Robòtica de la universitat UPC i CSIC.");
00375 my_qa.answer.push_back("I come from the Robotics Institute of the UPC university and CSIC.");
00376 my_qa.answer.push_back("Vengo del Instituto de Robótica de la universidad UPC y CSIC.");
00377 my_qa.answer_hri.push_back(this->hri_prefix +"Vinc del Institut de Robotica de la universitat UPC i C SIC.");
00378 my_qa.answer_hri.push_back(this->hri_prefix +"I come from the Robotics Institut of the UPC university and C SIC.");
00379 my_qa.answer_hri.push_back(this->hri_prefix +"Vengo del Instituto de Robótica de la universidad UPC y C SIC.");
00380 this->qas.push_back(my_qa);
00381
00382 my_qa.question.clear();
00383 my_qa.answer.clear();
00384 my_qa.answer_hri.clear();
00385
00386 my_qa.question.push_back("Quan és el workshop Echord++?");
00387 my_qa.question.push_back("When is the Echord++ workshop? ");
00388 my_qa.question.push_back("¿Cúando es el workshop Echord++?");
00389 my_qa.answer.push_back("El workshop Echord++ és aquesta tarda a les 15 hores a la sala de reunions C.");
00390 my_qa.answer.push_back("The workshop Echord++ is this afternoon at 3 o'clock in the meeting room C.");
00391 my_qa.answer.push_back("El workshop Echord++ es esta tarde las 15 horas en la sala de reuniones C.");
00392 my_qa.answer_hri.push_back(this->hri_prefix +"El gu orc xop és aquesta tarda a les 15 hores a la sala de reunions C.");
00393 my_qa.answer_hri.push_back(this->hri_prefix +"The work shop is this after noon at 3 o'clock in the meeting room C.");
00394 my_qa.answer_hri.push_back(this->hri_prefix +"El uorc xop es esta tarde a las 15 horas en la sala de reuniones C.");
00395 this->qas.push_back(my_qa);
00396
00397 my_qa.question.clear();
00398 my_qa.answer.clear();
00399 my_qa.answer_hri.clear();
00400
00401 my_qa.question.push_back("On et puc trobar?");
00402 my_qa.question.push_back("Where can I find you?");
00403 my_qa.question.push_back("¿Dónde puedo encontrarte?");
00404 my_qa.answer.push_back("Estem al stand de la universitat UPC.");
00405 my_qa.answer.push_back("We are at the UPC university stand.");
00406 my_qa.answer.push_back("Estamos en el stand de la universidad UPC.");
00407 my_qa.answer_hri.push_back(this->hri_prefix +"Estem al stand de la universitat U P C.");
00408 my_qa.answer_hri.push_back(this->hri_prefix +"We are at the U P C university stand.");
00409 my_qa.answer_hri.push_back(this->hri_prefix +"Estamos en el stand de la universidad U P C.");
00410 this->qas.push_back(my_qa);
00411
00412 my_qa.question.clear();
00413 my_qa.answer.clear();
00414 my_qa.answer_hri.clear();
00415
00416 my_qa.question.push_back("Hola!");
00417 my_qa.question.push_back("Hello!");
00418 my_qa.question.push_back("Hola!");
00419 my_qa.answer.push_back("Hola. Benvinguts al congrés mundial Smart City Expo 2013.");
00420 my_qa.answer.push_back("Hello. Welcome to the Smart City Expo World Congress 2013.");
00421 my_qa.answer.push_back("Hola. Bienvenidos al congreso mundial Smart City Expo 2013.");
00422 my_qa.answer_hri.push_back(this->hri_prefix +"Hola. Benvinguts al congrés mundial Smart City Expo 2013.");
00423 my_qa.answer_hri.push_back(this->hri_prefix +"Hello. Welcome to the Smart City Expo World Congress 2013.");
00424 my_qa.answer_hri.push_back(this->hri_prefix +"Hola. Bienvenidos al congreso mundial Smart City Expo 2013.");
00425 this->qas.push_back(my_qa);
00426
00427 my_qa.question.clear();
00428 my_qa.answer.clear();
00429 my_qa.answer_hri.clear();
00430 }
00431
00432 }