From 14f6db14859bfc5f430cf854bd4c77d2977126ab Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume@dexory.com>
Date: Tue, 16 Jan 2024 09:35:02 +0000
Subject: [PATCH] Quaternion in urdf (PR123 new attempt)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
---
 package.xml              |  2 +-
 urdf_parser/src/pose.cpp | 18 ++++++++++++++++++
 xsd/urdf.xsd             |  1 +
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/package.xml b/package.xml
index e2455b46..81199d1c 100644
--- a/package.xml
+++ b/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>
 
diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp
index ce6c92b2..f8cbbfb9 100644
--- a/urdf_parser/src/pose.cpp
+++ b/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;
 }
diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd
index ccebfb26..853fed5f 100644
--- a/xsd/urdf.xsd
+++ b/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 -->