Go to the documentation of this file.00001 #include "tibi_dabo_hri_teleop_alg.h"
00002 #include "xml/hri_cfg_file.hxx"
00003 #include <sys/types.h>
00004 #include <sys/stat.h>
00005 #include <unistd.h>
00006 #include <dirent.h>
00007 #include <fstream>
00008 #include <stdlib.h>
00009
00010 std::string motion_path="/motion/";
00011 std::string speech_path="/speech/";
00012
00013 TibiDaboHriTeleopAlgorithm::TibiDaboHriTeleopAlgorithm(void)
00014 {
00015 this->desired_home_interval_=3;
00016 this->desired_speech_interval=1;
00017 this->desired_repeat_interval_=3;
00018
00019
00020 this->xml_path = "./";
00021
00022 srand(time(NULL));
00023 }
00024
00025 void TibiDaboHriTeleopAlgorithm::scan_XML_files(void)
00026 {
00027 std::string full_path,full_speech_path;
00028 std::fstream speech_file;
00029 char *text_data=NULL;
00030 struct dirent *dp;
00031 struct stat st;
00032 DIR *dir;
00033
00034
00035 full_path=this->xml_path + speech_path;
00036 std::cout << "--- Full_path: " << full_path << std::endl;
00037 if(stat(full_path.c_str(),&st)==0)
00038 {
00039 this->speech_files.clear();
00040 this->speech_filenames.clear();
00041 dir=opendir(full_path.c_str());
00042 while((dp=readdir(dir)) != NULL)
00043 {
00044
00045 if(strstr(dp->d_name,".txt")!=NULL)
00046 {
00047
00048 this->speech_filenames.push_back(dp->d_name);
00049
00050 full_speech_path=full_path + dp->d_name;
00051 speech_file.open(full_speech_path.c_str(),std::fstream::in);
00052 stat(dp->d_name,&st);
00053 text_data=new char[st.st_size];
00054 speech_file.get(text_data,st.st_size);
00055 this->speech_files.push_back(text_data);
00056 delete[] text_data;
00057 speech_file.close();
00058 }
00059 }
00060 closedir(dir);
00061 std::cout << "--- Found " << this->speech_filenames.size() << " speech files" << std::endl;
00062 for(unsigned int i=0; i<this->speech_filenames.size(); i++)
00063 {
00064 std::cout << "--- " << this->speech_filenames[i].c_str() << ": " << this->speech_files[i].c_str() << std::endl;
00065 }
00066 }
00067 else
00068 {
00069 std::cout << "--- The folder does not exist." << std::endl;
00070 }
00071
00072
00073 full_path=this->xml_path + motion_path;
00074 std::cout << "--- Full_path: " << full_path << std::endl;
00075 if(stat(full_path.c_str(),&st)==0)
00076 {
00077 this->motion_seq_files_.clear();
00078 dir=opendir(full_path.c_str());
00079 while((dp=readdir(dir)) != NULL)
00080 {
00081 if(strstr(dp->d_name,".xml")!=NULL)
00082 this->motion_seq_files_.push_back(full_path + dp->d_name);
00083 }
00084 closedir(dir);
00085 std::cout << "--- Found " << this->motion_seq_files_.size() << " motion sequence files" << std::endl;
00086 for(unsigned int i=0; i<this->motion_seq_files_.size(); i++)
00087 {
00088 std::cout << "--- " << this->motion_seq_files_[i].c_str() << std::endl;
00089 }
00090 }
00091 else
00092 {
00093 std::cout << "--- The folder does not exist." << std::endl;
00094 }
00095
00096 }
00097
00098
00099 TibiDaboHriTeleopAlgorithm::~TibiDaboHriTeleopAlgorithm(void)
00100 {
00101 }
00102
00103 void TibiDaboHriTeleopAlgorithm::config_update(Config& new_cfg, uint32_t level)
00104 {
00105 this->lock();
00106 ROS_INFO("TibiDaboHriTeleopAlgorithm: Dynamic reconfigure");
00107
00108 if(this->xml_path!=new_cfg.path)
00109 {
00110 this->xml_path=new_cfg.path;
00111 this->scan_XML_files();
00112 ROS_INFO("Scaning XML files (new path)");
00113 }
00114 if(new_cfg.refresh)
00115 {
00116 ROS_INFO("Scaning XML files (refresh)");
00117 this->scan_XML_files();
00118 }
00119
00120 this->config_=new_cfg;
00121 this->unlock();
00122 }
00123
00124
00125 void TibiDaboHriTeleopAlgorithm::load_goal(std::string &filename,std::vector<std::string> &xml_files)
00126 {
00127 try
00128 {
00129 std::auto_ptr<hri_config_t> cfg(hri_config(filename,xml_schema::flags::dont_validate));
00130 xml_files.clear();
00131 xml_files.resize(NUM_CLIENTS);
00132 xml_files[LEDS_] =cfg->face_expression();
00133 xml_files[HEAD_] =cfg->head_seq();
00134 xml_files[LEFT_ARM_] =cfg->left_arm_seq();
00135 xml_files[RIGHT_ARM_] =cfg->right_arm_seq();
00136
00137 for(unsigned int i=0; i<this->speech_filenames.size(); i++)
00138 {
00139 if(this->speech_filenames[i] == cfg->tts_text())
00140 {
00141 xml_files[TTS_]=this->speech_files[i];
00142 break;
00143 }
00144 }
00145 }
00146 catch(const xml_schema::exception& e)
00147 {
00148 std::ostringstream os;
00149 os << e;
00150
00151 std::cout << os.str() << std::endl;
00152 throw;
00153 }
00154 }
00155