tibi_dabo_hideandseek_alg_node.cpp

Go to the documentation of this file.

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

TibiDaboHideandseekAlgNode::TibiDaboHideandseekAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboHideandseekAlgorithm>(),
  move_base_client_("move_base", true)
{
  ROS_INFO("TibiDaboHideandseekAlgNode::constructor!");
  //init class attributes if necessary
  this->loop_rate_ = 1;//in [Hz] //TODO RATE? turns

  this->seekerPoseReceived = false;
  this->hiderPoseReceived = false;
  this->moveBaseActive     = false;
  this->gameStatus= STATE_IDLE;
  this->gameIteration=0;
  this->paused=false;
  this->ready2Move=false;

  this->previousId=-1;
  //this->previousXY;
  std::string path;
  std::string pathOut;
  std::string map;
  std::string experiment;
  int solver;
  int windist;
  double cellsize;
  public_node_handle_.getParam("path", path);
  public_node_handle_.getParam("pathOut", pathOut);
  public_node_handle_.getParam("map", map);
  public_node_handle_.getParam("experiment", experiment);
  public_node_handle_.getParam("solver", solver);
                /* solver should be (see SeekerHSParam)
                    static const char SOLVER_NOT_SET = 0;
                    static const char SOLVER_OFFLINE = 1;
                    static const char SOLVER_LAYERED = 2;
                    static const char SOLVER_LAYERED_COMPARE = 3;
                    static const char SOLVER_MCVI_OFFLINE = 4;
                    static const char SOLVER_POMCP = 5;

                    static const char SOLVER_SMART_SEEKER = 9;
                */

  public_node_handle_.getParam("windist", windist);
  public_node_handle_.getParam("cellsize", cellsize);
  
  
  //this->alg_.initialize(path, pathOut, map, experiment, solver);
  //this->alg_.setWinDistToHider(windist);
  //this->alg_.setCellSizeM(cellsize);

  // [init publishers]
  
  // [init subscribers]
  this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &TibiDaboHideandseekAlgNode::people_callback, this);
  this->joy_subscriber_    = this->public_node_handle_.subscribe("joy",    100, &TibiDaboHideandseekAlgNode::joy_callback,    this);
  this->odom_subscriber_   = this->public_node_handle_.subscribe("odom",   100, &TibiDaboHideandseekAlgNode::odom_callback,   this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  //ros::Duration(1).sleep();
  this->alg_.initialize(path, pathOut, map, experiment, solver);
  this->alg_.setWinDistToHider(windist);
  this->alg_.setCellSizeM(cellsize);

  ROS_INFO("TibiDaboHideandseekAlgNode::constructor end");
}

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

void TibiDaboHideandseekAlgNode::mainNodeThread(void)
{ 
  // ros::Duration(0.1).sleep();
  this->alg_.lock();

  if(this->seekerPoseReceived /*&& this->hiderPoseReceived*/ && !this->moveBaseActive && this->paused && !this->ready2Move)
  {
    //ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread! new data");

    // if status is IDLE (first iteration), game must be initialized
    if(this->gameStatus==STATE_IDLE)
    {
      ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread: initGame!");
      ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread: seeker position x,y: %f, %f. In frame %s !!!", seekerPose.pose.position.x, seekerPose.pose.position.y, seekerPose.header.frame_id.c_str() );
      this->alg_.initGame(this->seekerPose, this->hiderPoses);
      ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread: initGame Done!");
      this->gameStatus=STATE_PLAYING;
    }
    else if(this->gameStatus!=STATE_FINISHED)
    {
      //send seeker and hider pose, receive seeker goal and game status
      this->seekerGoal = this->seekerPose;
      ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread: nextIteration!");
      bool ok = this->alg_.iteration(this->seekerPose, this->hiderPoses, this->seekerGoal, this->gameStatus);
      ROS_INFO("TibiDaboHideandseekAlgNode::mainNodeThread: nextIteration Done!");
      
      if(!ok)
      {
        ROS_ERROR("TibiDaboHideandseekAlgNode::mainNodeThread: unexpected iteration failed, check hider/seeker poses !!!");
        this->paused=false;
        this->hiderPoseReceived  = true;
      }
      else
      {
        switch(gameStatus)
        {
          case STATE_PLAYING:
            this->ready2Move=true;
            ROS_INFO("GAME: READY FOR NEXT TURN. PRESS WIIMOTE BUTTON 1!!!");
            //TEST uncomment
            //this->paused=false;
            break;
          case STATE_WIN_SEEKER:
            //say "I WIN"
            ROS_INFO("GAME: ROBOT WINS !!!");
            this->gameStatus=STATE_FINISHED;
            break;
          case STATE_WIN_HIDER:
            //say "YOU WIN"
            ROS_INFO("GAME: HUMAN WINS !!!");
            this->gameStatus=STATE_FINISHED;
            break;
          case STATE_TIE:
            //say "IT'S A TIE"
            ROS_INFO("GAME: IT'S A TIE !!!");
            this->gameStatus=STATE_FINISHED;
            break;
          case STATE_FINISHED:
            ROS_INFO("GAME: FINISHED!!!");
            break;
          case STATE_IDLE:
            ROS_INFO("GAME: IDLE!!!");
            break;
          default:
            break;
        }
      }
    }
    else
    {
      ROS_INFO("GAME: FINISHED !!!");
      ROS_INFO("CTRL-C TO EXIT");
    }
    //this->seekerPoseReceived = false;
    //this->hiderPoseReceived  = false;
  }
  // when ready2Move and not paused (joy button1 pressed), send robot goal (and the person should do a move too)
  else if(this->ready2Move && !this->paused)
  {
    //send new seeker goal
    move_base_goal_.target_pose = this->seekerGoal;
    ROS_INFO("GAME: NEXT TURN. MOVING!!!");
    move_baseMakeActionRequest();
    this->gameIteration++;
    //this->paused=true;
    this->ready2Move=false;
  }
  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]
  //move_baseMakeActionRequest();

  // [publish messages]
}

/*  [subscriber callbacks] */
void TibiDaboHideandseekAlgNode::people_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg) 
{ 
  //ROS_INFO("TibiDaboHideandseekAlgNode::people_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->people_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 
  if(this->seekerPoseReceived && !this->hiderPoseReceived && this->gameStatus!=STATE_FINISHED && !this->ready2Move && msg->header.frame_id.size()!=0 )
  {
    if(msg->header.frame_id != "/map")
    {
      ROS_ERROR("TibiDaboHideandseekAlgNode::people_callback: people detections need to be in /map frame_id, but are in %s instead", 
                msg->header.frame_id.c_str());
    }

    hiderPoses.clear();
    for(unsigned int i=0; i<msg->peopleSet.size(); i++)
    {
      geometry_msgs::Point person;
      person.x = msg->peopleSet[i].x;
      person.y = msg->peopleSet[i].y;
      person.z = 1.0; //quality TODO
      hiderPoses.push_back(person);
    }
    this->hiderPoseReceived = true;
  }
  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->people_mutex_.exit(); 
}

void TibiDaboHideandseekAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg) 
{ 
  ROS_DEBUG("TibiDaboHideandseekAlgNode::joy_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->joy_mutex_.enter(); 



  static bool first=true;

  if(first)
  {
    prev_buttons.resize(msg->buttons.size());
    first=false;
  }

  //std::cout << msg->data << std::endl; 
  if(msg->buttons[wiimote::State::MSG_BTN_HOME]==1 && prev_buttons[wiimote::State::MSG_BTN_HOME]==0)
  {
    ROS_INFO("GAME: BASE HOME BUTTON PRESSED !!!");
    this->gameStatus=STATE_WIN_HIDER;
    this->alg_.stopGame(this->gameStatus);
  }
  if(msg->buttons[wiimote::State::MSG_BTN_1]==1 && prev_buttons[wiimote::State::MSG_BTN_1]==0)
  {
    this->paused=false;
    ROS_INFO("GAME: NEXT TURN BUTTON 1 PRESSED, paused=%d !!!", this->paused);
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_2]==1 && prev_buttons[wiimote::State::MSG_BTN_2]==0)
  {
    this->paused=true;
    this->hiderPoseReceived  = false;
    this->seekerPoseReceived  = false;
    ROS_INFO("GAME: END OF TURN BUTTON 2 PRESSED, paused=%d !!!", this->paused);
  }
  else if(msg->buttons[wiimote::State::MSG_BTN_A]==1 && prev_buttons[wiimote::State::MSG_BTN_A]==0)
  {
    ROS_INFO("GAME: goal cancelled, use teleop to adjust it!!!");
    // cancel Goal
    move_base_client_.cancelGoal(); 
  }



  //unlock previously blocked shared variables 
  this->prev_buttons=msg->buttons;
  this->alg_.unlock(); 
  //this->joy_mutex_.exit(); 
}

void TibiDaboHideandseekAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
  ROS_DEBUG("TibiDaboHideandseekAlgNode::odom_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->odom_mutex_.enter();
  if(this->gameStatus!=STATE_FINISHED && !this->ready2Move)
  {
    //std::cout << msg->data << std::endl;
    try
    {
      std::string target_frame = "/map";
      std::string source_frame = msg->header.frame_id; // "/odom"
      ros::Time   target_time  = msg->header.stamp;
      //wait for transform between odom and map
      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 stampedIn;
        stampedIn.header = msg->header;
        stampedIn.pose   = msg->pose.pose;
        geometry_msgs::PoseStamped stampedOut;
        //transform odom pose from odom to map
        tf_listener_.transformPose( target_frame, stampedIn, stampedOut);
        this->seekerPose = stampedOut;
        this->seekerPoseReceived = true;
      }
    }
    catch (tf::TransformException &ex)
    {
      ROS_INFO("TibiDaboHideandseekAlgNode:: %s",ex.what());
    }
  }
  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->odom_mutex_.exit(); 
}

/*  [service callbacks] */

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

  //copy & work with requested result 
  this->moveBaseActive=false;
  ROS_INFO("TibiDaboHideandseekAlgNode::move_baseDone: PRESS WII BUTTON 2 TO CONTINUE!"); 
} 

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

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

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

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

void TibiDaboHideandseekAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  ROS_DEBUG("TibiDaboHideandseekAlgNode::config_update!");
  //this->variable = config.parameter;
  this->alg_.unlock();
}

void TibiDaboHideandseekAlgNode::addNodeDiagnostics(void)
{
}

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