Quaternion in urdf (PR123 new attempt)

Signed-off-by: Guillaume Doisy <[email protected]>

package.xml

@@ -27,7 +27,7 @@
   <exec_depend>libconsole-bridge-dev</exec_depend>
   <exec_depend>tinyxml2</exec_depend>
   <exec_depend>tinyxml2_vendor</exec_depend>
-  <exec_depend version_gte="0.2.3">urdfdom_headers</exec_depend>
+  <exec_depend version_gte="1.2.0">urdfdom_headers</exec_depend>
 
   <test_depend>python3</test_depend>
 

urdf_parser/src/pose.cpp

@@ -107,6 +107,13 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml)
     }
 
     const char* rpy_str = xml->Attribute("rpy");
+    const char* quat_str = xml->Attribute("quat_xyzw");
+    if (rpy_str != NULL && quat_str != NULL)
+    {
+      CONSOLE_BRIDGE_logError("Both rpy and quat_xyzw orientations are defined. Use either one or the other.");
+      return false;
+    }
+
     if (rpy_str != NULL)
     {
       try {
@@ -117,6 +124,17 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml)
         return false;
       }
     }
+
+    if (quat_str != NULL)
+    {
+      try {
+        pose.rotation.initQuaternion(quat_str);
+      }
+      catch (ParseError &e) {
+        CONSOLE_BRIDGE_logError(e.what());
+        return false;
+      }
+    }
   }
   return true;
 }

xsd/urdf.xsd

@@ -17,6 +17,7 @@
   <xs:complexType name="pose">
     <xs:attribute name="xyz" type="xs:string" default="0 0 0" />
     <xs:attribute name="rpy" type="xs:string" default="0 0 0" />
+    <xs:attribute name="quat_xyzw" type="xs:string" default="0 0 0 1" />
   </xs:complexType>
 
   <!-- pose node type -->