function to load parameters for each model from xml file

PX4 · May 30, 2018 · b4b7245 · b4b7245
1 parent 651ca35
commit b4b7245
Showing 1 changed file with 31 additions and 0 deletions.
diff --git a/include/common.h b/include/common.h
@@ -21,6 +21,7 @@
  */
 
 
+#include <tinyxml.h>
 #include <Eigen/Dense>
 #include <gazebo/gazebo.hh>
 #include <ignition/math.hh>
@@ -50,6 +51,36 @@ bool getSdfParam(sdf::ElementPtr sdf, const std::string& name, T& param, const T
   return false;
 }
 
+template <typename T>
+void model_param(const std::string& world_name, const std::string& model_name, const std::string& param, T& param_value)
+{
+  TiXmlDocument doc(world_name + ".xml");
+  if (doc.LoadFile())
+  {
+    TiXmlHandle hDoc(&doc);
+
+    TiXmlElement* pElem=hDoc.FirstChild( "options" ).FirstChild("model").Element();
+    for( pElem; pElem; pElem=pElem->NextSiblingElement("model"))
+    {
+      TiXmlElement* pName = pElem->FirstChildElement("name");
+      if (pName && model_name.compare(pName->GetText()) == 0)
+      {
+        TiXmlElement* pValue = pElem->FirstChildElement(param);
+
+        if (pValue)
+        {
+          std::istringstream iss(pValue->GetText());
+          iss >> param_value;
+
+          gzdbg << "get " << param << " " << param_value <<" for " << model_name << " model from " << doc.Value() << "\n";
+          break;
+        }
+      }
+    }
+  }
+
+}
+
 /**
  * \brief Get a math::Angle as an angle from [0, 360)
  */