tibi_dabo_hri_teleop_alg.cpp

Go to the documentation of this file.

#include "tibi_dabo_hri_teleop_alg.h"
#include "xml/hri_cfg_file.hxx"
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <dirent.h>
#include <fstream>
#include <stdlib.h>

std::string motion_path="/motion/";
std::string speech_path="/speech/";

TibiDaboHriTeleopAlgorithm::TibiDaboHriTeleopAlgorithm(void)
{  
  this->desired_home_interval_=3;
  this->desired_speech_interval=1;
  this->desired_repeat_interval_=3;
  
  // list all the existing XML files
  this->xml_path = "./";
  
  srand(time(NULL));
}

void TibiDaboHriTeleopAlgorithm::scan_XML_files(void)
{
  std::string full_path,full_speech_path;
  std::fstream speech_file;
  char *text_data=NULL;
  struct dirent *dp;
  struct stat st;
  DIR *dir;

  // check wether the config XML files folder exists or not
  full_path=this->xml_path + speech_path;
  std::cout << "--- Full_path: " << full_path << std::endl;
  if(stat(full_path.c_str(),&st)==0)// the folder exists
  {
    this->speech_files.clear();
        this->speech_filenames.clear();
    dir=opendir(full_path.c_str());
    while((dp=readdir(dir)) != NULL)
    {
        
      if(strstr(dp->d_name,".txt")!=NULL)
      {
        // save the filename
        this->speech_filenames.push_back(dp->d_name);
        // open the file
        full_speech_path=full_path + dp->d_name;
        speech_file.open(full_speech_path.c_str(),std::fstream::in);
        stat(dp->d_name,&st);// get information about the file 
        text_data=new char[st.st_size];
        speech_file.get(text_data,st.st_size);
        this->speech_files.push_back(text_data);
        delete[] text_data;
        speech_file.close();
      }
    }
    closedir(dir);
    std::cout << "--- Found " << this->speech_filenames.size() << " speech files" << std::endl;
    for(unsigned int i=0; i<this->speech_filenames.size(); i++)
    {
      std::cout << "---    " << this->speech_filenames[i].c_str() << ": " << this->speech_files[i].c_str() << std::endl;
    }
  }
  else
  {
    std::cout << "--- The folder does not exist." << std::endl;
  }
  

  full_path=this->xml_path + motion_path;
  std::cout << "--- Full_path: " << full_path << std::endl;
  if(stat(full_path.c_str(),&st)==0)// the folder exists
  {
    this->motion_seq_files_.clear();
    dir=opendir(full_path.c_str());
    while((dp=readdir(dir)) != NULL)
    {
      if(strstr(dp->d_name,".xml")!=NULL)
        this->motion_seq_files_.push_back(full_path + dp->d_name);
    }
    closedir(dir);
    std::cout << "--- Found " << this->motion_seq_files_.size() << " motion sequence files" << std::endl;
    for(unsigned int i=0; i<this->motion_seq_files_.size(); i++)
    {
      std::cout << "---    " << this->motion_seq_files_[i].c_str() << std::endl;
    }
  }
  else
  {
    std::cout << "--- The folder does not exist." << std::endl;
  }
  
}


TibiDaboHriTeleopAlgorithm::~TibiDaboHriTeleopAlgorithm(void)
{
}

void TibiDaboHriTeleopAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  ROS_INFO("TibiDaboHriTeleopAlgorithm: Dynamic reconfigure");
  // save the current configuration
  if(this->xml_path!=new_cfg.path)
  {
    this->xml_path=new_cfg.path;
    this->scan_XML_files();
    ROS_INFO("Scaning XML files (new path)");
  }
  if(new_cfg.refresh)
  {
    ROS_INFO("Scaning XML files (refresh)");
    this->scan_XML_files();
  }
  
  this->config_=new_cfg;  
  this->unlock();
}

// TibiDaboHriTeleopAlgorithm Public API
void TibiDaboHriTeleopAlgorithm::load_goal(std::string &filename,std::vector<std::string> &xml_files)
{
  try
  {
    std::auto_ptr<hri_config_t> cfg(hri_config(filename,xml_schema::flags::dont_validate));
    xml_files.clear();
    xml_files.resize(NUM_CLIENTS);
    xml_files[LEDS_]      =cfg->face_expression();
    xml_files[HEAD_]      =cfg->head_seq();
    xml_files[LEFT_ARM_]  =cfg->left_arm_seq();
    xml_files[RIGHT_ARM_] =cfg->right_arm_seq();

        for(unsigned int i=0; i<this->speech_filenames.size(); i++)
        { 
          if(this->speech_filenames[i] == cfg->tts_text())
          {
                xml_files[TTS_]=this->speech_files[i];
                break;
          }       
        }
  }
  catch(const xml_schema::exception& e)
  {
    std::ostringstream os;
    os << e;
    /* handle exceptions */
    std::cout << os.str() << std::endl;
    throw;
  }
}