Add overload of buildModel and buildGeom

* in order to build model from XML string (standard in ROS)

src/parsers/urdf.hpp

@@ -24,8 +24,6 @@
 #include "pinocchio/multibody/geometry.hpp"
 #include "pinocchio/parsers/urdf/types.hpp"
 
-#include <urdf_model/model.h>
-
 namespace se3
 {
   namespace urdf
@@ -59,6 +57,37 @@ namespace se3
                         Model & model,
                         const bool verbose = false) throw (std::invalid_argument);
 
+    ///
+    /// \brief Build the model from a URDF model with a particular joint as root of the model tree inside
+    /// the model given as reference argument.
+    ///
+    /// \param[in] urdfTree the tree build from the URDF
+    /// \param[in] rootJoint The joint at the root of the model tree.
+    /// \param[in] verbose Print parsing info.
+    /// \param[out] model Reference model where to put the parsed information.
+    /// \return Return the reference on argument model for convenience.
+    ///
+    /// \note urdfTree can be build from ::urdf::parseURDF
+    ///       or ::urdf::parseURDFFile
+    Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                        const JointModelVariant & rootJoint,
+                        Model & model,
+                        const bool verbose = false);
+
+    ///
+    /// \brief Build the model from a URDF model
+    ///
+    /// \param[in] urdfTree the tree build from the URDF
+    /// \param[in] verbose Print parsing info.
+    /// \param[out] model Reference model where to put the parsed information.
+    /// \return Return the reference on argument model for convenience.
+    ///
+    /// \note urdfTree can be build from ::urdf::parseURDF
+    ///       or ::urdf::parseURDFFile
+    Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                        Model & model,
+                        const bool verbose = false);
+
 
     /**
      * @brief      Build The GeometryModel from a URDF file. Search for meshes
@@ -87,6 +116,33 @@ namespace se3
                              const std::vector<std::string> & packageDirs = std::vector<std::string> ())
     throw (std::invalid_argument);
 
+    /**
+     * @brief      Build The GeometryModel from a URDF model. Search for meshes
+     *             in the directories specified by the user first and then in
+     *             the environment variable ROS_PACKAGE_PATH
+     *
+     * @param[in]  model         The model of the robot, built with
+     *                           urdf::buildModel
+     * @param[in]  urdfTree      The URDF model
+     * @param[in]  packageDirs  A vector containing the different directories
+     *                           where to search for models and meshes, typically
+     *                           obtained from calling se3::rosPaths()
+     *
+     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
+     * @param[out]  geomModel    Reference where to put the parsed information.
+     *
+     * @return      Returns the reference on geom model for convenience.
+     *
+     * \warning     If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
+     *
+     */
+    GeometryModel& buildGeom(const Model & model,
+                             const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                             const GeometryType type,
+                             GeometryModel & geomModel,
+                             const std::vector<std::string> & packageDirs = std::vector<std::string> ())
+    throw (std::invalid_argument);
+
 
   } // namespace urdf
 } // namespace se3

src/parsers/urdf/geometry.cpp

@@ -343,6 +343,17 @@ namespace se3
                              GeometryModel & geomModel,
                              const std::vector<std::string> & package_dirs)
       throw(std::invalid_argument)
+    {
+      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
+      return buildGeom (model, urdfTree, type, geomModel, package_dirs);
+    }
+
+    GeometryModel& buildGeom(const Model & model,
+                             const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                             const GeometryType type,
+                             GeometryModel & geomModel,
+                             const std::vector<std::string> & package_dirs)
+      throw(std::invalid_argument)
     {
       std::vector<std::string> hint_directories(package_dirs);
 
@@ -355,7 +366,6 @@ namespace se3
         throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
       }
 
-      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
       details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
       return geomModel;
     }

src/parsers/urdf/model.cpp

@@ -587,8 +587,7 @@ namespace se3
 
       if (urdfTree)
 	{
-	  model.name = urdfTree->getName();
-	  ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
+          return buildModel (urdfTree, root_joint, model, verbose);
 	}
       else
       {
@@ -604,8 +603,7 @@ namespace se3
       ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
       if (urdfTree)
 	{
-	  model.name = urdfTree->getName();
-	  details::parseRootTree(urdfTree->getRoot(),model,verbose);
+          return buildModel (urdfTree, model, verbose);
 	}
       else
       {
@@ -616,5 +614,26 @@ namespace se3
       return model;
     }
 
+    Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                      const JointModelVariant & root_joint,
+                      Model & model,
+                      const bool verbose)
+    {
+      assert(urdfTree);
+      model.name = urdfTree->getName();
+      ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
+      return model;
+    }
+                
+    Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                      Model& model,
+                      const bool verbose)
+    {
+      assert(urdfTree);
+      model.name = urdfTree->getName();
+      details::parseRootTree(urdfTree->getRoot(),model,verbose);
+      return model;
+    }
+
   } // namespace urdf
 } // namespace se3

src/parsers/urdf/types.hpp

@@ -87,6 +87,10 @@ namespace urdf
 
 #undef URDF_TYPEDEF_CLASS_POINTER
 
-#endif
+#else // URDFDOM_TYPEDEF_SHARED_PTR
+
+#include <urdf_world/types.h>
+
+#endif // URDFDOM_TYPEDEF_SHARED_PTR
 
 #endif // ifndef __se3_parsers_urdf_types_hpp__