tibi_dabo_hri_teleop_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_hri_teleop_alg_node.h"

#include <wiimote/State.h>

TibiDaboHriTeleopAlgNode::TibiDaboHriTeleopAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboHriTeleopAlgorithm>(),
  hri_client_("hri", true),
  hri_action_in_progress_(false),
  xml_files(NUM_CLIENTS, std::string(""))
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 100, &TibiDaboHriTeleopAlgNode::joy_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  
  hri_goal_.sequence_file   = std::vector<std::string>(NUM_CLIENTS, std::string(""));
  hri_goal_.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);
  this->sequence_file="";
  button_files_.resize(10);
  button_files_[0]= this->alg_.xml_path + "/motion/" + "motion1.xml";
  button_files_[1]= this->alg_.xml_path + "/motion/" + "motion2.xml";
  button_files_[2]= this->alg_.xml_path + "/motion/" + "motionPlus.xml";
  button_files_[3]= this->alg_.xml_path + "/motion/" + "motionMinus.xml";
  button_files_[4]= this->alg_.xml_path + "/motion/" + "motionHome.xml";
  button_files_[5]= this->alg_.xml_path + "/motion/" + "motionB1.xml";
  button_files_[6]= this->alg_.xml_path + "/motion/" + "motionB2.xml";
  button_files_[7]= this->alg_.xml_path + "/motion/" + "motionBPlus.xml";
  button_files_[8]= this->alg_.xml_path + "/motion/" + "motionBMinus.xml";
  button_files_[9]= this->alg_.xml_path + "/motion/" + "motionBHome.xml";
}

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

void TibiDaboHriTeleopAlgNode::mainNodeThread(void)
{
  // [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 TibiDaboHriTeleopAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& joy_msg) 
{ 
  bool execute_action=false;
  static bool first=true;

  if(first)
  {
    prev_buttons_.resize(joy_msg->buttons.size());
    first=false;
  }
  
  index_ = indexFromButtons(joy_msg->buttons);
  if(index_!=-1)
  {
    execute_action=true;
    ROS_INFO("TibiDaboHriTeleopAlgNode::joy_callback: hri sequence index %i",index_);
  }
  /*
  for(unsigned int i=0; i<joy_msg->buttons.size(); i++)
  {
    if(joy_msg->buttons[i]==1 && prev_buttons_[i]==0)
    {
      index_= useExtraButton(i);
      if(index_!=-1)
        execute_action=true;
    }
  }
  */ 

  if(hri_action_in_progress_ && execute_action)
  {
    ROS_INFO("TibiDaboHriTeleopAlgNode::joy_callback: hri_client_.cangelGoal()!");
    hri_client_.cancelGoal();
  }  
  // Request Action
  if(!hri_action_in_progress_ && execute_action)
  {
    for(unsigned int i=0; i<this->alg_.motion_seq_files_.size(); i++)
    {
      if(button_files_[index_]==this->alg_.motion_seq_files_[i])
      {
        this->alg_.load_goal(this->alg_.motion_seq_files_[i],xml_files);
        break;
      }
    }
    this->hriMakeActionRequest(xml_files);
    //hriMakeActionRequest();
  }
  prev_buttons_ = joy_msg->buttons;
}

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboHriTeleopAlgNode::hriDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
{ 
  ROS_INFO("TibiDaboHriTeleopAlgNode::HRIDone: %s", state.toString().c_str());
  if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    ROS_INFO("TibiDaboHriTeleopAlgNode::HRIDone: Goal Achieved!");
  }
  else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
  {
    ROS_INFO("TibiDaboHriTeleopAlgNode::HRIDone: Sequence aborted!");
  }
  else
  {
    ROS_INFO("TibiDaboHriTeleopAlgNode::HRIDone: Unknown termination state!");
  }
  hri_action_in_progress_=false;
} 

void TibiDaboHriTeleopAlgNode::hriActive() 
{ 
  //ROS_INFO("TibiDaboHriTeleopAlgNode::hriActive: Goal just went active!"); 
} 

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

/*  [action requests] */
void TibiDaboHriTeleopAlgNode::hriMakeActionRequest(std::vector<std::string> &xml_files) 
{ 
   ROS_INFO("TibiDaboHriTeleopAlgNode::hriMakeActionRequest: Starting New Request!"); 
 
   //wait for the action server to start 
   //will wait for infinite time
   ROS_DEBUG("TibiDaboHriTeleopAlgNode::hriMakeActionRequest: Waiting for Server...");
   hri_client_.waitForServer();  
   ROS_INFO("TibiDaboHriTeleopAlgNode::hriMakeActionRequest: Server is Available!"); 
 
   //set goal
   hri_goal_.sequence_file[TTS_]       = xml_files[TTS_];
   hri_goal_.sequence_file[LEDS_]      = xml_files[LEDS_];
   hri_goal_.sequence_file[HEAD_]      = xml_files[HEAD_];
   hri_goal_.sequence_file[RIGHT_ARM_] = xml_files[RIGHT_ARM_];
   hri_goal_.sequence_file[LEFT_ARM_]  = xml_files[LEFT_ARM_];
 
   //send a goal to the action 
   hri_client_.sendGoal(hri_goal_, 
               boost::bind(&TibiDaboHriTeleopAlgNode::hriDone,     this, _1, _2), 
               boost::bind(&TibiDaboHriTeleopAlgNode::hriActive,   this), 
               boost::bind(&TibiDaboHriTeleopAlgNode::hriFeedback, this, _1)); 
   ROS_INFO("TibiDaboHriTeleopAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!"); 
   hri_action_in_progress_ = true;
}

void TibiDaboHriTeleopAlgNode::node_config_update(Config &config, uint32_t level)
{
  ROS_INFO("TibiDaboHriTeleopAlgNode: Dynamic reconfigure!");
  this->alg_.lock();
  button_files_[0] = config.path + "/motion/" + config.file1;
  button_files_[1] = config.path + "/motion/" + config.file2;
  button_files_[2] = config.path + "/motion/" + config.file3;
  button_files_[3] = config.path + "/motion/" + config.file4;
  button_files_[4] = config.path + "/motion/" + config.file5;
  button_files_[5] = config.path + "/motion/" + config.file6;
  button_files_[6] = config.path + "/motion/" + config.file7;
  button_files_[7] = config.path + "/motion/" + config.file8;
  button_files_[8] = config.path + "/motion/" + config.file9;
  button_files_[9] = config.path + "/motion/" + config.file10;
  this->alg_.config_   = config; 
  this->alg_.unlock();
}


void TibiDaboHriTeleopAlgNode::addNodeDiagnostics(void)
{
}

int TibiDaboHriTeleopAlgNode::indexFromButtons(const std::vector<int> & buttons)
{
  ROS_DEBUG("Tibi Dabo wiimote use extra Button");
  int index=-1;
  for(unsigned int i=0; i<buttons.size(); i++)
  {
    if(buttons[i]==1 && prev_buttons_[i]==0 && i!=wiimote::State::MSG_BTN_B)
    {
      index= i;
    }
  }
  
  switch(index)
  {
    case wiimote::State::MSG_BTN_1:
      ROS_DEBUG("Button 1!");
      if(buttons[wiimote::State::MSG_BTN_B]==0)
        return 0;
      else
        return 5;
      break;

    case wiimote::State::MSG_BTN_2:
      ROS_DEBUG("Button 2!");
      if(buttons[wiimote::State::MSG_BTN_B]==0)
        return 1;
      else
        return 6;
      break;

    case wiimote::State::MSG_BTN_B:
      ROS_DEBUG("Button B!");
      return -1;
      break;

    case wiimote::State::MSG_BTN_PLUS:
      ROS_DEBUG("Button (+)!");
      if(buttons[wiimote::State::MSG_BTN_B]==0)
        return 2;
      else
        return 7;
      break;

    case wiimote::State::MSG_BTN_MINUS:
      ROS_DEBUG("Button (-)!");
      if(buttons[wiimote::State::MSG_BTN_B]==0)
        return 3;
      else
        return 8;
      break;

    case wiimote::State::MSG_BTN_HOME:
      ROS_DEBUG("Button Home!");
      if(buttons[wiimote::State::MSG_BTN_B]==0)
        return 4;
      else
        return 9;
      break;

    default:
      ROS_DEBUG("Button UNDEFINED!");
      return -1;
      break;
  }

}

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