dabo_coop_alg_node.cpp

Go to the documentation of this file.

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

DaboCoopAlgNode::DaboCoopAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DaboCoopAlgorithm>(),
  car_dabo_action_aserver_(public_node_handle_, "car_dabo_action"),
  gui_action_aserver_(public_node_handle_, "gui_action"),
  gui_action_client_client_("gui_action_client", true),
  dabo_tibi_action_client_("dabo_tibi_action", true),
  dabo_car_action_client_("dabo_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, &DaboCoopAlgNode::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_dabo_action_aserver_.registerStartCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionStartCallback, this, _1)); 
  car_dabo_action_aserver_.registerStopCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionStopCallback, this)); 
  car_dabo_action_aserver_.registerIsFinishedCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionIsFinishedCallback, this)); 
  car_dabo_action_aserver_.registerHasSucceedCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionHasSucceedCallback, this)); 
  car_dabo_action_aserver_.registerGetResultCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionGetResultCallback, this, _1)); 
  car_dabo_action_aserver_.registerGetFeedbackCallback(boost::bind(&DaboCoopAlgNode::car_dabo_actionGetFeedbackCallback, this, _1)); 
  car_dabo_action_aserver_.start();
  this->new_car_cmd=false;
  this->car_cmd=dabo_none;
  this->car_done=false;

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

  this->move_base_done=true;
  this->hri_client_done=true;
  this->dabo_tibi_action_done=true;
  this->dabo_car_action_done=true;
  this->current_state=dabo_idle;

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

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

void DaboCoopAlgNode::mainNodeThread(void)
{
  static dabo_states state;
  static int timeout=0;

  this->alg_.lock();
  switch(state)
  {
    case dabo_idle: if(this->new_car_cmd)
                    {
                      if(this->car_cmd==dabo_set_goal)
                      {
                        // get the goal pose from the database
                        get_goal_srv_.request.location_id=this->car_location_id;
                        get_goal_srv_.request.robot_id="dabo";
                        if (get_goal_client_.call(get_goal_srv_))
                        {
                          //send goal to be shown on gui
                          gui_action_client_goal_.cmd_id=gui_set_goal;
                          gui_action_client_goal_.num_params.clear();
                          gui_action_client_goal_.string_params.resize(1);
                          gui_action_client_goal_.string_params[0]=this->car_location_id;
                          gui_action_clientMakeActionRequest();

                          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())
                          {
                            state=dabo_wait_car_goal;
                            this->new_car_cmd=false;
                          }
                          else
                            state=dabo_idle;
                        }
                        else
                        {
                          ROS_INFO("TibiCoopAlgNode:: Failed to Call Server on topic get_goal ");
                          state=dabo_idle;
                        }
                      }
                      else
                        state=dabo_idle;
                    }
                    else
                      state=dabo_idle;
                    ROS_INFO("State: DABO_IDLE");
                    break;
    case dabo_wait_car_goal: if(this->move_base_done)
                               state=dabo_wait_car_done;
                             else if(this->new_joy_cmd)
                             {
                               if(this->joy_cmd==joy_cancel_goal)
                               {
                                 this->move_base_client_.cancelGoal();
                                 state=dabo_wait_car_done;
                                 this->new_joy_cmd=false;
                               }
                               else if(this->joy_cmd==joy_replan_goal)
                               {
                                 move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                 move_baseMakeActionRequest();
                                 state=dabo_wait_car_goal;
                                 this->new_joy_cmd=false;
                               }
                               else
                               {
                                 state=dabo_wait_car_goal;
                                 this->new_joy_cmd=false;
                               }
                             }
                             else
                               state=dabo_wait_car_goal;
                             ROS_INFO("State: DABO_WAIT_CAR_GOAL");   
                             break;
    case dabo_wait_car_done: if(this->new_car_cmd)
                             {
                               if(this->car_cmd==dabo_car_done)
                               {
                                 //start HRI action
                                 this->hri_client_goal_.sequence_file[0] = "Hello. Welcome to your destination. You can get out of the vehicle.";
                                 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=dabo_wait_speech;
                                   this->new_car_cmd=false;
                                 }
                                 else
                                   state=dabo_wait_car_done;
                               }
                               else
                                 state=dabo_wait_car_done;
                             }
                             else
                               state=dabo_wait_car_done;
                             ROS_INFO("State: DABO_WAIT_CAR_DONE");
                             break;
    case dabo_wait_speech: if(this->hri_client_done)
                           {
                             timeout=0;
                             state=dabo_wait_user_input;
                           }
                           else
                             state=dabo_wait_speech;
                           ROS_INFO("State: DABO_WAIT_SPEECH");
                           break;
    case dabo_wait_user_input: if(this->new_gui_cmd)
                               {
                                 this->new_gui_cmd=false;
                                 if(this->gui_cmd==gui_done)
                                 {
                                   // start HRI action
                                   this->hri_client_goal_.sequence_file[0] = "Please, follow me to the exit";
                                   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();

                                   // start move base
                                   // get the goal pose from the database
                                   get_goal_srv_.request.location_id="home";
                                   get_goal_srv_.request.robot_id="dabo";
                                   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
                                     move_baseMakeActionRequest();
                                     state=dabo_wait_home;
                                     this->new_gui_cmd=false;
                                   }
                                   else
                                   {
                                     ROS_INFO("TibiCoopAlgNode:: Failed to Call Server on topic get_goal ");
                                     state=dabo_wait_user_input;
                                   }
                                 }
                                 else
                                   state=dabo_wait_user_input;
                               }
                               else if(timeout>5)
                               {
                                 // start HRI action
                                 this->hri_client_goal_.sequence_file[0] = "When you are ready to leave, please press the Exit button.";
                                 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=dabo_wait_reminder;
                                 else
                                   state=dabo_wait_user_input;
                               }
                               else
                               {
                                 state=dabo_wait_user_input;
                                 timeout+=this->loop_rate_.expectedCycleTime().toSec();
                               }
                               ROS_INFO("State: DABO_WAIT_USER_INPUT");
                               break;
    case dabo_wait_reminder: if(this->hri_client_done)
                             {
                               timeout=0;
                               state=dabo_wait_user_input;
                             }
                             else
                               state=dabo_wait_reminder;
                             ROS_INFO("State: DABO_WAIT_REMINDER");
                             break;
    case dabo_wait_home: if(this->move_base_done && this->hri_client_done)
                         {
                           // start HRI action
                           this->hri_client_goal_.sequence_file[0] = "Thank you for your participation. Good bye!";
                           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=dabo_wait_goodbye;
                           else
                             state=dabo_wait_home;
                         }
                         else if(this->new_joy_cmd)
                         {
                           if(this->joy_cmd==joy_cancel_goal)
                           {
                             this->move_base_client_.cancelGoal();
                             state=dabo_wait_car_done;
                             this->new_joy_cmd=false;
                           }
                           else if(this->joy_cmd==joy_replan_goal)
                           {
                             move_base_goal_.target_pose.header.stamp=ros::Time::now();
                             move_baseMakeActionRequest();
                             state=dabo_wait_car_goal;
                             this->new_joy_cmd=false;
                           }
                           else
                           {
                             state=dabo_wait_car_goal;
                             this->new_joy_cmd=false;
                           }
                         }
                         else
                           state=dabo_wait_home;
                         ROS_INFO("State: DABO_WAIT_HOME");
                         break;
    case dabo_wait_goodbye: if(this->hri_client_done)
                            {
                              // send the car home
                              dabo_car_action_goal_.cmd_id=dabo_set_goal;
                              dabo_car_action_goal_.num_params.clear();
                              dabo_car_action_goal_.string_params.resize(1);
                              dabo_car_action_goal_.string_params[0]="home";
                              if(dabo_car_actionMakeActionRequest())
                                state=dabo_set_goal_tibi;
                              else
                                state=dabo_wait_goodbye;
                            }
                            else
                              state=dabo_wait_goodbye;
                            ROS_INFO("State: DABO_WAIT_GOODBye");
                            break;
    case dabo_set_goal_tibi: // send the tibi home
                             dabo_tibi_action_goal_.cmd_id=dabo_set_goal;
                             dabo_tibi_action_goal_.num_params.clear();
                             dabo_tibi_action_goal_.string_params.resize(1);
                             dabo_tibi_action_goal_.string_params[0]="home";
                             if(dabo_tibi_actionMakeActionRequest())
                               state=dabo_wait_car_tibi_home;
                             else
                               state=dabo_set_goal_tibi;
                            ROS_INFO("State: DABO_SET_GOAL_TIBI");
                            break;
    case dabo_wait_car_tibi_home: if(this->dabo_tibi_action_done && this->dabo_car_action_done)
                                    state=dabo_idle;
                                  else
                                    state=dabo_wait_car_tibi_home;
                                  ROS_INFO("State: DABO_WAIT_CAR_TIBI_HOME");
                                  break;
  }
  this->current_state=state;
  this->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 DaboCoopAlgNode::supervisor_callback(const sensor_msgs::Joy::ConstPtr& msg) 
{ 
  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->new_joy_cmd=true;
    this->joy_cmd=joy_replan_goal;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_MINUS]==1 && prev_buttons[wiimote::State::MSG_BTN_MINUS]==0)// cancel goal
  {
    ROS_INFO("Cancel"); 
    this->new_joy_cmd=true;
    this->joy_cmd=joy_cancel_goal;
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1 && prev_buttons[wiimote::State::MSG_BTN_HOME]==0)// teleop
  {
    ROS_INFO("Teleop"); 
    this->new_joy_cmd=true;
    this->joy_cmd=joy_teleop;
  }

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

/*  [service callbacks] */

/*  [action callbacks] */
void DaboCoopAlgNode::gui_action_clientDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DaboCoopAlgNode::gui_action_clientDone: Goal Achieved!"); 
  else 
    ROS_INFO("DaboCoopAlgNode::gui_action_clientDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void DaboCoopAlgNode::gui_action_clientActive() 
{ 
  //ROS_INFO("DaboCoopAlgNode::gui_action_clientActive: Goal just went active!"); 
} 

void DaboCoopAlgNode::gui_action_clientFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DaboCoopAlgNode::gui_action_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 ) 
  { 
    gui_action_client_client_.cancelGoal(); 
    //ROS_INFO("DaboCoopAlgNode::gui_action_clientFeedback: Cancelling Action!"); 
  } 
}
void DaboCoopAlgNode::dabo_tibi_actionDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result) 
{ 
  this->alg_.lock();
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionDone: Goal Achieved!"); 
  else 
    ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionDone: %s", state.toString().c_str()); 
  this->dabo_tibi_action_done=true;
  this->alg_.unlock();
  //copy & work with requested result 
} 

void DaboCoopAlgNode::dabo_tibi_actionActive() 
{ 
  //ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionActive: Goal just went active!"); 
} 

void DaboCoopAlgNode::dabo_tibi_actionFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DaboCoopAlgNode::dabo_tibi_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 ) 
  { 
    dabo_tibi_action_client_.cancelGoal(); 
    //ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionFeedback: Cancelling Action!"); 
  } 
}
void DaboCoopAlgNode::dabo_car_actionDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result) 
{ 
  this->alg_.lock();
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DaboCoopAlgNode::dabo_car_actionDone: Goal Achieved!"); 
  else 
    ROS_INFO("DaboCoopAlgNode::dabo_car_actionDone: %s", state.toString().c_str()); 
  this->dabo_car_action_done=true;
  this->alg_.unlock();
  //copy & work with requested result 
} 

void DaboCoopAlgNode::dabo_car_actionActive() 
{ 
  //ROS_INFO("DaboCoopAlgNode::dabo_car_actionActive: Goal just went active!"); 
} 

void DaboCoopAlgNode::dabo_car_actionFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DaboCoopAlgNode::dabo_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 ) 
  { 
    dabo_car_action_client_.cancelGoal(); 
    //ROS_INFO("DaboCoopAlgNode::dabo_car_actionFeedback: Cancelling Action!"); 
  } 
}
void DaboCoopAlgNode::car_dabo_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{ 
  alg_.lock(); 
    //check goal
  ROS_INFO("Cmd: %d",goal->cmd_id);//goal->cmd_id,goal->string_params[0].c_str()); 
  this->car_cmd=(dabo_cmds)goal->cmd_id;
  switch(this->car_cmd)
  {
    case dabo_set_goal: if(goal->string_params.size()==1)
                        {
                          this->car_location_id=goal->string_params[0];
                          this->new_car_cmd=true;
                        }
                        else
                          this->new_car_cmd=false;
                        break;
    case dabo_car_done: this->car_done=true;
                        this->new_car_cmd=true;
                        break;
    default: this->new_car_cmd=false;
             break;
  }
    //execute goal 
  alg_.unlock(); 
} 

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

bool DaboCoopAlgNode::car_dabo_actionIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
  if(this->current_state==dabo_wait_car_done && this->move_base_done)
    ret=true;
  else if(this->current_state==dabo_wait_speech)
    ret=true;
  else
    ret=false;

    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool DaboCoopAlgNode::car_dabo_actionHasSucceedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

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

void DaboCoopAlgNode::car_dabo_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}
void DaboCoopAlgNode::gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{ 
  alg_.lock(); 
    //check goal 
  this->gui_cmd=(gui_cmds)goal->cmd_id;
  this->new_gui_cmd=true;
    //execute goal 
  alg_.unlock(); 
} 

void DaboCoopAlgNode::gui_actionStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

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

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

  return ret; 
} 

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

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

  return ret; 
} 

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

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

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

void DaboCoopAlgNode::move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DaboCoopAlgNode::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("DaboCoopAlgNode::move_baseFeedback: Cancelling Action!"); 
  } 
}
void DaboCoopAlgNode::hri_clientDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
{ 
  this->alg_.lock();
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DaboCoopAlgNode::hri_clientDone: Goal Achieved!"); 
  else 
    ROS_INFO("DaboCoopAlgNode::hri_clientDone: %s", state.toString().c_str()); 
  this->hri_client_done=true;
  this->alg_.unlock();
 
  //copy & work with requested result 
} 

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

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

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

  //wait for the action server to start 
  //will wait for infinite time 
  gui_action_client_client_.waitForServer();  
  ROS_INFO("DaboCoopAlgNode::gui_action_clientMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //gui_action_client_goal_.data = my_desired_goal; 
  gui_action_client_client_.sendGoal(gui_action_client_goal_, 
              boost::bind(&DaboCoopAlgNode::gui_action_clientDone,     this, _1, _2), 
              boost::bind(&DaboCoopAlgNode::gui_action_clientActive,   this), 
              boost::bind(&DaboCoopAlgNode::gui_action_clientFeedback, this, _1)); 
  ROS_INFO("DaboCoopAlgNode::gui_action_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
}
bool DaboCoopAlgNode::dabo_tibi_actionMakeActionRequest() 
{ 
  ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionMakeActionRequest: Starting New Request!"); 

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

  //send a goal to the action 
  //dabo_tibi_action_goal_.data = my_desired_goal; 
  dabo_tibi_action_client_.sendGoal(dabo_tibi_action_goal_, 
              boost::bind(&DaboCoopAlgNode::dabo_tibi_actionDone,     this, _1, _2), 
              boost::bind(&DaboCoopAlgNode::dabo_tibi_actionActive,   this), 
              boost::bind(&DaboCoopAlgNode::dabo_tibi_actionFeedback, this, _1)); 
  this->dabo_tibi_action_done=false;
  ROS_INFO("DaboCoopAlgNode::dabo_tibi_actionMakeActionRequest: Goal Sent. Wait for Result!"); 
  
  return true;
}

bool DaboCoopAlgNode::dabo_car_actionMakeActionRequest() 
{ 
  ROS_INFO("DaboCoopAlgNode::dabo_car_actionMakeActionRequest: Starting New Request!"); 

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

  //send a goal to the action 
  //dabo_car_action_goal_.data = my_desired_goal; 
  dabo_car_action_client_.sendGoal(dabo_car_action_goal_, 
              boost::bind(&DaboCoopAlgNode::dabo_car_actionDone,     this, _1, _2), 
              boost::bind(&DaboCoopAlgNode::dabo_car_actionActive,   this), 
              boost::bind(&DaboCoopAlgNode::dabo_car_actionFeedback, this, _1)); 
  dabo_car_action_done=false;
  ROS_INFO("DaboCoopAlgNode::dabo_car_actionMakeActionRequest: Goal Sent. Wait for Result!"); 

  return true;
}

bool DaboCoopAlgNode::move_baseMakeActionRequest() 
{ 
  ROS_INFO("DaboCoopAlgNode::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("DaboCoopAlgNode::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(&DaboCoopAlgNode::move_baseDone,     this, _1, _2), 
              boost::bind(&DaboCoopAlgNode::move_baseActive,   this), 
              boost::bind(&DaboCoopAlgNode::move_baseFeedback, this, _1)); 
  this->move_base_done=false;
  ROS_INFO("DaboCoopAlgNode::move_baseMakeActionRequest: Goal Sent. Wait for Result!"); 

  return true;
}

bool DaboCoopAlgNode::hri_clientMakeActionRequest() 
{ 
  ROS_INFO("DaboCoopAlgNode::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("DaboCoopAlgNode::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(&DaboCoopAlgNode::hri_clientDone,     this, _1, _2), 
              boost::bind(&DaboCoopAlgNode::hri_clientActive,   this), 
              boost::bind(&DaboCoopAlgNode::hri_clientFeedback, this, _1)); 
  this->hri_client_done=false;
  ROS_INFO("DaboCoopAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
  ROS_INFO("DaboCoopAlgNode::hri_clientMakeActionRequest: Goal tts: %s",this->hri_client_goal_.sequence_file[0].c_str()); 

  return true;
}

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

  this->alg_.unlock();
}

void DaboCoopAlgNode::addNodeDiagnostics(void)
{
}

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