diff --git a/package.xml b/package.xml
index e2455b46..81199d1c 100644
--- a/package.xml
+++ b/package.xml
@@ -27,7 +27,7 @@
libconsole-bridge-dev
tinyxml2
tinyxml2_vendor
- urdfdom_headers
+ urdfdom_headers
python3
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 @@
+