tibi_coop_alg_node.cpp

Go to the documentation of this file.

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

TibiCoopAlgNode::TibiCoopAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiCoopAlgorithm>(),
  move_guide_client_("move_guide", true),
  car_tibi_action_aserver_(public_node_handle_, "car_tibi_action"),
  dabo_tibi_action_aserver_(public_node_handle_, "dabo_tibi_action"),
  gui_action_aserver_(public_node_handle_, "gui_action"),
  tibi_car_action_client_("tibi_car_action", true),
  move_base_client_("move_base", true),
  hri_client_client_("hri_client", true)
{
  //init class attributes if necessary
  this->loop_rate_ = 1;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  this->supervisor_subscriber_ = this->public_node_handle_.subscribe("supervisor", 10, &TibiCoopAlgNode::supervisor_callback, this);
  this->new_joy_cmd=false;
  this->joy_cmd=joy_none;
  
  // [init services]
  
  // [init clients]
  get_goal_client_ = this->public_node_handle_.serviceClient<iri_goal_database::get_goal>("get_goal");
  
  // [init action servers]
  car_tibi_action_aserver_.registerStartCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionStartCallback, this, _1)); 
  car_tibi_action_aserver_.registerStopCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionStopCallback, this)); 
  car_tibi_action_aserver_.registerIsFinishedCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionIsFinishedCallback, this)); 
  car_tibi_action_aserver_.registerHasSucceedCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionHasSucceedCallback, this)); 
  car_tibi_action_aserver_.registerGetResultCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionGetResultCallback, this, _1)); 
  car_tibi_action_aserver_.registerGetFeedbackCallback(boost::bind(&TibiCoopAlgNode::car_tibi_actionGetFeedbackCallback, this, _1)); 
  car_tibi_action_aserver_.start();
  this->new_car_cmd=false;
  this->car_cmd=tibi_none;

  dabo_tibi_action_aserver_.registerStartCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionStartCallback, this, _1)); 
  dabo_tibi_action_aserver_.registerStopCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionStopCallback, this)); 
  dabo_tibi_action_aserver_.registerIsFinishedCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionIsFinishedCallback, this)); 
  dabo_tibi_action_aserver_.registerHasSucceedCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionHasSucceedCallback, this)); 
  dabo_tibi_action_aserver_.registerGetResultCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionGetResultCallback, this, _1)); 
  dabo_tibi_action_aserver_.registerGetFeedbackCallback(boost::bind(&TibiCoopAlgNode::dabo_tibi_actionGetFeedbackCallback, this, _1)); 
  dabo_tibi_action_aserver_.start();
  this->new_dabo_cmd=false;
  this->dabo_cmd=tibi_none;

  gui_action_aserver_.registerStartCallback(boost::bind(&TibiCoopAlgNode::gui_actionStartCallback, this, _1)); 
  gui_action_aserver_.registerStopCallback(boost::bind(&TibiCoopAlgNode::gui_actionStopCallback, this)); 
  gui_action_aserver_.registerIsFinishedCallback(boost::bind(&TibiCoopAlgNode::gui_actionIsFinishedCallback, this)); 
  gui_action_aserver_.registerHasSucceedCallback(boost::bind(&TibiCoopAlgNode::gui_actionHasSucceedCallback, this)); 
  gui_action_aserver_.registerGetResultCallback(boost::bind(&TibiCoopAlgNode::gui_actionGetResultCallback, this, _1)); 
  gui_action_aserver_.registerGetFeedbackCallback(boost::bind(&TibiCoopAlgNode::gui_actionGetFeedbackCallback, this, _1)); 
  gui_action_aserver_.start();
  this->new_gui_cmd=false;
  this->gui_cmd=gui_none;

  this->tibi_car_action_done=true;
  
  // [init action clients]
  this->hri_client_done=true;
  this->move_base_done=true;
  this->move_guide_done=true;

  this->hri_client_goal_.sequence_file.resize(5); 

  this->current_state=tibi_idle;

  this->hri_client_goal_.sequence_file.resize(5);
  this->loadSentences();
}

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

void TibiCoopAlgNode::mainNodeThread(void)
{
  static tibi_states state=tibi_idle;
  static int timeout=0;

  alg_.lock(); 
  switch(state)
  {
    case tibi_idle: if(this->new_gui_cmd)
                    {
                      // start HRI action
                      if(this->gui_cmd==gui_set_goal)
                      {
                        this->hri_client_goal_.sequence_file[0] = "Destination chosen. Follow me, please.";
                        this->hri_client_goal_.sequence_file[1] = "";
                        this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                        this->hri_client_goal_.sequence_file[4] = "";
                        this->hri_client_goal_.sequence_file[3] = "";
                        if(hri_clientMakeActionRequest())
                        {
                          this->new_gui_cmd=false;
                          state=tibi_wait_intro;
                        }
                        else
                          state=tibi_idle;
                      }
                      else
                        state=tibi_idle;
                    }
                    else if(timeout>25)
                    {
                      // start HRI action
                      this->hri_client_goal_.sequence_file[0] = "Where do you want to go?";
                      this->hri_client_goal_.sequence_file[1] = "";
                      this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                      this->hri_client_goal_.sequence_file[4] = "";
                      this->hri_client_goal_.sequence_file[3] = "";
                      if(hri_clientMakeActionRequest())
                        state=tibi_wait_bored;
                      else
                        state=tibi_idle;
                    }
                    else
                    {
                      state=tibi_idle;
                      timeout+=this->loop_rate_.expectedCycleTime().toSec();
                    }
                    ROS_INFO("TibiCoopAlgNode: state tibi_idle!");
                    break;
    case tibi_wait_bored: if(this->hri_client_done)
                          {
                            timeout=0;
                            state=tibi_idle;
                          }
                          else
                            state=tibi_wait_bored;
                          ROS_INFO("TibiCoopAlgNode: state tibi_wait_bored!");
                          break;
    case tibi_wait_intro: if(this->hri_client_done)
                          {
                            move_guide_goal_.location_id="taxi_stop";
                            move_guide_goal_.robot_id="tibi";
                            move_guide_goal_.target_id=0;
                            // start moving to the goal
                            move_guideMakeActionRequest();
                            state=tibi_set_car_goal;
                          }
                          else
                            state=tibi_wait_intro;
                          ROS_INFO("TibiCoopAlgNode: state tibi_wait_intro!");
                          break;
    case tibi_set_car_goal: // make the car move to the goal
                            tibi_car_action_goal_.cmd_id=car_set_goal;
                            tibi_car_action_goal_.num_params.clear();
                            tibi_car_action_goal_.string_params.resize(1);
                            tibi_car_action_goal_.string_params[0]="taxi_stop";
                            if(tibi_car_actionMakeActionRequest())
                              state=tibi_wait_guiding;
                            else
                              state=tibi_set_car_goal;
                            ROS_INFO("TibiCoopAlgNode: state tibi_set_car_goal!");
                            break;
    case tibi_wait_guiding: if(this->move_guide_done)
                            {
                              if(this->hri_client_done)
                              {
                                if(this->tibi_car_action_done)
                                {
                                  // start HRI
                                  this->hri_client_goal_.sequence_file[0] = "Get in the car and press the screen button when you are ready.";
                                  this->hri_client_goal_.sequence_file[1] = "";
                                  this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                                  this->hri_client_goal_.sequence_file[4] = "";
                                  this->hri_client_goal_.sequence_file[3] = "";
                                  hri_clientMakeActionRequest();
                                  state=tibi_wait_done;
                                }
                                else 
                                {
                                  // start HRI
                                  this->hri_client_goal_.sequence_file[0] = "Wait for the car";
                                  this->hri_client_goal_.sequence_file[1] = "";
                                  this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                                  this->hri_client_goal_.sequence_file[4] = "";
                                  this->hri_client_goal_.sequence_file[3] = "";
                                  hri_clientMakeActionRequest();
                                  state=tibi_wait_no_car;
                                }
                              }
                              else
                                state=tibi_wait_guiding;
                            }
                            else if(this->new_joy_cmd)
                            {
                              if(this->joy_cmd==joy_cancel_goal)
                              {
                                this->move_base_client_.cancelGoal();
                                if(this->tibi_car_action_done)
                                {
                                  // start HRI
                                  this->hri_client_goal_.sequence_file[0] = "Get in the car and press the screen button when you are ready.";
                                  this->hri_client_goal_.sequence_file[1] = "";
                                  this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                                  this->hri_client_goal_.sequence_file[4] = "";
                                  this->hri_client_goal_.sequence_file[3] = "";
                                  hri_clientMakeActionRequest();
                                  state=tibi_wait_done;
                                }
                                else 
                                {
                                  // start HRI
                                  this->hri_client_goal_.sequence_file[0] = "Wait for the car";
                                  this->hri_client_goal_.sequence_file[1] = "";
                                  this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                                  this->hri_client_goal_.sequence_file[4] = "";
                                  this->hri_client_goal_.sequence_file[3] = "";
                                  hri_clientMakeActionRequest();
                                  state=tibi_wait_no_car;
                                }
                                this->new_joy_cmd=false;
                              }
                              else if(this->joy_cmd==joy_replan_goal)
                              {
                                move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                // start moving to the goal
                                move_baseMakeActionRequest();
                                state=tibi_wait_guiding;
                                this->new_joy_cmd=false;
                              }
                              else
                              {
                                state=tibi_wait_guiding;
                                this->new_joy_cmd=false;
                              }
                            }
                            else
                              state=tibi_wait_guiding;
                            ROS_INFO("TibiCoopAlgNode: state tibi_wait_guiding!");
                            break;
    case tibi_wait_no_car: if(this->hri_client_done)
                           {
                             if(this->tibi_car_action_done)
                             {
                               // start HRI
                               this->hri_client_goal_.sequence_file[0] = "Here it is. Get in the car and press the screen button when you are ready.";
                               this->hri_client_goal_.sequence_file[1] = "";
                               this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                               this->hri_client_goal_.sequence_file[4] = "";
                               this->hri_client_goal_.sequence_file[3] = "";
                               if(hri_clientMakeActionRequest())
                                 state=tibi_wait_done;
                               else
                                 state=tibi_wait_no_car;
                             }
                             else
                             {
                               timeout=0;
                               state=tibi_delay;
                             }
                           }
                           else
                             state=tibi_wait_no_car;
                           ROS_INFO("TibiCoopAlgNode: state tibi_wait_no_car!");
                           break;
    case tibi_delay: if(this->hri_client_done)
                     {
                       if(this->tibi_car_action_done)
                       {
                         // start HRI
                         this->hri_client_goal_.sequence_file[0] = "Here it is. Get in the car and press the screen button when you are ready.";
                         this->hri_client_goal_.sequence_file[1] = "";
                         this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                         this->hri_client_goal_.sequence_file[4] = "";
                         this->hri_client_goal_.sequence_file[3] = "";
                         if(hri_clientMakeActionRequest())
                           state=tibi_wait_done;
                         else
                           state=tibi_delay;
                       }
                       else if(timeout>5)
                       {
                         this->hri_client_goal_.sequence_file[0] = "The car is about to arrive";
                         this->hri_client_goal_.sequence_file[1] = "";
                         this->hri_client_goal_.sequence_file[2] = "head_home.xml";
                         this->hri_client_goal_.sequence_file[4] = "";
                         this->hri_client_goal_.sequence_file[3] = "";
                         if(hri_clientMakeActionRequest())
                           state=tibi_delay;
                         else
                           state=tibi_delay;
                         timeout=0;
                       }
                       else
                       {
                         state=tibi_delay;
                         timeout+=this->loop_rate_.expectedCycleTime().toSec();
                       }
                     }
                     else
                       state=tibi_delay;
                     ROS_INFO("TibiCoopAlgNode: state tibi_delay!");
                     break;
    case tibi_wait_done: if(this->hri_client_done)
                         {
                           if(this->tibi_car_action_done)
                           {
                             // send the user goal to the car
                             tibi_car_action_goal_.cmd_id=car_set_goal;
                             tibi_car_action_goal_.num_params.clear();
                             tibi_car_action_goal_.string_params.resize(1);
                             tibi_car_action_goal_.string_params[0]=this->gui_location_id;
                             if(tibi_car_actionMakeActionRequest())
                               state=tibi_wait_car;
                             else
                               state=tibi_wait_done;
                           }
                           else
                             state=tibi_wait_done;
                         }
                         else
                           state=tibi_wait_done;
                         ROS_INFO("TibiCoopAlgNode: state tibi_wait_done!");
                         break;
    case tibi_wait_car: if(this->tibi_car_action_done)
                          state=tibi_wait_dabo;
                        else
                          state=tibi_wait_car;
                        ROS_INFO("TibiCoopAlgNode: state tibi_wait_car!");
                        break;
    case tibi_wait_dabo: if(this->new_dabo_cmd)
                         {
                           if(this->dabo_cmd==tibi_set_goal)
                           {
                             // get the goal position from the database (gui_cmd)
                             get_goal_srv_.request.location_id=this->dabo_location_id;
                             get_goal_srv_.request.robot_id="tibi";
                             if (get_goal_client_.call(get_goal_srv_))
                             {
                               move_base_goal_.target_pose.header.stamp=ros::Time::now();
                               move_base_goal_.target_pose.header.frame_id="/map";
                               move_base_goal_.target_pose.pose.position=get_goal_srv_.response.goal_pose.position;
                               move_base_goal_.target_pose.pose.orientation=get_goal_srv_.response.goal_pose.orientation;
                               // start moving to the goal
                               if(move_baseMakeActionRequest())
                               {
                                 this->new_dabo_cmd=false;
                                 state=tibi_wait_home;
                               }
                               else
                                 state=tibi_wait_dabo;
                             }
                             else
                             {
                               ROS_INFO("TibiCoopAlgNode:: Failed to Call Server on topic get_goal ");
                               state=tibi_wait_dabo;
                             }
                           }
                           else
                             state=tibi_wait_dabo;
                         }
                         else
                           state=tibi_wait_dabo;
                         ROS_INFO("TibiCoopAlgNode: state tibi_wait_dabo!");
                         break;
    case tibi_wait_home: if(this->move_base_done)
                           state=tibi_wait_car_home;
                         else if(this->new_joy_cmd)
                         {
                           if(this->joy_cmd==joy_cancel_goal)
                           {
                             this->move_base_client_.cancelGoal();
                             state=tibi_wait_car_home;
                             this->new_joy_cmd=false;
                           }
                           else if(this->joy_cmd==joy_replan_goal)
                           {
                             move_base_goal_.target_pose.header.stamp=ros::Time::now();
                             // start moving to the goal
                             move_baseMakeActionRequest();
                             state=tibi_wait_home;
                             this->new_joy_cmd=false;
                           }
                           else
                           {
                             state=tibi_wait_home;
                             this->new_joy_cmd=false;
                           }
                         }
                         else
                           state=tibi_wait_home;
                         ROS_INFO("TibiCoopAlgNode: state tibi_wait_home!");
                         break;
    case tibi_wait_car_home: if(this->new_car_cmd)
                             {
                               this->new_car_cmd=false;
                               if(this->car_cmd==tibi_car_done)
                                 state=tibi_idle;
                               else
                                 state=tibi_wait_car_home;
                             }
                             else
                               state=tibi_wait_car_home;
                             ROS_INFO("TibiCoopAlgNode: state tibi_wait_car_home!");
                             break;
  }
  this->current_state=state;
  alg_.unlock(); 

  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */
void TibiCoopAlgNode::supervisor_callback(const sensor_msgs::Joy::ConstPtr& msg) 
{ 
//  ROS_INFO("TibiCoopAlgNode::supervisor_callback: New Message Received"); 
  static std::vector<int> prev_buttons(msg->buttons);

  this->alg_.lock(); 

  if(msg->buttons[wiimote::State::MSG_BTN_PLUS]==1 && prev_buttons[wiimote::State::MSG_BTN_PLUS]==0)//replan
  {
    ROS_INFO("Replan"); 
    this->joy_cmd=joy_replan_goal;
    this->new_joy_cmd=true;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_MINUS]==1 && prev_buttons[wiimote::State::MSG_BTN_MINUS]==0)// cancel goal
  {
  ROS_INFO("Cancel"); 
    this->joy_cmd=joy_cancel_goal;
    this->new_joy_cmd=true;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1 && prev_buttons[wiimote::State::MSG_BTN_HOME]==0)// teleop
  {
  ROS_INFO("Teleop"); 
    this->joy_cmd=joy_teleop;
    this->new_joy_cmd=true;
  }

  prev_buttons=msg->buttons;

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

/*  [service callbacks] */

/*  [action callbacks] */
void TibiCoopAlgNode::move_guideDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guideGoalResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiCoopAlgNode::move_guideDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiCoopAlgNode::move_guideDone: %s", state.toString().c_str()); 
  this->move_guide_done=true;

  //copy & work with requested result 
} 

void TibiCoopAlgNode::move_guideActive() 
{ 
  //ROS_INFO("TibiCoopAlgNode::move_guideActive: Goal just went active!"); 
} 

void TibiCoopAlgNode::move_guideFeedback(const tibi_dabo_msgs::guideGoalFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiCoopAlgNode::move_guideFeedback: 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 ) 
  { 
    move_guide_client_.cancelGoal(); 
    //ROS_INFO("TibiCoopAlgNode::move_guideFeedback: Cancelling Action!"); 
  } 
}
void TibiCoopAlgNode::car_tibi_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{
  ROS_INFO("TibiCoopAlgNode::car_tibi_actionStartCallback!"); 
  alg_.lock(); 
    //check goal 
  this->car_cmd=(tibi_cmds)goal->cmd_id;
  switch(this->car_cmd)
  {
    case tibi_set_goal: this->new_car_cmd=false;
                        break;
    case tibi_car_done: this->new_car_cmd=true;
                        break;
    default: this->new_car_cmd=false;
             break;
  }
    //execute goal 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::car_tibi_actionStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

bool TibiCoopAlgNode::car_tibi_actionIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
  if(this->current_state==tibi_idle)
    ret=true;
  else
    ret=false;
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool TibiCoopAlgNode::car_tibi_actionHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if goal was accomplished 
    ret = true; 
  alg_.unlock(); 

  return ret; 
} 

void TibiCoopAlgNode::car_tibi_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::car_tibi_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}
void TibiCoopAlgNode::dabo_tibi_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{ 
  ROS_INFO("TibiCoopAlgNode::dabo_tibi_actionStartCallback");
  alg_.lock(); 
    //check goal 
  this->dabo_cmd=(tibi_cmds)goal->cmd_id;
  switch(this->dabo_cmd)
  {
    case tibi_set_goal: if(goal->string_params.size()==1)
                        {
                          this->dabo_location_id=goal->string_params[0];
                          this->new_dabo_cmd=true;
                        }
                        else
                          this->new_dabo_cmd=false;
                        break;
    case tibi_car_done: this->new_dabo_cmd=false;
                        break;
    default: this->new_dabo_cmd=false;
             break;
  }

    //execute goal 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::dabo_tibi_actionStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  // cancel the move base goal
  move_base_client_.cancelGoal();
  alg_.unlock(); 
} 

bool TibiCoopAlgNode::dabo_tibi_actionIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
  if(this->current_state==tibi_idle)
    ret=true;
  else
    ret=false;
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool TibiCoopAlgNode::dabo_tibi_actionHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if goal was accomplished 
    ret = true; 
  alg_.unlock(); 

  return ret; 
} 

void TibiCoopAlgNode::dabo_tibi_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::dabo_tibi_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}
void TibiCoopAlgNode::tibi_car_actionDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result) 
{
  this->alg_.lock(); 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiCoopAlgNode::tibi_car_actionDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiCoopAlgNode::tibi_car_actionDone: %s", state.toString().c_str()); 
  this->tibi_car_action_done=true;

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

void TibiCoopAlgNode::tibi_car_actionActive() 
{ 
  //ROS_INFO("TibiCoopAlgNode::tibi_car_actionActive: Goal just went active!"); 
} 

void TibiCoopAlgNode::tibi_car_actionFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiCoopAlgNode::tibi_car_actionFeedback: 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 ) 
  { 
    tibi_car_action_client_.cancelGoal(); 
    //ROS_INFO("TibiCoopAlgNode::tibi_car_actionFeedback: Cancelling Action!"); 
  } 
}
void TibiCoopAlgNode::gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{
  ROS_INFO("TibiCoopAlgNode::gui_actionStartCallback"); 
  alg_.lock(); 
    //check goal 
  this->gui_cmd=(gui_cmds)goal->cmd_id;
  switch(this->gui_cmd)
  {
    case gui_set_goal: if(goal->string_params.size()==1)
                       {
                         this->gui_location_id=goal->string_params[0];
                         this->new_gui_cmd=true;
                       }
                       else
                         this->new_gui_cmd=false;
                       break;
    case gui_cancel_goal: this->new_gui_cmd=false;
                          break;
    default: this->new_gui_cmd=false; 
             break;
  }
    //execute goal 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::gui_actionStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  // cancel the move base goal
  move_base_client_.cancelGoal();
  alg_.unlock(); 
} 

bool TibiCoopAlgNode::gui_actionIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
  if(this->current_state==tibi_wait_car_home)
    ret=true;
  else
    ret=false;
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool TibiCoopAlgNode::gui_actionHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if goal was accomplished 
  ret=true;
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

void TibiCoopAlgNode::gui_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void TibiCoopAlgNode::gui_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

void TibiCoopAlgNode::move_baseDone(const actionlib::SimpleClientGoalState& state,  const move_base_msgs::MoveBaseResultConstPtr& result) 
{ 
  this->alg_.lock(); 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiCoopAlgNode::move_baseDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiCoopAlgNode::move_baseDone: %s", state.toString().c_str()); 
  this->move_base_done=true;
  //copy & work with requested result 
  this->alg_.unlock(); 
} 

void TibiCoopAlgNode::move_baseActive() 
{ 
  //ROS_INFO("TibiCoopAlgNode::move_baseActive: Goal just went active!"); 
} 

void TibiCoopAlgNode::move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiCoopAlgNode::move_baseFeedback: 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 ) 
  { 
    move_base_client_.cancelGoal(); 
    //ROS_INFO("TibiCoopAlgNode::move_baseFeedback: Cancelling Action!"); 
  } 
}

void TibiCoopAlgNode::hri_clientDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
{
  this->alg_.lock(); 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiCoopAlgNode::hri_clientDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiCoopAlgNode::hri_clientDone: %s", state.toString().c_str()); 

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

void TibiCoopAlgNode::hri_clientActive() 
{ 
  //ROS_INFO("TibiCoopAlgNode::hri_clientActive: Goal just went active!"); 
} 

void TibiCoopAlgNode::hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiCoopAlgNode::hri_clientFeedback: 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_client_.cancelGoal(); 
    //ROS_INFO("TibiCoopAlgNode::hri_clientFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void TibiCoopAlgNode::move_guideMakeActionRequest() 
{ 
  ROS_INFO("TibiCoopAlgNode::move_guideMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  move_guide_client_.waitForServer();  
  ROS_INFO("TibiCoopAlgNode::move_guideMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //move_guide_goal_.data = my_desired_goal; 
  move_guide_client_.sendGoal(move_guide_goal_, 
              boost::bind(&TibiCoopAlgNode::move_guideDone,     this, _1, _2), 
              boost::bind(&TibiCoopAlgNode::move_guideActive,   this), 
              boost::bind(&TibiCoopAlgNode::move_guideFeedback, this, _1)); 
  this->move_guide_done=false;
  ROS_INFO("TibiCoopAlgNode::move_guideMakeActionRequest: Goal Sent. Wait for Result!"); 
}
bool TibiCoopAlgNode::tibi_car_actionMakeActionRequest() 
{ 
  ROS_INFO("TibiCoopAlgNode::tibi_car_actionMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  if(!tibi_car_action_client_.waitForServer(ros::Duration(2.0)))  
    return false;
  ROS_INFO("TibiCoopAlgNode::tibi_car_actionMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //tibi_car_action_goal_.data = my_desired_goal; 
  tibi_car_action_client_.sendGoal(tibi_car_action_goal_, 
              boost::bind(&TibiCoopAlgNode::tibi_car_actionDone,     this, _1, _2), 
              boost::bind(&TibiCoopAlgNode::tibi_car_actionActive,   this), 
              boost::bind(&TibiCoopAlgNode::tibi_car_actionFeedback, this, _1)); 
  this->tibi_car_action_done=false;
  ROS_INFO("TibiCoopAlgNode::tibi_car_actionMakeActionRequest: Goal Sent. Wait for Result!"); 

  return true;
}

bool TibiCoopAlgNode::move_baseMakeActionRequest() 
{ 
  ROS_INFO("TibiCoopAlgNode::move_baseMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  if(!move_base_client_.waitForServer(ros::Duration(2.0)))
    return false;  
  ROS_INFO("TibiCoopAlgNode::move_baseMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //move_base_goal_.data = my_desired_goal; 
  move_base_client_.sendGoal(move_base_goal_, 
              boost::bind(&TibiCoopAlgNode::move_baseDone,     this, _1, _2), 
              boost::bind(&TibiCoopAlgNode::move_baseActive,   this), 
              boost::bind(&TibiCoopAlgNode::move_baseFeedback, this, _1)); 
  this->move_base_done=false;
  ROS_INFO("TibiCoopAlgNode::move_baseMakeActionRequest: Goal Sent. Wait for Result!"); 

  return true;
}

bool TibiCoopAlgNode::hri_clientMakeActionRequest() 
{ 
  ROS_INFO("TibiCoopAlgNode::hri_clientMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  if(!hri_client_client_.waitForServer(ros::Duration(2.0)))
    return false;  
  ROS_INFO("TibiCoopAlgNode::hri_clientMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //hri_client_goal_.data = my_desired_goal; 
  hri_client_client_.sendGoal(hri_client_goal_, 
              boost::bind(&TibiCoopAlgNode::hri_clientDone,     this, _1, _2), 
              boost::bind(&TibiCoopAlgNode::hri_clientActive,   this), 
              boost::bind(&TibiCoopAlgNode::hri_clientFeedback, this, _1)); 
  this->hri_client_done=false;
  ROS_INFO("TibiCoopAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
  ROS_INFO("TibiCoopAlgNode::hri_clientMakeActionRequest: Goal tts: %s",this->hri_client_goal_.sequence_file[0].c_str()); 

  return true;
}

void TibiCoopAlgNode::loadSentences()
{
  std::vector<std::string> sentence;
  sentence.push_back("Destinació seleccionada. Segueix-me si us plau.");
  sentence.push_back("Destination selected. Follow me, please.");
  sentence.push_back("Destinación seleccionada. Sígueme por favor");
  sentences.push_back(sentence);
  sentence.clear();
  sentence.push_back("Where do you want to go?");
  sentence.push_back("A on vols anar?");
  sentence.push_back("¿A dónde quieres ir?");
  sentences.push_back(sentence);
  sentence.clear();
  sentence.push_back("Puja al vehicle i prem el botó de la pantalla quan estiguis llest.");
  sentence.push_back("Get in the vehicle and press the screen button when you are ready");
  sentence.push_back("Sube al vehículo y pulta el botón de la pantalla cuando estés listo.");
  sentences.push_back(sentence);
  sentence.clear();
  sentence.push_back("Espera al vehicle.");
  sentence.push_back("Wait for the vehicle.");
  sentence.push_back("Espera al vehículo.");
  sentences.push_back(sentence);
  sentence.clear();
  sentence.push_back("El vehicle està a punt d'arribar.");
  sentence.push_back("The vehicle is about to arrive.");
  sentence.push_back("El vehículo está a punto de llegar.");
  sentences.push_back(sentence);
  sentence.clear();
}

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

  this->alg_.unlock();
}

void TibiCoopAlgNode::addNodeDiagnostics(void)
{
}

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