tibi_dabo_hideandseek_alg.cpp

Go to the documentation of this file.

#include "tibi_dabo_hideandseek_alg.h"

TibiDaboHideandseekAlgorithm::TibiDaboHideandseekAlgorithm(void)
{
  ROS_INFO("TibiDaboHideandseekAlgorithm::constructor!");
  //this->mySeekerHS = new SeekerHS();
  this->path="nopath";
  this->pathOut="nopathout";
  this->map="nomap";
  this->experiment ="experiment1";
  this->solver=1;
  this->windist=1;
  this->cellsize=1;
}

TibiDaboHideandseekAlgorithm::~TibiDaboHideandseekAlgorithm(void)
{
}

void TibiDaboHideandseekAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  ROS_DEBUG("TibiDaboHideandseekAlgorithm::config_update!");

  // save the current configuration
  this->config_=new_cfg;
  this->path       = new_cfg.path;
  this->pathOut    = new_cfg.pathOut;
  this->map        = new_cfg.map;
  this->experiment = new_cfg.experiment;
  this->solver     = new_cfg.solver;
  this->windist    = new_cfg.windist;
  this->setWinDistToHider(this->windist);
  this->cellsize   = new_cfg.cellsize;
  this->setCellSizeM(this->cellsize);

  this->unlock();
}

// TibiDaboHideandseekAlgorithm Public API

void TibiDaboHideandseekAlgorithm::initialize(std::string path, std::string pathOut, std::string map, std::string experiment, int solver)
{
  ROS_INFO("TibiDaboHideandseekAlgorithm::initialize!");
  this->path       = path;
  this->pathOut    = pathOut;
  this->map        = map;
  this->experiment = experiment;
  this->solver     = solver;

  std::string mapFile     = this->path + "/maps/"  + this->map + ".txt";
  std::string pomdpFile   = this->path + "/pomdpx/simplerew/"+ this->map + ".pomdpx";
  std::string policyFile  = this->path + "/policy/simplerew/"+ this->map + ".pol";

  std::string logFile     = this->pathOut + "/" + this->experiment+"_log.txt";
  std::string timeLogFile = this->pathOut + "/" + this->experiment+"_timeLog.txt";
  std::string gameLogFile = this->pathOut + "/" + this->experiment+"_gameLog.txt";

  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: experiment: %s", this->experiment.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: solver: %i", this->solver);
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: INPUT FILES");
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: map   : %s", mapFile.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: pomdp : %s", pomdpFile.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: policy: %s", policyFile.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: OUTPUT FILES");
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: log    : %s", logFile.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: timelog: %s", timeLogFile.c_str());
  ROS_INFO(" TibiDaboHideandseekAlgorithm::initialize: gamelog: %s", gameLogFile.c_str());
  
  SeekerHSParams* params = new SeekerHSParams();

  //parameters for seeker
  params->mapFile     = mapFile;
  params->expName     = this->experiment;
  params->solverType  = this->solver; //use SeekerHSParams::SOLVER_*
  //in case of pomdp
  params->pomdpFile   = pomdpFile;
  //for offline pomdp
  params->policyFile  = policyFile;
  //log files
  params->logFile     = logFile;
  params->timeLogFile = timeLogFile;
  params->gameLogFile = gameLogFile;
  //online pomdp params
  params->segmenterType = SeekerHSParams::SEGMENTER_ROBOT_CENTERED;
  params->rcSegmDist    = 10;
  params->rcAngDist     = 45;
  params->rcBaseRad     = 0;
  params->rcHighResR    = 0;
  //on-line params, set top reward and final state
  params->setFinalStateOnTop = true;
  params->setFinalTopRewards = true;

  //to make faster (but worse results)
  params->ubInitType = SeekerHSParams::UPPERBOUND_INIT_MDP;
  params->timeoutSeconds = 10;

  params->allowInconsistObs=true;  

  this->mySeekerHS = new SeekerHS(params);

  //this->mySeekerHS = new SeekerHS(mapFile, this->experiment, this->solver, pomdpFile, policyFile, logFile, timeLogFile, gameLogFile);
  ROS_INFO("TibiDaboHideandseekAlgorithm::initialize done!");
}

bool TibiDaboHideandseekAlgorithm::initGame(const geometry_msgs::PoseStamped seekerPose, 
                                               const std::vector < geometry_msgs::Point > hiderPoses)
{
  std::vector<double> seekerPosVector;

  seekerPosVector.push_back(seekerPose.pose.position.x);
  seekerPosVector.push_back(seekerPose.pose.position.y);

  std::vector < std::vector<double> > hiderPositionsVector;
  std::vector<double> hiderPosition;
  for(unsigned int i=0; i<hiderPoses.size(); i++)
  {
    hiderPosition.clear();
    hiderPosition.push_back(hiderPoses[i].x);
    hiderPosition.push_back(hiderPoses[i].y);
    hiderPosition.push_back(hiderPoses[i].z);
    hiderPositionsVector.push_back(hiderPosition);
  }

  try
  {
    this->mySeekerHS->initMultipleObs(seekerPosVector, hiderPositionsVector);
    return true;
  }
  catch (CException & e)
  {
    ROS_ERROR("Exception: %s", e.what().c_str());
    ROS_DEBUG("TibiDaboHideAndSeekAlgorithm::initGame: exception!");
    return false;
  }
}

bool TibiDaboHideandseekAlgorithm::iteration(const geometry_msgs::PoseStamped seekerPose, 
                                              const std::vector < geometry_msgs::Point > hiderPoses, 
                                              geometry_msgs::PoseStamped & seekerGoal, 
                                              int & gameStatus)
{
  std::vector<double> newSeekerPosVector;  
  int winState;

  std::vector<double> seekerPosVector;
  seekerPosVector.push_back(seekerPose.pose.position.x);
  seekerPosVector.push_back(seekerPose.pose.position.y);

  std::vector < std::vector<double> > hiderPositionsVector;
  std::vector<double> hiderPosition;

  for(unsigned int i=0; i<hiderPoses.size(); i++)
  {
    hiderPosition.clear();
    hiderPosition.push_back(hiderPoses[i].x);
    hiderPosition.push_back(hiderPoses[i].y);
    hiderPosition.push_back(hiderPoses[i].z);
    hiderPositionsVector.push_back(hiderPosition);
  }

  try
  {
    this->mySeekerHS->getNextMultiplePosesForMultipleObs(seekerPosVector, hiderPositionsVector, 1, newSeekerPosVector, &winState);
    seekerGoal.pose.position.x = newSeekerPosVector[0];
    seekerGoal.pose.position.y = newSeekerPosVector[1];
    double yaw = newSeekerPosVector[2];
    tf::quaternionTFToMsg (tf::createQuaternionFromRPY(0.0,0.0,yaw),  seekerGoal.pose.orientation);
    gameStatus=winState;
    return true;
  }
  catch (CException & e)
  {
    ROS_ERROR("Exception: %s", e.what().c_str());
    ROS_DEBUG("TibiDaboHideandseekAlgorithm::iteration: done!");
    return false;
  }

}

void TibiDaboHideandseekAlgorithm::setWinDistToHider(int winDist)
{
  this->mySeekerHS->setWinDistToHider(winDist);
}

void TibiDaboHideandseekAlgorithm::setCellSizeM(double cellSizeM)
{
  this->mySeekerHS->setCellSizeM(cellSizeM);
}

void TibiDaboHideandseekAlgorithm::stopGame(int winState)
{
  this->mySeekerHS->stopGame(winState);
}