tibi_dabo_haod_alg_node.cpp
Go to the documentation of this file.
00001 #include "tibi_dabo_haod_alg_node.h"
00002 #include "wiimote/State.h"
00003 #include <stdlib.h>
00004 
00005 std::string catalan_speech[]={"\\language=Catalan \\voice=Montserrat Per començar selecciona una de les cares detectades a la imatge, i llavors em pots ajudar a millorar les meves deteccions",
00006                               "\\language=Catalan \\voice=Montserrat Ho sento, no puc detectar cap cara en aquest punt.", 
00007                               "\\language=Catalan \\voice=Montserrat Gracies per la teva ajuda. Adeu!",
00008                               "\\language=Catalan \\voice=Montserrat És correcte la detecció?",
00009                               "\\language=Catalan \\voice=Montserrat El rectangle blau està sobre la teva cara?",
00010                               "\\language=Catalan \\voice=Montserrat Corretgeix-me si estic equivocada",
00011                               "\\language=Catalan \\voice=Montserrat Tinc un dubte, digue'm si és correcte"};
00012 
00013 std::string english_speech[]={"\\language=English \\voice=Kate To start select one of the faces detected in the image, and then you may help me improve my detections",
00014                               "\\language=English \\voice=Kate Sorry, I can not detect a face there.",
00015                               "\\language=English \\voice=Kate Thanks for your help. Bye!",
00016                               "\\language=English \\voice=Kate Is the detection correct?",
00017                               "\\language=English \\voice=Kate Is the blue rectangle on your face?",
00018                               "\\language=English \\voice=Kate Correct me if I'm wrong please.",
00019                               "\\language=English \\voice=Kate I have a doubt, tell me if is it correct?"};
00020 
00021 std::string spanish_speech[]={"\\language=Spanish \\voice=Leonor Para empezar selecciona una de las caras detectadad en la imagen, y entonces me puedes ayudar a mejhorar mis detecciones",
00022                               "\\language=Spanish \\voice=Leonor Lo siento, no detecto ninguna cara en ese punto.", 
00023                               "\\language=Spanish \\voice=Leonor Gracias por tu ayuda. Adios!",
00024                               "\\language=Spanish \\voice=Leonor Es correta la detección?",
00025                               "\\language=Spanish \\voice=Leonor El rectangulo azul está sobre tu cara?",
00026                               "\\language=Spanish \\voice=Leonor Corrigeme si estoy equivocada.",
00027                               "\\language=Spanish \\voice=Leonor Tengo una duda, dime si es correcto."};
00028 
00029 std::string head_motion[]={"",
00030                            "",
00031                            ""};
00032 
00033 std::string left_arm_motion[]={"left_arm_home.xml",
00034                                "left_arm_home.xml",
00035                                "left_arm_home.xml"};
00036 
00037 std::string right_arm_motion[]={"right_arm_home.xml",
00038                                 "right_arm_home.xml",
00039                                 "right_arm_home.xml"};
00040 
00041 TibiDaboHaodAlgNode::TibiDaboHaodAlgNode(void) :
00042   algorithm_base::IriBaseAlgorithm<TibiDaboHaodAlgorithm>(),
00043   haod_server_aserver_(public_node_handle_, "haod_server"),
00044   hri_client_client_("hri_client", true),
00045   follow_target_client_("follow_target", true)
00046 {
00047   //init class attributes if necessary
00048   this->loop_rate_ = 10;//in [Hz]
00049 
00050   // [init publishers]
00051   this->target_joints_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("target_joints", 10);
00052   
00053   // [init subscribers]
00054   this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 10, &TibiDaboHaodAlgNode::joy_callback, this);
00055   
00056   // [init services]
00057   
00058   // [init clients]
00059   haod_learn_client_ = this->public_node_handle_.serviceClient<iri_human_assisted_object_detection::haod_learn>("haod_learn");
00060   haod_detect_client_ = this->public_node_handle_.serviceClient<iri_human_assisted_object_detection::haod_detect>("haod_detect");
00061   haod_init_client_ = this->public_node_handle_.serviceClient<iri_human_assisted_object_detection::haod_init>("haod_init");
00062   
00063   // [init action servers]
00064   haod_server_aserver_.registerStartCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverStartCallback, this, _1)); 
00065   haod_server_aserver_.registerStopCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverStopCallback, this)); 
00066   haod_server_aserver_.registerIsFinishedCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverIsFinishedCallback, this)); 
00067   haod_server_aserver_.registerHasSucceedCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverHasSucceedCallback, this)); 
00068   haod_server_aserver_.registerGetResultCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverGetResultCallback, this, _1)); 
00069   haod_server_aserver_.registerGetFeedbackCallback(boost::bind(&TibiDaboHaodAlgNode::haod_serverGetFeedbackCallback, this, _1)); 
00070   haod_server_aserver_.start();
00071   
00072   // [init action clients]
00073   this->new_joy_msg=false;
00074   this->joy_action=none;
00075   this->language=english;
00076 
00077   this->new_haod_action=false;
00078   this->haod_need_help=false;
00079   this->stop_haod=false;
00080   this->init_x=0;
00081   this->init_y=0;
00082 
00083   this->hri_action_done=false;
00084   this->hri_client_goal_.sequence_file.resize(5,std::string(""));
00085   this->hri_client_goal_.sequence_file[2]="head_home.xml";
00086   hri_clientMakeActionRequest();
00087   while(!this->hri_action_done)
00088     usleep(100000);
00089   this->hri_client_goal_.sequence_file[2]="";
00090 
00091   this->tracking_head=false;
00092   this->JointTrajectoryPoint_msg_.positions.resize(2);
00093   this->JointTrajectoryPoint_msg_.positions[0]=0;
00094   this->JointTrajectoryPoint_msg_.positions[1]=0;
00095   this->JointTrajectoryPoint_msg_.velocities.resize(2);
00096   this->JointTrajectoryPoint_msg_.velocities[0]=2.61;
00097   this->JointTrajectoryPoint_msg_.velocities[1]=2.61;
00098 
00099   srand(time(NULL));
00100 }
00101 
00102 void TibiDaboHaodAlgNode::execute_hri_action(hri_actions action)
00103 {
00104   int random_answer=0;
00105 
00106   switch(action)
00107   {
00108     case hri_intro: if(this->language==catalan)
00109                       this->hri_client_goal_.sequence_file[0] = catalan_speech[0];
00110                     else if(this->language==english)
00111                       this->hri_client_goal_.sequence_file[0]  = english_speech[0];
00112                     else
00113                       this->hri_client_goal_.sequence_file[0] = spanish_speech[0];
00114                     this->hri_client_goal_.sequence_file[1] = "";
00115                     this->hri_client_goal_.sequence_file[2] = head_motion[0];
00116                     this->hri_client_goal_.sequence_file[4] = right_arm_motion[0];
00117                     this->hri_client_goal_.sequence_file[3]  = left_arm_motion[0];
00118                     hri_clientMakeActionRequest();
00119                     break;
00120     case hri_init: if(this->language==catalan)
00121                      this->hri_client_goal_.sequence_file[0] = catalan_speech[1];
00122                    else if(this->language==english)
00123                      this->hri_client_goal_.sequence_file[0]  = english_speech[1];
00124                    else
00125                      this->hri_client_goal_.sequence_file[0] = spanish_speech[1];
00126                    this->hri_client_goal_.sequence_file[1] = "";
00127                    this->hri_client_goal_.sequence_file[2] = head_motion[1];
00128                    this->hri_client_goal_.sequence_file[4] = right_arm_motion[1];
00129                    this->hri_client_goal_.sequence_file[3]  = left_arm_motion[1];
00130                    hri_clientMakeActionRequest();
00131                    break;
00132     case hri_help: // select a random speech
00133                    random_answer=((double)rand()/(double)RAND_MAX)*4+3;
00134                    if(this->language==catalan)
00135                      this->hri_client_goal_.sequence_file[0] = catalan_speech[random_answer];
00136                    else if(this->language==english)
00137                      this->hri_client_goal_.sequence_file[0]  = english_speech[random_answer];
00138                    else
00139                      this->hri_client_goal_.sequence_file[0] = spanish_speech[random_answer];
00140                    this->hri_client_goal_.sequence_file[1] = "";
00141                    this->hri_client_goal_.sequence_file[2] = head_motion[1];
00142                    this->hri_client_goal_.sequence_file[4] = right_arm_motion[1];
00143                    this->hri_client_goal_.sequence_file[3]  = left_arm_motion[1];
00144                    hri_clientMakeActionRequest();
00145                    break;
00146     case hri_exit: if(this->language==catalan)
00147                      this->hri_client_goal_.sequence_file[0] = catalan_speech[2];
00148                    else if(this->language==english)
00149                      this->hri_client_goal_.sequence_file[0]  = english_speech[2];
00150                    else
00151                      this->hri_client_goal_.sequence_file[0] = spanish_speech[2];
00152                    this->hri_client_goal_.sequence_file[1] = "";
00153                    this->hri_client_goal_.sequence_file[2] = head_motion[2];
00154                    this->hri_client_goal_.sequence_file[4] = right_arm_motion[2];
00155                    this->hri_client_goal_.sequence_file[3]  = left_arm_motion[2];
00156                    hri_clientMakeActionRequest();
00157                    break;
00158   }
00159 }
00160 
00161 TibiDaboHaodAlgNode::~TibiDaboHaodAlgNode(void)
00162 {
00163   // [free dynamic memory]
00164 }
00165 
00166 void TibiDaboHaodAlgNode::mainNodeThread(void)
00167 {
00168   double pan_angle,tilt_angle;
00169   static haod_states state=haod_idle;
00170   static int init_fail_count=0,user_input_timeout=0;
00171 
00172   this->alg_.lock(); 
00173   switch(state)
00174   {
00175     case haod_idle: if(this->new_joy_msg)
00176                     {
00177                       if(this->joy_action==help)
00178                       {
00179                         state=haod_hri_intro;
00180                         // initialize the HRI gola message
00181                         this->execute_hri_action(hri_intro);
00182                       }
00183                       else
00184                         state=haod_idle;
00185                       this->new_joy_msg=false;
00186                       this->joy_action=none;
00187                     }
00188                     else if(this->new_haod_action)
00189                     {
00190                       state=haod_init;
00191                       this->new_haod_action=false;
00192                       init_fail_count=0;
00193                     }
00194                     else
00195                       state=haod_idle;
00196                     ROS_INFO("HAOD State: idle");
00197                     break;
00198     case haod_hri_intro: if(this->hri_action_done)
00199                            state=haod_idle;
00200                          else if(this->new_joy_msg && this->joy_action==force_end)
00201                          {
00202                            this->new_joy_msg=false;
00203                            this->joy_action=none;
00204                            hri_client_client_.cancelGoal();
00205                            haod_server_aserver_.setAborted();
00206                            if(this->tracking_head)
00207                              follow_target_client_.cancelGoal();    
00208                            state=haod_idle;
00209                          }
00210                          else
00211                            state=haod_hri_intro;
00212                          ROS_INFO("HAOD State: hri_intro");
00213                          break;
00214     case haod_init: this->haod_init_srv_.request.x=this->init_x;
00215                     this->haod_init_srv_.request.y=this->init_y;
00216                     if (haod_init_client_.call(haod_init_srv_)) 
00217                     { 
00218                       if(this->haod_init_srv_.response.success)
00219                         state=haod_detect;
00220                       else
00221                       {
00222                         init_fail_count++;
00223                         if(init_fail_count>10)
00224                         {
00225                           state=haod_hri_init;
00226                           // initialize the HRI gola message
00227                           this->execute_hri_action(hri_init);
00228                         }
00229                         else
00230                           state=haod_init;
00231                       }
00232                     } 
00233                     else 
00234                     { 
00235                       ROS_INFO("TibiDaboHaodAlgNode:: Failed to Call Server on topic haod_init "); 
00236                       state=haod_init;
00237                     }
00238                     ROS_INFO("HAOD State: haod_init");
00239 
00240                     break;
00241     case haod_hri_init: if(this->hri_action_done)
00242                         {
00243                           state=haod_idle;
00244                           haod_server_aserver_.setAborted();
00245                           init_fail_count=0;
00246                         }
00247                         else if(this->new_joy_msg && this->joy_action==force_end)
00248                         {
00249                           this->new_joy_msg=false;
00250                           this->joy_action=none;
00251                           hri_client_client_.cancelGoal();
00252                           haod_server_aserver_.setAborted();
00253                           if(this->tracking_head)
00254                             follow_target_client_.cancelGoal();
00255                           state=haod_idle;
00256                         }
00257                         else
00258                           state=haod_hri_init;
00259                         ROS_INFO("HAOD State: hri_init");
00260                         break;
00261     case haod_detect: if(this->new_joy_msg)
00262                       {
00263                         if(this->joy_action==end)
00264                         {
00265                           state=haod_hri_exit;
00266                           // initialize the HRI gola message
00267                           this->execute_hri_action(hri_exit);
00268                         }
00269                         else if(this->joy_action==force_end)
00270                         {
00271                           haod_server_aserver_.setAborted();
00272                           if(this->tracking_head)
00273                             follow_target_client_.cancelGoal();
00274                           state=haod_idle;
00275                         }
00276                         else
00277                           state=haod_detect;
00278                         this->new_joy_msg=false;
00279                         this->joy_action=none;
00280                       }
00281                       else if(this->new_haod_action)
00282                       {
00283                         state=haod_init;
00284                         this->new_haod_action=false;
00285                         init_fail_count=0;
00286                       }
00287                       else if(this->stop_haod)
00288                       {
00289                         state=haod_idle;
00290                       }
00291                       else
00292                       {
00293                         if(haod_detect_client_.call(haod_detect_srv_)) 
00294                         { 
00295                           if(!this->haod_detect_srv_.response.success)
00296                           {
00297                             state=haod_detect;
00298                             // update the target angles
00299                             if(this->haod_detect_srv_.response.x>=0 && this->haod_detect_srv_.response.y>=0)
00300                             {
00301                               pan_angle = -atan2(this->haod_detect_srv_.response.x-256,833);
00302                               tilt_angle = -atan2(this->haod_detect_srv_.response.y-192,835);
00303                               this->JointTrajectoryPoint_msg_.positions[0]=pan_angle;
00304                               this->JointTrajectoryPoint_msg_.positions[1]=tilt_angle;
00305                               this->target_joints_publisher_.publish(this->JointTrajectoryPoint_msg_);
00306                             
00307                               // start the head tracking (the first time only) 
00308                               if(!this->tracking_head)
00309                                 follow_targetMakeActionRequest();
00310                             }
00311                           }
00312                           else
00313                           {
00314                             state=haod_hri_help;
00315                             this->haod_need_help=true;
00316                             this->execute_hri_action(hri_help);
00317                           }
00318                         }
00319                         else 
00320                         { 
00321                           ROS_INFO("TibiDaboHaodAlgNode:: Failed to Call Server on topic haod_detect "); 
00322                            
00323                         }
00324                       }
00325                       ROS_INFO("HAOD State: haod_detect");
00326                       break;
00327     case haod_hri_help: if(this->hri_action_done)
00328                         {
00329                           state=haod_user_input;
00330                           user_input_timeout=0;
00331                         }
00332                         else if(this->new_joy_msg && this->joy_action==force_end)
00333                         {
00334                           this->new_joy_msg=false;
00335                           this->joy_action=none;
00336                           hri_client_client_.cancelGoal();
00337                           haod_server_aserver_.setAborted();
00338                           if(this->tracking_head)
00339                             follow_target_client_.cancelGoal();
00340                           state=haod_idle;
00341                         }
00342                         else
00343                           state=haod_hri_help;
00344                         ROS_INFO("HAOD State: hri_help");
00345                         break;
00346     case haod_user_input: if(this->new_joy_msg)
00347                           {
00348                             user_input_timeout=0;
00349                             if(this->joy_action==valid_det || joy_action==invalid_det)
00350                             {
00351                               state=haod_learn;
00352                               this->haod_need_help=false;
00353                             }
00354                             else if(this->joy_action==end)
00355                             {
00356                               state=haod_hri_exit;
00357                               // initialize the HRI gola message
00358                               this->execute_hri_action(hri_exit);
00359                               this->joy_action=none;
00360                               this->haod_need_help=false;
00361                             }
00362                             else if(this->joy_action==force_end)
00363                             {
00364                               this->joy_action=none;
00365                               hri_client_client_.cancelGoal();
00366                               haod_server_aserver_.setAborted();
00367                               if(this->tracking_head)
00368                                 follow_target_client_.cancelGoal();
00369                               state=haod_idle;
00370                             }
00371                             this->new_joy_msg=false;
00372                           }
00373                           else
00374                           {
00375                             user_input_timeout++;
00376                             if(user_input_timeout>100)
00377                             {
00378                               state=haod_detect;
00379                               this->haod_need_help=false;
00380                             }
00381                             else
00382                               state=haod_user_input;
00383                           }
00384                           ROS_INFO("HAOD State: haod_user_input");
00385                           break;
00386     case haod_learn: if(this->joy_action==valid_det)
00387                        this->haod_learn_srv_.request.valid=true;
00388                      else
00389                        this->haod_learn_srv_.request.valid=false;
00390                      if (haod_learn_client_.call(haod_learn_srv_)) 
00391                      { 
00392                        state=haod_detect;
00393                      } 
00394                      else 
00395                      { 
00396                        ROS_INFO("TibiDaboHaodAlgNode:: Failed to Call Server on topic haod_learn "); 
00397                        state=haod_learn;
00398                      }
00399                      ROS_INFO("HAOD State: haod_learn");
00400                      break;
00401     case haod_hri_exit: if(this->hri_action_done)
00402                         {
00403                           state=haod_idle;
00404                           haod_server_aserver_.setAborted();
00405                         }
00406                         else if(this->new_joy_msg && this->joy_action==force_end)
00407                         {
00408                           this->new_joy_msg=false;
00409                           this->joy_action=none;
00410                           hri_client_client_.cancelGoal();
00411                           haod_server_aserver_.setAborted();
00412                           if(this->tracking_head)
00413                             follow_target_client_.cancelGoal();
00414                           state=haod_idle;
00415                         }
00416                         else
00417                           state=haod_hri_exit;
00418                         ROS_INFO("HAOD State: hri_exit");
00419                         break;
00420     default:
00421              break;
00422   }
00423   this->alg_.unlock(); 
00424   // [fill msg structures]
00425   
00426   // [fill action structure and make request to the action server]
00427 
00428   // [publish messages]
00429 }
00430 
00431 /*  [subscriber callbacks] */
00432 void TibiDaboHaodAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg) 
00433 { 
00434   //ºROS_INFO("TibiDaboHaodAlgNode::joy_callback: New Message Received"); 
00435 
00436   //use appropiate mutex to shared variables if necessary 
00437   this->alg_.lock(); 
00438   this->new_joy_msg=true;
00439   if(msg->buttons[wiimote::State::MSG_BTN_1]==1)// valid detection
00440     this->joy_action=valid_det;
00441   else if(msg->buttons[wiimote::State::MSG_BTN_2]==1)// invalid detection
00442     this->joy_action=invalid_det;
00443   else if(msg->buttons[wiimote::State::MSG_BTN_B]==1)// invalid detection
00444     this->joy_action=end;
00445   else if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1)// invalid detection
00446     this->joy_action=help;
00447   else if(msg->buttons[wiimote::State::MSG_BTN_PLUS]!=0)// change language
00448   {
00449     if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==1) this->language=catalan;
00450     else if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==2) this->language=english;
00451     else if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==3) this->language=spanish; 
00452   }  
00453   else if(msg->buttons[wiimote::State::MSG_BTN_MINUS]==1)// change tab
00454     this->joy_action=force_end;
00455   else
00456     this->joy_action=none;
00457   //this->joy_mutex_.enter(); 
00458 
00459   //unlock previously blocked shared variables 
00460   this->alg_.unlock(); 
00461   //this->joy_mutex_.exit(); 
00462 }
00463 
00464 /*  [service callbacks] */
00465 
00466 /*  [action callbacks] */
00467 void TibiDaboHaodAlgNode::follow_targetDone(const actionlib::SimpleClientGoalState& state,  const iri_nav_msgs::followTargetResultConstPtr& result) 
00468 { 
00469   if( state.toString().compare("SUCCEEDED") == 0 ) 
00470     ROS_INFO("TibiDaboHaodAlgNode::follow_targetDone: Goal Achieved!"); 
00471   else 
00472     ROS_INFO("TibiDaboHaodAlgNode::follow_targetDone: %s", state.toString().c_str()); 
00473   this->tracking_head=false;
00474 
00475   //copy & work with requested result 
00476 } 
00477 
00478 void TibiDaboHaodAlgNode::follow_targetActive() 
00479 { 
00480   //ROS_INFO("TibiDaboHaodAlgNode::follow_targetActive: Goal just went active!"); 
00481 } 
00482 
00483 void TibiDaboHaodAlgNode::follow_targetFeedback(const iri_nav_msgs::followTargetFeedbackConstPtr& feedback) 
00484 { 
00485   //ROS_INFO("TibiDaboHaodAlgNode::follow_targetFeedback: Got Feedback!"); 
00486 
00487   bool feedback_is_ok = true; 
00488 
00489   //analyze feedback 
00490   //my_var = feedback->var; 
00491 
00492   //if feedback is not what expected, cancel requested goal 
00493   if( !feedback_is_ok ) 
00494   { 
00495     follow_target_client_.cancelGoal(); 
00496     //ROS_INFO("TibiDaboHaodAlgNode::follow_targetFeedback: Cancelling Action!"); 
00497   } 
00498 }
00499 void TibiDaboHaodAlgNode::haod_serverStartCallback(const tibi_dabo_msgs::haodGoalConstPtr& goal)
00500 { 
00501   alg_.lock(); 
00502     //check goal 
00503   this->new_haod_action=true;
00504   this->init_x=goal->x;
00505   this->init_y=goal->y;
00506     //execute goal 
00507   alg_.unlock(); 
00508 } 
00509 
00510 void TibiDaboHaodAlgNode::haod_serverStopCallback(void) 
00511 { 
00512   alg_.lock(); 
00513   this->stop_haod=true;
00514   this->haod_need_help=false;
00515     //stop action 
00516   alg_.unlock(); 
00517 } 
00518 
00519 bool TibiDaboHaodAlgNode::haod_serverIsFinishedCallback(void) 
00520 { 
00521   bool ret = false; 
00522 
00523   alg_.lock(); 
00524     //if action has finish for any reason 
00525     //ret = true; 
00526   alg_.unlock(); 
00527 
00528   return ret; 
00529 } 
00530 
00531 bool TibiDaboHaodAlgNode::haod_serverHasSucceedCallback(void) 
00532 { 
00533   bool ret = false; 
00534 
00535   alg_.lock(); 
00536     //if goal was accomplished 
00537     //ret = true; 
00538   alg_.unlock(); 
00539 
00540   return ret; 
00541 } 
00542 
00543 void TibiDaboHaodAlgNode::haod_serverGetResultCallback(tibi_dabo_msgs::haodResultPtr& result) 
00544 { 
00545   alg_.lock(); 
00546     //update result data to be sent to client 
00547     //result->data = data; 
00548   alg_.unlock(); 
00549 } 
00550 
00551 void TibiDaboHaodAlgNode::haod_serverGetFeedbackCallback(tibi_dabo_msgs::haodFeedbackPtr& feedback) 
00552 { 
00553   alg_.lock(); 
00554     //keep track of feedback 
00555     //ROS_INFO("feedback: %si", feedback->data.c_str()); 
00556   if(this->haod_need_help)
00557     feedback->assistance=true;
00558   else
00559     feedback->assistance=false;
00560   alg_.unlock(); 
00561 }
00562 void TibiDaboHaodAlgNode::hri_clientDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
00563 { 
00564   if( state.toString().compare("SUCCEEDED") == 0 ) 
00565     ROS_INFO("TibiDaboHaodAlgNode::hri_clientDone: Goal Achieved!"); 
00566   else 
00567     ROS_INFO("TibiDaboHaodAlgNode::hri_clientDone: %s", state.toString().c_str()); 
00568   this->hri_action_done=true;
00569 
00570   //copy & work with requested result 
00571 } 
00572 
00573 void TibiDaboHaodAlgNode::hri_clientActive() 
00574 { 
00575   //ROS_INFO("TibiDaboHaodAlgNode::hri_clientActive: Goal just went active!"); 
00576 } 
00577 
00578 void TibiDaboHaodAlgNode::hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
00579 { 
00580   //ROS_INFO("TibiDaboHaodAlgNode::hri_clientFeedback: Got Feedback!"); 
00581 
00582   bool feedback_is_ok = true; 
00583 
00584   //analyze feedback 
00585   //my_var = feedback->var; 
00586 
00587   //if feedback is not what expected, cancel requested goal 
00588   if( !feedback_is_ok ) 
00589   { 
00590     hri_client_client_.cancelGoal(); 
00591     //ROS_INFO("TibiDaboHaodAlgNode::hri_clientFeedback: Cancelling Action!"); 
00592   } 
00593 }
00594 
00595 /*  [action requests] */
00596 void TibiDaboHaodAlgNode::follow_targetMakeActionRequest() 
00597 { 
00598   ROS_INFO("TibiDaboHaodAlgNode::follow_targetMakeActionRequest: Starting New Request!"); 
00599 
00600   //wait for the action server to start 
00601   //will wait for infinite time 
00602   follow_target_client_.waitForServer();  
00603   ROS_INFO("TibiDaboHaodAlgNode::follow_targetMakeActionRequest: Server is Available!"); 
00604 
00605   //send a goal to the action 
00606   //follow_target_goal_.data = my_desired_goal; 
00607   follow_target_client_.sendGoal(follow_target_goal_, 
00608               boost::bind(&TibiDaboHaodAlgNode::follow_targetDone,     this, _1, _2), 
00609               boost::bind(&TibiDaboHaodAlgNode::follow_targetActive,   this), 
00610               boost::bind(&TibiDaboHaodAlgNode::follow_targetFeedback, this, _1)); 
00611   ROS_INFO("TibiDaboHaodAlgNode::follow_targetMakeActionRequest: Goal Sent. Wait for Result!"); 
00612   this->tracking_head=true;
00613 }
00614 void TibiDaboHaodAlgNode::hri_clientMakeActionRequest() 
00615 { 
00616   ROS_INFO("TibiDaboHaodAlgNode::hri_clientMakeActionRequest: Starting New Request!"); 
00617 
00618   //wait for the action server to start 
00619   //will wait for infinite time 
00620   hri_client_client_.waitForServer();  
00621   ROS_INFO("TibiDaboHaodAlgNode::hri_clientMakeActionRequest: Server is Available!"); 
00622 
00623   //send a goal to the action 
00624   //hri_client_goal_.data = my_desired_goal; 
00625   hri_client_client_.sendGoal(hri_client_goal_, 
00626               boost::bind(&TibiDaboHaodAlgNode::hri_clientDone,     this, _1, _2), 
00627               boost::bind(&TibiDaboHaodAlgNode::hri_clientActive,   this), 
00628               boost::bind(&TibiDaboHaodAlgNode::hri_clientFeedback, this, _1)); 
00629   ROS_INFO("TibiDaboHaodAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
00630   this->hri_action_done=false;
00631 }
00632 
00633 void TibiDaboHaodAlgNode::node_config_update(Config &config, uint32_t level)
00634 {
00635   this->alg_.lock();
00636 
00637   this->alg_.unlock();
00638 }
00639 
00640 void TibiDaboHaodAlgNode::addNodeDiagnostics(void)
00641 {
00642 }
00643 
00644 /* main function */
00645 int main(int argc,char *argv[])
00646 {
00647   return algorithm_base::main<TibiDaboHaodAlgNode>(argc, argv, "tibi_dabo_haod_alg_node");
00648 }


tibi_dabo_haod
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 23:42:07