$search
00001 #include <iostream> 00002 #include <fstream> 00003 00004 #include <rcpdf/parser.hh> 00005 00006 using namespace rcpdf; 00007 using namespace rcpdf_interface; 00008 00009 void displayPose(const Pose& pose) 00010 { 00011 double roll = 0.; 00012 double pitch = 0.; 00013 double yaw = 0.; 00014 00015 pose.rotation.getRPY(roll, pitch, yaw); 00016 00017 std::cout << pose.position.x << " " 00018 << pose.position.y << " " 00019 << pose.position.z << " / " 00020 << roll << " " 00021 << pitch << " " 00022 << yaw << std::endl; 00023 } 00024 00025 void displayLimit(boost::shared_ptr<Limit> limit) 00026 { 00027 if (!limit) 00028 { 00029 std::cout << " - Limit: none" << std::endl; 00030 return; 00031 } 00032 std::cout << " - Limit: " << std::endl 00033 << " - Normal force: " 00034 << limit->normal_force_ << std::endl; 00035 } 00036 00037 void displayGeometry(boost::shared_ptr<Geometry> geometry) 00038 { 00039 if (!geometry) 00040 { 00041 std::cout << " - Geometry: none" << std::endl; 00042 return; 00043 } 00044 00045 if (geometry->type == Geometry::SPHERE) 00046 std::cout << "sphere" << std::endl; 00047 else if (geometry->type == Geometry::BOX) 00048 std::cout << "box" << std::endl; 00049 else if (geometry->type == Geometry::CYLINDER) 00050 std::cout << "cylinder" << std::endl; 00051 else if (geometry->type == Geometry::MESH) 00052 std::cout << "mesh" << std::endl; 00053 else 00054 std::cout << "unkown" << std::endl; 00055 } 00056 00057 void displayContact(boost::shared_ptr<Contact> contact) 00058 { 00059 if (!contact) 00060 { 00061 std::cout << " - null pointer in contact list" << std::endl; 00062 return; 00063 } 00064 std::cout << " - Name: " << contact->name_ << std::endl; 00065 std::cout << " - Link: " << contact->link_ << std::endl; 00066 std::cout << " - Origin: "; 00067 displayPose(contact->origin_); 00068 std::cout << " - Geometry: "; 00069 displayGeometry(contact->geometry_); 00070 displayLimit(contact->limit_); 00071 } 00072 00073 void display(boost::shared_ptr<ModelInterface> model) 00074 { 00075 if (!model) 00076 { 00077 std::cerr << "model pointer is null" << std::endl; 00078 return; 00079 } 00080 std::cout << "Name: " << model->name_ << std::endl; 00081 00082 for (std::vector<boost::shared_ptr<Contact> >::const_iterator 00083 it = model->contacts_.begin(); 00084 it != model->contacts_.end(); ++it) 00085 displayContact(*it); 00086 } 00087 00088 int main(int argc, char** argv) 00089 { 00090 if (argc < 2) 00091 { 00092 std::cerr << "Expect RCPDF xml file to parse" << std::endl; 00093 return -1; 00094 } 00095 00096 std::string xml_string; 00097 std::fstream xml_file (argv[1], std::fstream::in); 00098 while ( xml_file.good() ) 00099 { 00100 std::string line; 00101 std::getline (xml_file, line); 00102 xml_string += (line + "\n"); 00103 } 00104 xml_file.close(); 00105 00106 boost::shared_ptr<ModelInterface> model = parseRCPDF(xml_string); 00107 if (!model) 00108 { 00109 std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; 00110 return -1; 00111 } 00112 std::cout << "---------- Successfully Parsed XML ---------------" 00113 << std::endl; 00114 display(model); 00115 return 0; 00116 }