00001 #include "tibi_dabo_intro_alg_node.h"
00002 #include <wiimote/State.h>
00003
00004 TibiDaboIntroAlgNode::TibiDaboIntroAlgNode(void) :
00005 algorithm_base::IriBaseAlgorithm<TibiDaboIntroAlgorithm>(),
00006 hri_client_client_("hri_client", true)
00007 {
00008
00009
00010
00011
00012
00013
00014 this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 1, &TibiDaboIntroAlgNode::joy_callback, this);
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 this->hriActive =false;
00026 this->trigger_forward =false;
00027 this->trigger_backward=false;
00028 this->current_language=0;
00029 this->robot=1;
00030 this->hri_client_goal_.sequence_file.resize(5);
00031
00032 std::locale::global(std::locale(""));
00033 this->loadSequences();
00034 }
00035
00036 TibiDaboIntroAlgNode::~TibiDaboIntroAlgNode(void)
00037 {
00038
00039 }
00040
00041 void TibiDaboIntroAlgNode::mainNodeThread(void)
00042 {
00043 static int state=1;
00044
00045
00046
00047
00048
00049
00050
00051 alg_.lock();
00052 switch(state)
00053 {
00054 case 0:
00055 if(this->trigger_forward)
00056 {
00057
00058 {
00059 this->trigger_forward=false;
00060 state=1;
00061 }
00062 }
00063 else if(this->trigger_backward)
00064 {
00065 this->trigger_backward=false;
00066 state=6;
00067 }
00068 break;
00069 case 1:
00070 if(this->trigger_forward)
00071 {
00072 if(this->doHri(state))
00073 {
00074 this->trigger_forward=false;
00075 state=2;
00076 }
00077 }
00078 else if(this->trigger_backward)
00079 {
00080 this->trigger_backward=false;
00081 state=6;
00082 }
00083 break;
00084 case 2:
00085 if(this->trigger_forward)
00086 {
00087 if(this->doHri(state))
00088 {
00089 this->trigger_forward=false;
00090 state=4;
00091 }
00092 }
00093 else if(this->trigger_backward)
00094 {
00095 this->trigger_backward=false;
00096 state=1;
00097 }
00098 break;
00099 case 3:
00100 if(this->trigger_forward)
00101 {
00102
00103 {
00104 this->trigger_forward=false;
00105 state++;
00106 }
00107 }
00108 else if(this->trigger_backward)
00109 {
00110 this->trigger_backward=false;
00111 state--;
00112 }
00113 break;
00114 case 4:
00115 if(this->trigger_forward)
00116 {
00117 if(this->doHri(state))
00118 {
00119 this->trigger_forward=false;
00120 state=6;
00121 }
00122 }
00123 else if(this->trigger_backward)
00124 {
00125 this->trigger_backward=false;
00126 state=2;
00127 }
00128 break;
00129 case 5:
00130 if(this->trigger_forward)
00131 {
00132
00133 {
00134 this->trigger_forward=false;
00135 state++;
00136 }
00137 }
00138 else if(this->trigger_backward)
00139 {
00140 this->trigger_backward=false;
00141 state--;
00142 }
00143 break;
00144 case 6:
00145 if(this->trigger_forward)
00146 {
00147 if(this->doHri(state))
00148 {
00149 this->trigger_forward=false;
00150 state=1;
00151 }
00152 }
00153 else if(this->trigger_backward)
00154 {
00155 this->trigger_backward=false;
00156 state=4;
00157 }
00158 break;
00159 case 7:
00160 if(this->trigger_forward)
00161 {
00162
00163 {
00164 this->trigger_forward=false;
00165 state=0;
00166 }
00167 }
00168 else if(this->trigger_backward)
00169 {
00170 this->trigger_backward=false;
00171 state--;
00172 }
00173 break;
00174 }
00175
00176 this->alg_.unlock();
00177
00178
00179
00180
00181
00182
00183
00184
00185 }
00186
00187 bool TibiDaboIntroAlgNode::doHri(int state)
00188 {
00189 bool done=false;
00190 static int index = 0;
00191 int number = this->speech[state].size();
00192
00193 if(!hriActive)
00194 {
00195 if(index<number)
00196 {
00197 this->fillHriGoal(state,index);
00198 hri_clientMakeActionRequest();
00199 ROS_INFO("TibiDaboIntroAlgNode:doHri: speech[%d][%d][%d]: %s", state, index, this->current_language, this->hri_client_goal_.sequence_file[0].c_str());
00200 index++;
00201 }
00202 else
00203 {
00204 done=true;
00205 index=0;
00206 }
00207 }
00208 return done;
00209 }
00210
00211 void TibiDaboIntroAlgNode::fillHriGoal(int state, int index)
00212 {
00213 this->hri_client_goal_.sequence_file[0] = this->hri_prefix[this->robot][this->current_language] + this->speech[state][index][this->current_language];
00214 this->hri_client_goal_.sequence_file[1] = "";
00215 this->hri_client_goal_.sequence_file[2] = this->head_sequence[state][index][0];
00216 this->hri_client_goal_.sequence_file[3] = this->left_arm_sequence[state][index][0];
00217 this->hri_client_goal_.sequence_file[4] = this->right_arm_sequence[state][index][0];
00218 }
00219
00220
00221 void TibiDaboIntroAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg)
00222 {
00223
00224 static std::vector<int> prev_buttons(msg->buttons);
00225
00226 this->alg_.lock();
00227
00228 if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==1 && prev_buttons[wiimote::State::MSG_BTN_PLUS]==0)
00229 {
00230 ROS_INFO("TibiDaboIntroAlgNode::joy_callback: trigger forward");
00231 this->trigger_forward =true;
00232 this->trigger_backward=false;
00233 }
00234 else if(msg->buttons[wiimote::State::MSG_BTN_MINUS]==1 && prev_buttons[wiimote::State::MSG_BTN_MINUS]==0)
00235 {
00236 ROS_INFO("TibiDaboIntroAlgNode::joy_callback: trigger backward");
00237 this->trigger_backward=true;
00238 this->trigger_forward =false;
00239 }
00240 else if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1 && prev_buttons[wiimote::State::MSG_BTN_HOME]==0)
00241 {
00242 ROS_INFO("TibiDaboIntroAlgNode::joy_callback: cancel hri");
00243 this->hri_client_client_.cancelGoal();
00244 }
00245 prev_buttons=msg->buttons;
00246 this->alg_.unlock();
00247 }
00248
00249
00250
00251
00252 void TibiDaboIntroAlgNode::hri_clientDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::sequenceResultConstPtr& result)
00253 {
00254 this->alg_.lock();
00255 if( state.toString().compare("SUCCEEDED") == 0 )
00256 ROS_INFO("TibiDaboIntroAlgNode::hri_clientDone: Goal Achieved!");
00257 else
00258 ROS_INFO("TibiDaboIntroAlgNode::hri_clientDone: %s", state.toString().c_str());
00259
00260
00261 this->hriActive=false;
00262 this->alg_.unlock();
00263 }
00264
00265 void TibiDaboIntroAlgNode::hri_clientActive()
00266 {
00267 this->alg_.lock();
00268
00269 this->hriActive=true;
00270 this->alg_.unlock();
00271 }
00272
00273 void TibiDaboIntroAlgNode::hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback)
00274 {
00275 this->alg_.lock();
00276
00277
00278 bool feedback_is_ok = true;
00279
00280
00281
00282
00283
00284 if( !feedback_is_ok )
00285 {
00286 hri_client_client_.cancelGoal();
00287
00288 }
00289 this->alg_.unlock();
00290 }
00291
00292
00293 void TibiDaboIntroAlgNode::hri_clientMakeActionRequest()
00294 {
00295 ROS_INFO("TibiDaboIntroAlgNode::hri_clientMakeActionRequest: Starting New Request!");
00296
00297
00298
00299 hri_client_client_.waitForServer();
00300 ROS_INFO("TibiDaboIntroAlgNode::hri_clientMakeActionRequest: Server is Available!");
00301
00302
00303
00304 hri_client_client_.sendGoal(hri_client_goal_,
00305 boost::bind(&TibiDaboIntroAlgNode::hri_clientDone, this, _1, _2),
00306 boost::bind(&TibiDaboIntroAlgNode::hri_clientActive, this),
00307 boost::bind(&TibiDaboIntroAlgNode::hri_clientFeedback, this, _1));
00308 ROS_INFO("TibiDaboIntroAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!");
00309 this->hriActive=true;
00310 }
00311
00312 void TibiDaboIntroAlgNode::loadSequences()
00313 {
00314 std::vector < std::vector < std::string > > sequence;
00315 std::vector < std::string > sentence;
00316
00317
00318 sentence.push_back("Hola, bienvenidos al laboratorio de robótica móvil.");
00319 sentence.push_back("Hola, benvinguts al laboratori de robòtica mòbil.");
00320 sentence.push_back("Hello, welcome to the mobile robotics laboratory.");
00321 sequence.push_back(sentence);
00322 sentence.clear();
00323 sentence.push_back("Ahora os presentaremos algunos de los robots que tenemos por aquí.");
00324 sentence.push_back("Ara us presentarem alguns dels robots que tenim per aquí.");
00325 sentence.push_back("Now we will present some of the robots we have here.");
00326 sequence.push_back(sentence);
00327 sentence.clear();
00328 this->speech.push_back(sequence);
00329 sequence.clear();
00330
00331
00332 sentence.push_back("Yo soy Dabo. Un robot de servicio, social, y urbano.");
00333 sentence.push_back("Jo sóc Dàbo. Un robot de servei, social, i urbà.");
00334 sentence.push_back("I am Dabo. A service, social, and urban robot.");
00335 sequence.push_back(sentence);
00336 sentence.clear();
00337 this->speech.push_back(sequence);
00338 sequence.clear();
00339
00340
00341 sentence.push_back("Continuemos. Por aquí tenemos a Teo. Es un robot todo terreno de cuatro ruedas, que puede hacer mapas 3 d.");
00342 sentence.push_back("Continuem. Per aquí tenim el el Téóh. Es un robot tot terreny de quatre rodes, que pot fer mapes 3 d.");
00343 sentence.push_back("Let's continue. Here we have Teo. It's an all terrain robot with four wheels.");
00344 sequence.push_back(sentence);
00345 sentence.clear();
00346 this->speech.push_back(sequence);
00347 sequence.clear();
00348
00349
00350 sentence.push_back("Ahora os enseñaremos los humanoides.");
00351 sentence.push_back("Ara us ensenyarem els humanoides.");
00352 sentence.push_back("Now we will show you the humanoids.");
00353 sequence.push_back(sentence);
00354 sentence.clear();
00355 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.");
00356 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.");
00357 sentence.push_back("They are little robots with legs and arms who can walk, climb stairs, play sumo, and even soccer.");
00358 sequence.push_back(sentence);
00359 sentence.clear();
00360 this->speech.push_back(sequence);
00361 sequence.clear();
00362
00363
00364 sentence.push_back("Y aquí veis a Kinton, nuestro robot volador. Es como un helicóptero con cuatro hélices.");
00365 sentence.push_back("I aquí veieu el kíntoun, el nostre robot volador. És com un helicòpter amb quatre hèlices.");
00366 sentence.push_back("And here you can see Kinton, our flying robot. Is like a helicopter with four helixs.");
00367 sequence.push_back(sentence);
00368 sentence.clear();
00369 sentence.push_back("Aunque no puede volar aquí, lo utilizamos en investigación de vehículos aéreos, y vuelo autònomo.");
00370 sentence.push_back("Encara que aquí no pot volar, el fem servir en investigació de vehicles aeris i, vol autònom.");
00371 sentence.push_back("Although it can not fly here, we use it for air vehicle research and autonomous flight.");
00372 sequence.push_back(sentence);
00373 sentence.clear();
00374 this->speech.push_back(sequence);
00375 sequence.clear();
00376
00377
00378 sentence.push_back("Si teneis preguntas, ahora es el momento. No dudeis en preguntar, intentaremos responderlas todas.");
00379 sentence.push_back("Si teniu preguntes, ara és el moment. No dubteu en demanar-les, intentarem resoldre-les totes.");
00380 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.");
00381 sequence.push_back(sentence);
00382 sentence.clear();
00383 this->speech.push_back(sequence);
00384 sequence.clear();
00385
00386
00387 sentence.push_back("Muchas gracias por haber venido. Esperamos que os haya gustado esta visita virtual.");
00388 sentence.push_back("Moltes gràcies per haver vingut. Esperem que us hagi agradat aquesta visita virtual.");
00389 sentence.push_back("Thank you for coming, We hope you have enjoyed this virtual visit.");
00390 sequence.push_back(sentence);
00391 sentence.clear();
00392 this->speech.push_back(sequence);
00393 sequence.clear();
00394
00395
00396 sentence.push_back("Ha sido un placer. Hasta la próxima. Adiós!");
00397 sentence.push_back("Ha estat un plaer. Fins la propera. Adeu!");
00398 sentence.push_back("It's been a pleasure. See you next time. Bye!");
00399 sequence.push_back(sentence);
00400 sentence.clear();
00401 this->speech.push_back(sequence);
00402 sequence.clear();
00403
00404 int max_sentences = 2;
00405
00406 sentence.resize(3,"head_home.xml");
00407 sequence.resize(max_sentences,sentence);
00408 sentence.clear();
00409 this->head_sequence.resize(8,sequence);
00410 sequence.clear();
00411
00412
00413 sentence.resize(3,"left_arm_home.xml");
00414 sequence.resize(max_sentences,sentence);
00415 sentence.clear();
00416 this->left_arm_sequence.resize(8,sequence);
00417 sequence.clear();
00418
00419
00420 sentence.resize(3,"right_arm_home.xml");
00421 sequence.resize(max_sentences,sentence);
00422 sentence.clear();
00423 this->right_arm_sequence.resize(8,sequence);
00424 sequence.clear();
00425
00426
00427
00428 this->head_sequence[0][0][0] ="head_hi.xml";
00429 this->left_arm_sequence[0][0][0] ="left_arm_home.xml";
00430 this->right_arm_sequence[0][0][0]="right_arm_home.xml";
00431
00432 this->head_sequence[0][1][0] ="head_home.xml";
00433 this->left_arm_sequence[0][1][0] ="left_arm_home.xml";
00434 this->right_arm_sequence[0][1][0]="right_arm_home.xml";
00435
00436 this->head_sequence[1][0][0] ="head_talk_circle_6s.xml";
00437 this->left_arm_sequence[1][0][0] ="left_arm_talk.xml";
00438 this->right_arm_sequence[1][0][0]="right_arm_talk.xml";
00439
00440 this->head_sequence[1][1][0] ="head_talk_l_4s.xml";
00441
00442
00443
00444 this->head_sequence[2][0][0] = "head_down.xml";
00445
00446 this->head_sequence[3][0][0] ="head_talk_right_6s.xml";
00447 this->left_arm_sequence[3][0][0] ="left_arm_right.xml";
00448 this->right_arm_sequence[3][0][0]="right_arm_right.xml";
00449
00450 this->head_sequence[4][0][0] = "head_talk_l_4s.xml";
00451
00452 this->head_sequence[5][0][0] = "head_yes_4s.xml";
00453
00454 this->head_sequence[6][0][0] = "head_roll.xml";
00455
00456 this->head_sequence[7][0][0] = "head_side.xml";
00457
00458
00459 sentence.resize(3,"left_arm_home.xml");
00460 sequence.resize(max_sentences,sentence);
00461 sentence.clear();
00462 this->left_arm_sequence.resize(8,sequence);
00463 sequence.clear();
00464
00465
00466 sentence.resize(3,"right_arm_home.xml");
00467 sequence.resize(max_sentences,sentence);
00468 sentence.clear();
00469 this->right_arm_sequence.resize(8,sequence);
00470 sequence.clear();
00471
00472
00473 std::vector < std::string > prefix;
00474 prefix.push_back("\\language=Spanish \\voice=Leonor ");
00475 prefix.push_back("\\language=Catalan \\voice=Montserrat ");
00476 prefix.push_back("\\language=English \\voice=Kate ");
00477 this->hri_prefix.push_back(prefix);
00478 prefix.clear();
00479 prefix.push_back("\\language=Spanish \\voice=Jorge ");
00480 prefix.push_back("\\language=Catalan \\voice=Jordi ");
00481 prefix.push_back("\\language=English \\voice=Simon ");
00482 this->hri_prefix.push_back(prefix);
00483 prefix.clear();
00484 }
00485
00486 void TibiDaboIntroAlgNode::node_config_update(Config &config, uint32_t level)
00487 {
00488 this->alg_.lock();
00489 if(config.trigger_forward)
00490 {
00491 this->trigger_forward=config.trigger_forward;
00492 config.trigger_forward=false;
00493 }
00494 else if(config.trigger_backward)
00495 {
00496 this->trigger_backward=config.trigger_backward;
00497 config.trigger_backward=false;
00498 }
00499 this->current_language=config.language;
00500 if(config.robot=="tibi")
00501 {
00502 this->robot=0;
00503 }
00504 else if(config.robot=="dabo")
00505 {
00506 this->robot=1;
00507 }
00508 else
00509 {
00510 ROS_ERROR("TibiDaboIntroAlgNode::node_config_update: wrong parameter robot, must be tibi or dabo");
00511 config.robot="tibi";
00512 }
00513 if(config.stop_hri)
00514 {
00515 this->hri_client_client_.cancelGoal();
00516 config.stop_hri=false;
00517 }
00518 this->alg_.unlock();
00519 }
00520
00521 void TibiDaboIntroAlgNode::addNodeDiagnostics(void)
00522 {
00523 }
00524
00525
00526 int main(int argc,char *argv[])
00527 {
00528 return algorithm_base::main<TibiDaboIntroAlgNode>(argc, argv, "tibi_dabo_intro_alg_node");
00529 }