world2yaml.cpp
Go to the documentation of this file.
00001 #include <tinyxml.h>
00002 #include <fstream>
00003 #include <vector>
00004 #include <boost/algorithm/string.hpp>
00005 #include <tf/transform_broadcaster.h>
00006 #include <sstream>
00007 
00008 // tutorial demo program
00009 //#include <stdafx.h>
00010 
00011 
00012 // ----------------------------------------------------------------------
00013 // STDOUT dump and indenting utility functions
00014 // ----------------------------------------------------------------------
00015 const unsigned int NUM_INDENTS_PER_SPACE=2;
00016 std::ofstream ofs("test.txt");
00017 
00018 const char * getIndent( unsigned int numIndents )
00019 {
00020   static const char * pINDENT="                                      + ";
00021   static const unsigned int LENGTH=strlen( pINDENT );
00022   unsigned int n=numIndents*NUM_INDENTS_PER_SPACE;
00023   if ( n > LENGTH ) n = LENGTH;
00024 
00025   return &pINDENT[ LENGTH-n ];
00026 }
00027 
00028 // same as getIndent but no "+" at the end
00029 const char * getIndentAlt( unsigned int numIndents )
00030 {
00031   static const char * pINDENT="                                        ";
00032   static const unsigned int LENGTH=strlen( pINDENT );
00033   unsigned int n=numIndents*NUM_INDENTS_PER_SPACE;
00034   if ( n > LENGTH ) n = LENGTH;
00035 
00036   return &pINDENT[ LENGTH-n ];
00037 }
00038 
00039 int dump_attribs_to_stdout(TiXmlElement* pElement, unsigned int indent)
00040 {
00041   if ( !pElement ) return 0;
00042 
00043   TiXmlAttribute* pAttrib=pElement->FirstAttribute();
00044   int i=0;
00045   int ival;
00046   double dval;
00047   const char* pIndent=getIndent(indent);
00048   printf("\n");
00049   while (pAttrib)
00050     {
00051       printf( "%s%s: value=[%s]", pIndent, pAttrib->Name(), pAttrib->Value());
00052 
00053       if (pAttrib->QueryIntValue(&ival)==TIXML_SUCCESS)    printf( " int=%d", ival);
00054       if (pAttrib->QueryDoubleValue(&dval)==TIXML_SUCCESS) printf( " d=%1.1f", dval);
00055       printf( "\n" );
00056       i++;
00057       pAttrib=pAttrib->Next();
00058     }
00059   return i;
00060 }
00061 
00062 void dump_include_model( TiXmlNode* pParent, unsigned int indent = 0 ){
00063   TiXmlNode* pChild;
00064   TiXmlText* pText;
00065   int t = pParent->Type();
00066   std::cout << "-" << std::endl;
00067   for ( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) 
00068     {
00069       TiXmlNode* pChild2;
00070       std::string type = pChild->Value();
00071       //std::cout << pChild->Value() << std::endl;
00072 
00073       if(type == "uri"){
00074 
00075         //std::cout << "uuuuuuuuuuurrrrrrrrrrrrrrrriiiiiiiiiiiiI" << std::endl;
00076         std::cout << getIndentAlt(indent) << "model: \"" << pChild->FirstChild()->Value() << "\"" << std::endl;
00077       }else if(type == "name"){
00078         //std::cout << "namerrrrrrrrrrrriiiiiiiiiiiiI" << std::endl;
00079         std::cout << getIndentAlt(indent) << "name: \"" << pChild->FirstChild()->Value() << "\"" << std::endl;
00080       }else if(type == "pose"){
00081         //      btQuaternion qua(
00082         std::string child_value = pChild->FirstChild()->Value();
00083         std::vector<std::string> v;
00084         boost::algorithm::split( v, child_value, boost::algorithm::is_space() );
00085         //std::cout << getIndentAlt(indent) << "pose: " << pChild->FirstChild()->Value() << std::endl;
00086         std::cout << getIndentAlt(indent) << "pose:" << std::endl;
00087         std::cout << getIndentAlt(indent + 1) << "position:" << std::endl;
00088         std::cout << getIndentAlt(indent + 2) << "x: " << v[0] << std::endl;
00089         std::cout << getIndentAlt(indent + 2) << "y: " << v[1] << std::endl;
00090         std::cout << getIndentAlt(indent + 2) << "z: " << v[2] << std::endl;
00091         
00092         std::string s = v[3];
00093         //std::istringstream r_str(s.c_str());
00094         std::istringstream r_str(v[3]);
00095         int r,p,y;
00096         r_str >> r;
00097         std::istringstream p_str(v[4]);
00098         p_str >> p;
00099         std::istringstream y_str(v[5]);
00100         y_str >> y;
00101 
00102         //      istr << v[4];
00103         //istr >> p;
00104         //istr << v[5];
00105         //istr >> y;
00106 
00107         //btQuaternion qua(y,p,r);
00108         //tf::Quaternion qua(y,p,r);
00109         tf::Matrix3x3 mat;
00110         mat.setRPY(r,p,y);
00111         tf::Quaternion q;
00112         mat.getRotation(q);
00113         std::cout << getIndentAlt(indent + 1) << "orientation:" << std::endl;
00114         //double x = q.x();
00115         std::cout << getIndentAlt(indent + 2) << "x: " << q.x() << std::endl;
00116         std::cout << getIndentAlt(indent + 2) << "y: " << q.y() << std::endl;
00117         std::cout << getIndentAlt(indent + 2) << "z: " << q.z() << std::endl;
00118         std::cout << getIndentAlt(indent + 2) << "w: " << q.w() << std::endl;
00119         //std::cout << getIndentAlt(indent + 2) << "y: " << q.y << std::endl;
00120         //std::cout << getIndentAlt(indent + 2) << "z: " << q.z << std::endl;
00121 
00122         
00123         /*
00124         for(int i=0; i< 3; i++){
00125           std::cout << v[i] << "," << std::endl;
00126           }*/
00127         //std::cout << "poserrrrrrrrrrrriiiiiiiiiiiiI" << std::endl;
00128 
00129       }
00130     }
00131   std::cout << getIndentAlt(indent) << "frmae-id: \"map\"" << std::endl;
00132   std::cout << getIndentAlt(indent) << "robot: false" << std::endl;
00133 }
00134 
00135 
00136 void dump_to_stdout( TiXmlNode* pParent, unsigned int indent = 0 )
00137 {
00138   if ( !pParent ) return;
00139 
00140   TiXmlNode* pChild;
00141   TiXmlText* pText;
00142   int t = pParent->Type();
00143   //printf( "%s", getIndent(indent));
00144   int num;
00145   //std::cout << "aaaaa"  << std::endl;
00146   //std::cout << pParent->Value() << std::endl;
00147   std::string value = pParent->Value();
00148   if ( value == "include"){
00149     //std::cout << "aaaaaaaaaaaaa" << std::endl;
00150     dump_include_model(pParent, 1);
00151   }
00152 
00153   /*
00154   switch ( t )
00155     {
00156     case TiXmlNode::TINYXML_DOCUMENT:
00157       printf( "Document" );
00158       break;
00159 
00160     case TiXmlNode::TINYXML_ELEMENT:
00161       printf( "Element [%s]", pParent->Value() );
00162       num=dump_attribs_to_stdout(pParent->ToElement(), indent+1);
00163       switch(num)
00164         {
00165         case 0:  printf( " (No attributes)"); break;
00166         case 1:  printf( "%s1 attribute", getIndentAlt(indent)); break;
00167         default: printf( "%s%d attributes", getIndentAlt(indent), num); break;
00168         }
00169       break;
00170 
00171     case TiXmlNode::TINYXML_COMMENT:
00172       printf( "Comment: [%s]", pParent->Value());
00173       break;
00174 
00175     case TiXmlNode::TINYXML_UNKNOWN:
00176       printf( "Unknown" );
00177       break;
00178 
00179     case TiXmlNode::TINYXML_TEXT:
00180       pText = pParent->ToText();
00181       printf( "Text: [%s]", pText->Value() );
00182       break;
00183 
00184     case TiXmlNode::TINYXML_DECLARATION:
00185       printf( "Declaration" );
00186       break;
00187     default:
00188       break;
00189     }
00190   printf( "\n" );
00191   */
00192   for ( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) 
00193     {
00194       dump_to_stdout( pChild, indent+1 );
00195     }
00196 }
00197 
00198 void dump_to_stdout(const char* pFilename)
00199 {
00200   TiXmlDocument doc(pFilename);
00201   bool loadOkay = doc.LoadFile();
00202   if (loadOkay)
00203     {
00204       printf("\n%s:\n", pFilename);
00205       dump_to_stdout( &doc ); // defined later in the tutorial
00206     }
00207   else
00208     {
00209       printf("Failed to load file \"%s\"\n", pFilename);
00210     }
00211 }
00212 
00213 
00214 
00215 int main(int argc, char** argv){
00216 
00217   //ofs<<"aa"<<std::endl;
00218   dump_to_stdout("vrc_final_task1.world");
00219 
00220   return 0;
00221 
00222 
00223 }
00224 


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31