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 @@ +