stopgap measure for handling use of 'package:/' in URDFs for #271

dart/dynamics/MeshShape.cpp

@@ -46,6 +46,7 @@
 #include <assimp/cimport.h>
 
 #include "dart/renderer/RenderInterface.h"
+#include "dart/common/Console.h"
 
 namespace dart {
 namespace dynamics {
@@ -181,6 +182,9 @@ const aiScene* MeshShape::loadMesh(const std::string& _fileName) {
                                    aiProcess_SortByPType            |
                                    aiProcess_OptimizeMeshes,
                                    NULL, propertyStore);
+  if(!scene)
+    dtwarn << "[MeshShape] Assimp could not load file: '" << _fileName << "'. "
+           << "This will likely result in a segmentation fault." << std::endl;
   aiReleasePropertyStore(propertyStore);
 
   // Assimp rotates collada files such that the up-axis (specified in the

dart/utils/urdf/DartLoader.cpp

@@ -93,6 +93,41 @@ simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {
     return world;
 }
 
+/**
+ * @function setPackageDirectory
+ */
+void DartLoader::setPackageDirectory(const std::string &_urdfPackageDirectory)
+{
+  mPackageDirectory = _urdfPackageDirectory;
+}
+
+/**
+ * @function getFullFilePath
+ */
+std::string DartLoader::getFullFilePath(const std::string &_filename,
+                                        const std::string& _rootToSkelPath) const
+{
+  std::string fullpath = _filename;
+  size_t p = fullpath.find("package:/");
+  if(p < std::string::npos)
+  {
+    if(mPackageDirectory.empty())
+      dtwarn << "[DartLoader] Trying to load a URDF that uses the 'package://' keyword, "
+                "but you did not set a package directory using "
+                "dart::utils::DartLoader::setPackageDirectory(~). "
+                "This will likely result in a segmentation fault." << std::endl;
+
+    fullpath.erase(p, p+9);
+    fullpath.insert(p, mPackageDirectory);
+  }
+  else
+  {
+    fullpath = _rootToSkelPath + fullpath;
+  }
+
+  return fullpath;
+}
+
 /**
  * @function parseWorldToEntityPaths
  */
@@ -355,7 +390,7 @@ dynamics::Shape* DartLoader::createShape(const VisualOrCollision* _vizOrCol, std
 
   // Mesh
   else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get())) {
-    std::string fullPath = _rootToSkelPath + mesh->filename;
+    std::string fullPath = getFullFilePath(mesh->filename, _rootToSkelPath);
     const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );
 
     if(!model) {

dart/utils/urdf/DartLoader.h

@@ -40,8 +40,10 @@ class DartLoader {
 public:
     dynamics::Skeleton* parseSkeleton(std::string _urdfFileName);
     simulation::World* parseWorld(std::string _urdfFileName);
+    void setPackageDirectory(const std::string& _urdfPackageDirectory);
 
 private:
+    std::string getFullFilePath(const std::string& _filename, const std::string& _rootToSkelPath) const;
     void parseWorldToEntityPaths(const std::string &_xml_string);
 
     dynamics::Skeleton* modelInterfaceToSkeleton(const urdf::ModelInterface* _model, std::string _rootToSkelPath = "");
@@ -58,6 +60,8 @@ class DartLoader {
     std::string readFileToString(std::string _xmlFile);
 
     std::map<std::string, std::string> mWorld_To_Entity_Paths;
+
+    std::string mPackageDirectory;
 };
 
 }