refactoring sensor parsing

urdf_parser/CMakeLists.txt

@@ -58,7 +58,8 @@ add_urdfdom_library(
   LIBNAME
     urdfdom_sensor
   SOURCES
-    src/urdf_sensor.cpp
+    src/sensor_parser.cpp
+    src/visual_sensor_parsers.cpp
   LINK
     urdfdom_model)
 

urdf_parser/include/urdf_parser/joint.h

@@ -0,0 +1,49 @@
+/*********************************************************************
+* 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
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * 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
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef URDF_PARSER_JOINT_H
+#define URDF_PARSER_JOINT_H
+
+#include <urdf_model/joint.h>
+#include <tinyxml.h>
+
+namespace urdf {
+
+bool parseJoint(Joint &joint, TiXmlElement *config);
+
+bool exportJoint(Joint &joint, TiXmlElement *config);
+
+}
+
+#endif

urdf_parser/include/urdf_parser/link.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+* 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
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * 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
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef URDF_PARSER_LINK_H
+#define URDF_PARSER_LINK_H
+
+#include <urdf_model/link.h>
+#include <tinyxml.h>
+#include "exportdecl.h"
+
+namespace urdf {
+
+URDFDOM_DLLAPI bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok);
+
+URDFDOM_DLLAPI bool parseSphere(Sphere &s, TiXmlElement *c);
+URDFDOM_DLLAPI bool parseBox(Box &b, TiXmlElement *c);
+URDFDOM_DLLAPI bool parseCylinder(Cylinder &y, TiXmlElement *c);
+URDFDOM_DLLAPI bool parseMesh(Mesh &m, TiXmlElement *c);
+URDFDOM_DLLAPI GeometrySharedPtr parseGeometry(TiXmlElement *g);
+
+URDFDOM_DLLAPI bool parseInertial(Inertial &i, TiXmlElement *config);
+URDFDOM_DLLAPI bool parseVisual(Visual &vis, TiXmlElement *config);
+URDFDOM_DLLAPI bool parseCollision(Collision &col, TiXmlElement* config);
+
+URDFDOM_DLLAPI bool parseLink(Link &link, TiXmlElement *config);
+
+
+URDFDOM_DLLAPI bool exportMaterial(Material &material, TiXmlElement *config);
+
+URDFDOM_DLLAPI bool exportSphere(Sphere &s, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportBox(Box &b, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportCylinder(Cylinder &y, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportMesh(Mesh &m, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml);
+
+URDFDOM_DLLAPI bool exportInertial(Inertial &i, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportVisual(Visual &vis, TiXmlElement *xml);
+URDFDOM_DLLAPI bool exportCollision(Collision &col, TiXmlElement* xml);
+
+URDFDOM_DLLAPI bool exportLink(Link &link, TiXmlElement *config);
+
+}
+
+#endif

urdf_parser/include/urdf_parser/pose.h

@@ -0,0 +1,60 @@
+/*********************************************************************
+* 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
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * 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
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef URDF_PARSER_POSE_H
+#define URDF_PARSER_POSE_H
+
+#include <urdf_model/pose.h>
+#include <urdf_model/color.h>
+#include <tinyxml.h>
+#include "exportdecl.h"
+
+namespace urdf_export_helpers {
+
+URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL);
+URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec);
+URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot);
+URDFDOM_DLLAPI std::string values2str(urdf::Color c);
+URDFDOM_DLLAPI std::string values2str(double d);
+
+}
+
+namespace urdf {
+
+URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
+URDFDOM_DLLAPI bool exportPose(Pose &pose, TiXmlElement* xml);
+
+}
+
+#endif

urdf_parser/include/urdf_parser/sensor_parser.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Bielefeld University
+*  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
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Bielefeld University 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
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Robert Haschke */
+
+#ifndef URDF_PARSER_URDF_SENSOR_PARSER_H
+#define URDF_PARSER_URDF_SENSOR_PARSER_H
+
+#include <string>
+#include <map>
+#include <tinyxml.h>
+#include <urdf_sensor/types.h>
+
+#include "exportdecl.h"
+
+namespace urdf {
+
+  /// API for any custom SensorParser
+  class URDFDOM_DLLAPI SensorParser {
+  public:
+    virtual ~SensorParser() = default;
+    virtual SensorBase* parse(TiXmlElement &sensor_element) = 0;
+  };
+  URDF_TYPEDEF_CLASS_POINTER(SensorParser);
+
+  /// map from sensor name to Sensor instance
+  using SensorMap = std::map<std::string, SensorSharedPtr>;
+  /// map from sensor type to SensorParser instance
+  using SensorParserMap = std::map<std::string, SensorParserSharedPtr>;
+
+  /** parse <sensor> tags in URDF document for which a parser exists in SensorParserMap */
+  URDFDOM_DLLAPI SensorMap parseSensors(TiXmlDocument &urdf,  const SensorParserMap &parsers);
+
+  /** convienency function to fetch a sensor with given name and type from the map */
+  template <typename T>
+  URDFDOM_DLLAPI std::shared_ptr<T> getSensor(const std::string &name, const SensorMap &sensors) {
+    SensorMap::const_iterator s = sensors.find(name);
+    if (s == sensors.end()) return std::shared_ptr<T>();
+    else return std::dynamic_pointer_cast<T>(s->second->sensor_);
+  }
+
+}
+
+#endif

urdf_parser/include/urdf_parser/urdf_parser.h

@@ -46,19 +46,13 @@
 #include <urdf_model/color.h>
 #include <urdf_model/utils.h>
 #include <urdf_model_state/model_state.h>
-#include <urdf_sensor/sensor.h>
 #include <urdf_world/types.h>
 
 #include "exportdecl.h"
+#include "pose.h"
 
 namespace urdf_export_helpers {
 
-URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL);
-URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec);
-URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot);
-URDFDOM_DLLAPI std::string values2str(urdf::Color c);
-URDFDOM_DLLAPI std::string values2str(double d);
-
 // This lives here (rather than in model.cpp) so we can run tests on it.
 class URDFVersion final
 {
@@ -145,10 +139,6 @@ namespace urdf{
   URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path);
   URDFDOM_DLLAPI TiXmlDocument*  exportURDF(ModelInterfaceSharedPtr &model);
   URDFDOM_DLLAPI TiXmlDocument*  exportURDF(const ModelInterface &model);
-  URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
-  URDFDOM_DLLAPI bool parseCamera(Camera&, TiXmlElement*);
-  URDFDOM_DLLAPI bool parseRay(Ray&, TiXmlElement*);
-  URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*);
   URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*);
 }
 

urdf_parser/include/urdf_parser/utils.h

@@ -0,0 +1,88 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, CITEC, Bielefeld University
+*  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
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the copyright holder 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
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Robert Haschke */
+
+#ifndef URDF_PARSER_UTIL_H
+#define URDF_PARSER_UTIL_H
+
+#include <urdf_exception/exception.h>
+#include <urdf_model/utils.h>
+
+namespace urdf {
+
+template <typename T>
+T parseAttribute(const char* value);
+
+template<>
+inline std::string parseAttribute<std::string>(const char* value)
+{
+  return value;
+}
+
+template<>
+inline double parseAttribute<double>(const char* value)
+{
+  return strToDouble(value);
+}
+
+template<>
+inline unsigned int parseAttribute<unsigned int>(const char* value)
+{
+  return std::stoul(value);
+}
+
+template <typename T>
+T parseAttribute(const TiXmlElement &tag, const char* attr, const T* default_value=NULL)
+{
+  const char* value = tag.Attribute(attr);
+  if (!value)
+  {
+    if (default_value) return *default_value;
+    else throw ParseError(std::string("missing '") + attr + "' attribute");
+  }
+
+  try
+  {
+    return parseAttribute<T>(value);
+  }
+  catch (const std::exception &e)
+  {
+    throw ParseError(std::string("failed to parse '") + attr + "' attribute: " + e.what());
+  }
+}
+
+}
+
+#endif