Add quaternion in URDF attributes

CMakeLists.txt

@@ -2,8 +2,8 @@ cmake_minimum_required( VERSION 2.8 FATAL_ERROR )
 project (urdfdom CXX C)
 
 set (URDF_MAJOR_VERSION 1)
-set (URDF_MINOR_VERSION 0)
-set (URDF_PATCH_VERSION 3)
+set (URDF_MINOR_VERSION 1)
+set (URDF_PATCH_VERSION 0)
 
 set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION})
 set (URDF_MAJOR_MINOR_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION})
@@ -45,7 +45,7 @@ find_package(console_bridge 0.3 REQUIRED)
 include_directories(SYSTEM ${console_bridge_INCLUDE_DIRS})
 link_directories(${console_bridge_LIBRARY_DIRS})
 
-#In Visual Studio a special postfix for 
+#In Visual Studio a special postfix for
 #libraries compiled in debug is used
 if(MSVC)
 set(CMAKE_DEBUG_POSTFIX "d")

urdf_parser/src/pose.cpp

@@ -1,13 +1,13 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
-* 
+*
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
-* 
+*
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
-* 
+*
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
@@ -17,7 +17,7 @@
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
-* 
+*
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -105,6 +105,13 @@ bool parsePose(Pose &pose, TiXmlElement* 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 {
@@ -115,6 +122,17 @@ bool parsePose(Pose &pose, TiXmlElement* 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 -->
@@ -318,7 +319,7 @@
       <xs:attribute name="name" type="xs:string" use="required" />
 
       <!-- TM: I suggest adding the following attribute. -->
-      <xs:attribute name="version" type="xs:string" default="1.0" />
+      <xs:attribute name="version" type="xs:string" default="1.1.0" />
     </xs:complexType>
   </xs:element>
 </xs:schema>