tibi_dabo_guide_nav_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_guide_nav_alg_node.h"
#include <limits>

TibiDaboGuideNavAlgNode::TibiDaboGuideNavAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboGuideNavAlgorithm>(),
  move_guide_aserver_(public_node_handle_, "move_guide"),
  hri_client_client_("hri_client", true),
  move_base_client_("move_base", true),
  tf_listener_(ros::Duration(10.f))
{
  //init class attributes if necessary
  this->loop_rate_ = 1;//in [Hz]

  this->move_base_done        = true;
  this->distanceThresholdFar  = -1;
  this->distanceThresholdNear = -1;
  this->current_state         = guide_idle;
  this->public_node_handle_.getParam("robot_frame",this->robot_frame);
  this->person_distance       = -1.0;
  this->person_lost           = true;
  this->hri_client_done       = true;
  this->success               = false;

  // [init publishers]
  
  // [init subscribers]

  this->peopleSet_subscriber_ = this->public_node_handle_.subscribe("peopleSet", 100, &TibiDaboGuideNavAlgNode::peopleSet_callback, this);
  
  // [init services]
  
  // [init clients]
  change_speed_client_ = this->public_node_handle_.serviceClient<iri_diff_local_planner::change_max_vel>("change_max_vel");
  get_goal_client_ = this->public_node_handle_.serviceClient<iri_goal_database::get_goal>("get_goal");
  
  // [init action servers]
  move_guide_aserver_.registerStartCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideStartCallback, this, _1)); 
  move_guide_aserver_.registerStopCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideStopCallback, this)); 
  move_guide_aserver_.registerIsFinishedCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideIsFinishedCallback, this)); 
  move_guide_aserver_.registerHasSucceedCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideHasSucceedCallback, this)); 
  move_guide_aserver_.registerGetResultCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideGetResultCallback, this, _1)); 
  move_guide_aserver_.registerGetFeedbackCallback(boost::bind(&TibiDaboGuideNavAlgNode::move_guideGetFeedbackCallback, this, _1)); 
  move_guide_aserver_.start();  
  this->new_guide_request = false;
  this->target_id = -1;
  // [init action clients]

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

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

void TibiDaboGuideNavAlgNode::mainNodeThread(void)
{
  static guiding_states state=guide_idle;
  static int timeout=0, num_lost=0,num_wait=0;

  this->alg_.lock();
  switch(state)
  {
    case guide_idle: if(this->new_guide_request)
                     {
                       change_speed_srv_.request.new_max_vel = 0.2;
                       ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!"); 
                       if (change_speed_client_.call(change_speed_srv_))
                       {
                         get_goal_srv_.request.location_id = this->location_id;
                         get_goal_srv_.request.robot_id = this->robot_id;
                         ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!");
                         if (get_goal_client_.call(get_goal_srv_))
                         {
                           //ROS_INFO("TibiDaboGuideNavAlgNode:: Response: %s", get_goal_srv_.response.result); 
                           //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;
                           //move_base_goal_.target_pose.header.stamp=ros::Time::now();
                           //move_base_goal_.target_pose.header.frame_id="/map";
                           //move_baseMakeActionRequest();
                           move_base_goal_.target_pose.pose.position.x=-1.0;
                           move_base_goal_.target_pose.pose.position.y=0.0;
                           move_base_goal_.target_pose.pose.position.z=0.0;
                           move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(3.14159);
                           move_base_goal_.target_pose.header.stamp=ros::Time::now();
                           move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                           move_baseMakeActionRequest();
                           state=guide_wait_goal;
                           this->new_guide_request=false;
                           num_lost=0;
                           num_wait=0;
                           timeout=0;
                         }
                         else
                         {
                           ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic get_goal "); 
                           state=guide_idle;
                         }
                       }
                       else
                       {
                         ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic change_max_vel "); 
                         state=guide_idle;
                       }
                     }
                     else
                       state=guide_idle;
                     ROS_INFO("State: GUIDE_IDLE");  
                     break;
    case guide_wait_goal: if(move_base_done)
                          {
                            move_base_goal_.target_pose.header.stamp=ros::Time::now();
                            move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                            move_base_goal_.target_pose.pose.position.x=0.0;
                            move_base_goal_.target_pose.pose.position.y=0.0;
                            move_base_goal_.target_pose.pose.position.z=0.0;
                            move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(atan2(this->person_pose.pose.position.y,this->person_pose.pose.position.x));
                            move_baseMakeActionRequest();
                            state=guide_face_person;
                          }
                          else
                          {
                            if(person_lost) 
                            {
                              // stop the robot
                              this->move_base_client_.cancelGoal(); 
                              state=guide_person_lost_hri;;
                            }
                            else
                            {
                              if(this->person_distance>this->distanceThresholdFar)
                              {
                                // stop and turn arround
                                state=guide_cancel_goal;
                              }
                              else if(this->person_distance>this->distanceThresholdNear)
                              {
                                // slow down
                                state=guide_slow_down;
                              }
                              else
                              {
                                state=guide_wait_goal;
                              }
                            }
                          }
                          ROS_INFO("State: GUIDE_WAIT_GOAL");  
                          break;
    case guide_slow_down: // change the planner max speed with a service     
                          change_speed_srv_.request.new_max_vel = 0.1;
                          //ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!"); 
                          if (change_speed_client_.call(change_speed_srv_)) 
                          {  
                            //ROS_INFO("TibiDaboGuideNavAlgNode:: Response: %s", change_speed_srv_.response.result); 
                            if(move_base_done)
                            {
                              move_base_goal_.target_pose.header.stamp=ros::Time::now();
                              move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                              move_base_goal_.target_pose.pose.position.x=0.0;
                              move_base_goal_.target_pose.pose.position.y=0.0;
                              move_base_goal_.target_pose.pose.position.z=0.0;
                              move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(atan2(this->person_pose.pose.position.y,this->person_pose.pose.position.x));
                              move_baseMakeActionRequest();
                              state=guide_face_person;
                            }
                            else
                            {
                              if(person_lost)
                              {
                                // stop the robot
                                this->move_base_client_.cancelGoal();
                                state=guide_person_lost_hri;;
                              }
                              else
                              {
                                if(this->person_distance>this->distanceThresholdFar)
                                {
                                  // stop and turn arround
                                  state=guide_cancel_goal;
                                }
                                else if(this->person_distance<this->distanceThresholdNear)
                                {
                                  change_speed_srv_.request.new_max_vel = 0.2;
                                  //ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!"); 
                                  if (change_speed_client_.call(change_speed_srv_))
                                  {
                                    state=guide_wait_goal;
                                  }
                                  else
                                  {
                                    //ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic change_speed "); 
                                    state=guide_slow_down;
                                  }
                                }
                                else
                                {
                                  state=guide_slow_down;
                                }
                              }
                            }
                          }  
                          else 
                          {  
                            //ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic change_speed "); 
                            state=guide_slow_down;
                          }
                          ROS_INFO("State: GUIDE_SLOW_DOWN");  
                          break;
    case guide_cancel_goal: // cancel the current goal
                            this->move_base_client_.cancelGoal(); 
                            state=guide_turn_arround;
                            ROS_INFO("State: GUIDE_CANCEL_GOAL");  
                            break;
    case guide_turn_arround: if(this->move_base_done)
                             {
                               move_base_goal_.target_pose.header.stamp=ros::Time::now();
                               move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                               move_base_goal_.target_pose.pose.position.x=0.0;
                               move_base_goal_.target_pose.pose.position.y=0.0;
                               move_base_goal_.target_pose.pose.position.z=0.0;
                               move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(atan2(this->person_pose.pose.position.y,this->person_pose.pose.position.x));
                               move_baseMakeActionRequest();
                               state=guide_wait_person_hri;                       
                             }
                             else
                               state=guide_turn_arround;
                             ROS_INFO("State: GUIDE_TURN_ARROUND");  
                             break;
    case guide_wait_person_hri: if(this->move_base_done)
                                {
                                  // start HRI action
                                  this->hri_client_goal_.sequence_file[0] = "Your are quite far, please come closer to me!";
                                  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=guide_wait_person;
                                  timeout=0;
                                  num_wait++;
                                }
                                else
                                  state=guide_wait_person_hri;
                                ROS_INFO("State: GUIDE_WAIT_PERSON_HRI");  
                                break;
    case guide_wait_person: if(this->hri_client_done)
                            {
                              if(person_lost)
                                state=guide_person_lost_hri;
                              else
                              {
                                if(this->person_distance<this->distanceThresholdNear)
                                {
                                  // continue with the goal
                                  get_goal_srv_.request.location_id = this->location_id;
                                  get_goal_srv_.request.robot_id = this->robot_id;
                                  ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!");
                                  if (get_goal_client_.call(get_goal_srv_))
                                  {
                                    //ROS_INFO("TibiDaboGuideNavAlgNode:: Response: %s", get_goal_srv_.response.result); 
                                    //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;
                                    //move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                    //move_base_goal_.target_pose.header.frame_id="/map";
                                    //move_baseMakeActionRequest();
                                    move_base_goal_.target_pose.pose.position.x=-1.0;
                                    move_base_goal_.target_pose.pose.position.y=0.0;
                                    move_base_goal_.target_pose.pose.position.z=0.0;
                                    move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(3.14159);
                                    move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                    move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                                    move_baseMakeActionRequest();
                                    state=guide_wait_goal;
                                  }
                                  else
                                  {
                                    ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic get_goal "); 
                                    state=guide_wait_person;
                                  }
                                }
                                else if(timeout>5)
                                {
                                  if(num_wait>1)
                                  {
                                    // start HRI
                                    this->hri_client_goal_.sequence_file[0] = "If you will not come closer, I will look for another person.";
                                    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=guide_ignored;
                                  }
                                  else
                                    state=guide_wait_person_hri;
                                }
                                else
                                {
                                  state=guide_wait_person;
                                  timeout+=this->loop_rate_.expectedCycleTime().toSec();
                                }
                              }
                            }
                            else
                              state=guide_wait_person;
                            ROS_INFO("State: GUIDE_WAIT_PERSON");  
                            break;
    case guide_person_lost_hri: if(num_lost<3)
                                {
                                  if(this->person_lost)
                                  {
                                    // start HRI action
                                    this->hri_client_goal_.sequence_file[0] = "I have lost you, please come closer!";
                                    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=guide_person_lost;
                                    timeout=0;
                                  }
                                  else
                                  {
                                    // continue with the goal
                                    get_goal_srv_.request.location_id = this->location_id;
                                    get_goal_srv_.request.robot_id = this->robot_id;
                                    ROS_INFO("TibiDaboGuideNavAlgNode:: Sending New Request!");
                                    if (get_goal_client_.call(get_goal_srv_))
                                    {
                                      //ROS_INFO("TibiDaboGuideNavAlgNode:: Response: %s", get_goal_srv_.response.result); 
                                      //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;
                                      //move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                      //move_base_goal_.target_pose.header.frame_id="/map";
                                      //move_baseMakeActionRequest();
                                      move_base_goal_.target_pose.pose.position.x=-1.0;
                                      move_base_goal_.target_pose.pose.position.y=0.0;
                                      move_base_goal_.target_pose.pose.position.z=0.0;
                                      move_base_goal_.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(3.14159);
                                      move_base_goal_.target_pose.header.stamp=ros::Time::now();
                                      move_base_goal_.target_pose.header.frame_id=this->robot_frame;
                                      move_baseMakeActionRequest();
                                      state=guide_wait_goal;
                                    }
                                    else
                                    {
                                      ROS_INFO("TibiDaboGuideNavAlgNode:: Failed to Call Server on topic get_goal "); 
                                      state=guide_wait_person_hri;
                                    }
                                  }
                                  num_lost++;
                                }
                                else
                                {
                                  this->hri_client_goal_.sequence_file[0] = "You are ignoring me, I will look for another person!";
                                  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=guide_ignored;
                                }
                                ROS_INFO("State: GUIDE_PERSON_LOST_HRI");  
                                break;
    case guide_ignored: if(this->hri_client_done)
                        {
                          state=guide_idle;
                          this->success=false;
                        }
                        else
                          state=guide_ignored;
                        ROS_INFO("State: GUIDE_IGNORED");  
                        break;
    case guide_person_lost: if(this->hri_client_done)
                            {
                              if(timeout>5)
                                state=guide_person_lost_hri;
                              else
                              {
                                state=guide_person_lost;
                                timeout+=this->loop_rate_.expectedCycleTime().toSec();
                              }
                            }
                            else
                              state=guide_person_lost;
                            ROS_INFO("State: GUIDE_PERSON_LOST");  
                            break;
    case guide_face_person: if(this->move_base_done)
                            {
                              state=guide_idle;
                              this->success=true;
                            }
                            else
                              state=guide_face_person; 
                            ROS_INFO("State: GUIDE_FACE_PERSON");  
                            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 TibiDaboGuideNavAlgNode::peopleSet_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg) 
{ 
  ROS_DEBUG("TibiDaboGuideNavAlgNode::peopleSet_callback: New Message Received"); 
  geometry_msgs::PoseStamped closest_pose;
  unsigned int i,closest_id=this->target_id;
  double closest_dist=std::numeric_limits<double>::max(),x0,y0;
  geometry_msgs::PoseStamped poseOut;

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
    if(this->target_id!=-1 && this->current_state!=guide_idle)
    {
      if(msg->peopleSet.size()==0)
        this->person_lost=true;
      else
      {
        for(i=0; i<msg->peopleSet.size(); i++)
        {
          // found target_id
          double x = msg->peopleSet[i].x;
          double y = msg->peopleSet[i].y;
          if(msg->header.frame_id != this->robot_frame)
          {
            // transform target pose to target_frame (base_link)
            try
            {
              std::string source_frame = msg->header.frame_id;
              std::string target_frame = this->robot_frame;
              ros::Time   target_time  = msg->header.stamp;
              bool tf_exists = tf_listener_.waitForTransform(target_frame, source_frame, target_time, ros::Duration(5), ros::Duration(0.01));
              if(tf_exists)
              {
                geometry_msgs::PoseStamped poseIn;
                poseIn.header = msg->header;
                poseIn.pose.position.x   = msg->peopleSet[i].x;
                poseIn.pose.position.y   = msg->peopleSet[i].y;
                poseIn.pose.orientation=tf::createQuaternionMsgFromYaw(0);
                tf_listener_.transformPose( target_frame, poseIn, poseOut);
                // update transformed x,y coordinates
                x = poseOut.pose.position.x;
                y = poseOut.pose.position.y;
              }
              else
              {
                ROS_INFO("TibiDaboGuideNavAlgNode::No transform: %s-->%s", source_frame.c_str(), target_frame.c_str());
                break;
              }
            }
            catch (tf::TransformException &ex)
            {
              ROS_INFO("TibiDaboGuideNavAlgNode:: %s",ex.what());
              break;
            }
          }
          if(msg->peopleSet[i].targetId == this->target_id)
          {
            //distance to target
            this->person_distance = sqrt(x*x+y*y);
            this->person_lost=false;
            this->person_pose=poseOut;
            ROS_INFO("Target id: %d at %f meters",this->target_id,this->person_distance);
            break;
          }
          else
          {
            x0=this->person_pose.pose.position.x;
            y0=this->person_pose.pose.position.y;
            if(sqrt((x-x0)*(x-x0)+(y-y0)*(y-y0))<closest_dist)
            {
              closest_dist=sqrt(x*x+y*y);
              closest_pose=this->person_pose;
              closest_id=msg->peopleSet[i].targetId;
            }
          }
        }
        if(i>=msg->peopleSet.size())
        {
          ROS_INFO("Target id: %d not prenset, switching to target id %d at %f meters", this->target_id,closest_id, closest_dist);
          this->person_distance=closest_dist;
          this->person_pose=closest_pose;
          this->person_lost=false;
          this->target_id=closest_id;
        }
      }
    }
    //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->peopleSet_mutex_.exit(); 
}

/*  [service callbacks] */

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

  //copy & work with requested result 
} 

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

void TibiDaboGuideNavAlgNode::hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboGuideNavAlgNode::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("TibiDaboGuideNavAlgNode::hri_clientFeedback: Cancelling Action!"); 
  } 
}
void TibiDaboGuideNavAlgNode::move_guideStartCallback(const tibi_dabo_msgs::guideGoalGoalConstPtr& goal)
{ 
  alg_.lock(); 
  ROS_INFO("TibiDaboGuideNavAlgNode::New guide request"); 
  this->location_id=goal->location_id;
  this->robot_id=goal->robot_id;
  this->target_id = goal->target_id;
  this->new_guide_request=true;
  alg_.unlock(); 
} 

void TibiDaboGuideNavAlgNode::move_guideStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action
    //move_base_client_.cancelGoal();
  alg_.unlock(); 
} 

bool TibiDaboGuideNavAlgNode::move_guideIsFinishedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

bool TibiDaboGuideNavAlgNode::move_guideHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if goal was accomplished 
    ret = this->success;
  alg_.unlock(); 

  return ret; 
} 

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

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

void TibiDaboGuideNavAlgNode::move_baseDone(const actionlib::SimpleClientGoalState& state,  const move_base_msgs::MoveBaseResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 )
    ROS_INFO("TibiDaboGuideNavAlgNode::move_baseDone: Goal Achieved!");
  else 
    ROS_INFO("TibiDaboGuideNavAlgNode::move_baseDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
  this->move_base_done=true;
} 

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

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

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

  //wait for the action server to start 
  //will wait for infinite time 
  hri_client_client_.waitForServer();  
  ROS_INFO("TibiDaboGuideNavAlgNode::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(&TibiDaboGuideNavAlgNode::hri_clientDone,     this, _1, _2), 
              boost::bind(&TibiDaboGuideNavAlgNode::hri_clientActive,   this), 
              boost::bind(&TibiDaboGuideNavAlgNode::hri_clientFeedback, this, _1)); 
  this->hri_client_done=false;
  ROS_INFO("TibiDaboGuideNavAlgNode::hri_clientMakeActionRequest: Goal Sent. Wait for Result!"); 
}
void TibiDaboGuideNavAlgNode::move_baseMakeActionRequest() 
{ 
  ROS_INFO("TibiDaboGuideNavAlgNode::move_baseMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  move_base_client_.waitForServer();  
  ROS_INFO("TibiDaboGuideNavAlgNode::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(&TibiDaboGuideNavAlgNode::move_baseDone,     this, _1, _2), 
              boost::bind(&TibiDaboGuideNavAlgNode::move_baseActive,   this), 
              boost::bind(&TibiDaboGuideNavAlgNode::move_baseFeedback, this, _1)); 
  ROS_INFO("TibiDaboGuideNavAlgNode::move_baseMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->move_base_done=false;
}

void TibiDaboGuideNavAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
    this->distanceThresholdFar  = config.distanceThresholdFar;
    this->distanceThresholdNear = config.distanceThresholdNear;
    this->robot_frame           = config.robot_frame;
    this->target_id             = config.target_id;
  this->alg_.unlock();
}

void TibiDaboGuideNavAlgNode::addNodeDiagnostics(void)
{
}

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