Merge pull request #439 from mkoval/bugfix/DartLoaderSEGFAULT

Fixed SEGFAULTs in DartLoader

dart/dynamics/MeshShape.cpp

@@ -273,9 +273,11 @@ const aiScene* MeshShape::loadMesh(const std::string& _fileName) {
                                    aiProcess_OptimizeMeshes,
                                    nullptr, propertyStore);
   if(!scene)
-    dtwarn << "[MeshShape] Assimp could not load file: '" << _fileName << "'. "
-           << "This will likely result in a segmentation fault "
-           << "if you attempt to use the nullptr we return." << std::endl;
+  {
+    dtwarn << "[MeshShape::loadMesh] Assimp could not load file: '"
+           << _fileName << "'.\n";
+    return nullptr;
+  }
   aiReleasePropertyStore(propertyStore);
 
   // Assimp rotates collada files such that the up-axis (specified in the

dart/utils/urdf/DartLoader.cpp

@@ -174,12 +174,13 @@ std::string DartLoader::getFullFilePath(const std::string& _filename) const
 
     if(packageDirectory == mPackageDirectories.end())
     {
-      dtwarn << "[DartLoader] Trying to load a URDF that uses package '"
-             << fullpath.substr(scheme, authority_end-scheme)
-             << "' (the full line is '" << fullpath
-             << "'), but we do not know the path to that package directory. "
-             << "Please use addPackageDirectory(~) to allow us to find the "
-             << "package directory.\n";
+      dterr << "[DartLoader] Trying to load a URDF that uses package '"
+            << fullpath.substr(scheme, authority_end-scheme)
+            << "' (the full line is '" << fullpath
+            << "'), but we do not know the path to that package directory. "
+            << "Please use addPackageDirectory(~) to allow us to find the "
+            << "package directory.\n";
+      fullpath = "";
     }
     else
     {
@@ -279,8 +280,10 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte
     else
     {
       root = root->child_links[0].get();
-      dynamics::BodyNode::Properties rootProperties =
-          createDartNodeProperties(root);
+      dynamics::BodyNode::Properties rootProperties;
+      if (!createDartNodeProperties(root, rootProperties))
+        return nullptr;
+
       rootNode = createDartJointAndNode(
             root->parent_joint.get(), rootProperties, nullptr, skeleton);
       if(nullptr == rootNode)
@@ -292,8 +295,10 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte
   }
   else
   {
-    dynamics::BodyNode::Properties rootProperties =
-        createDartNodeProperties(root);
+    dynamics::BodyNode::Properties rootProperties;
+    if (!createDartNodeProperties(root, rootProperties))
+      return nullptr;
+
     std::pair<dynamics::Joint*, dynamics::BodyNode*> pair =
         skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
           nullptr, dynamics::FreeJoint::Properties(
@@ -305,25 +310,34 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte
 
   for(size_t i = 0; i < root->child_links.size(); i++)
   {
-    createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode);
+    if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode))
+      return nullptr;
+
   }
 
   return skeleton;
 }
 
-void DartLoader::createSkeletonRecursive(
+bool DartLoader::createSkeletonRecursive(
     dynamics::SkeletonPtr _skel,
     const urdf::Link* _lk,
     dynamics::BodyNode* _parentNode)
 {
-  dynamics::BodyNode::Properties properties = createDartNodeProperties(_lk);
+  dynamics::BodyNode::Properties properties;
+  if (!createDartNodeProperties(_lk, properties))
+    return false;
+
   dynamics::BodyNode* node = createDartJointAndNode(
         _lk->parent_joint.get(), properties, _parentNode, _skel);
+  if(!node)
+    return false;
 
-  for(unsigned int i = 0; i < _lk->child_links.size(); ++i)
+  for(size_t i = 0; i < _lk->child_links.size(); ++i)
   {
-      createSkeletonRecursive(_skel, _lk->child_links[i].get(), node);
+      if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node))
+        return false;
   }
+  return true;
 }
 
 
@@ -442,7 +456,6 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode(
     {
       dterr << "[DartLoader::createDartJoint] Unsupported joint type ("
             << _jt->type << ")\n";
-      assert(false);
       return nullptr;
     }
   }
@@ -453,10 +466,10 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode(
 /**
  * @function createDartNode
  */
-dynamics::BodyNode::Properties DartLoader::createDartNodeProperties(
-    const urdf::Link* _lk)
+bool DartLoader::createDartNodeProperties(
+    const urdf::Link* _lk, dynamics::BodyNode::Properties &node)
 {
-  dynamics::BodyNode::Properties node(_lk->name);
+  node.mName = _lk->name;
 
   // Load Inertial information
   if(_lk->inertial) {
@@ -477,22 +490,23 @@ dynamics::BodyNode::Properties DartLoader::createDartNodeProperties(
   }
 
   // Set visual information
-  for(unsigned int i = 0; i < _lk->visual_array.size(); i++)
+  for(size_t i = 0; i < _lk->visual_array.size(); i++)
   {
     if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get()))
-    {
       node.mVizShapes.push_back(shape);
-    }
+    else
+      return false;
   }
 
   // Set collision information
-  for(unsigned int i = 0; i < _lk->collision_array.size(); i++) {
-    if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) {
+  for(size_t i = 0; i < _lk->collision_array.size(); i++) {
+    if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get()))
       node.mColShapes.push_back(shape);
-    }
+    else
+      return false;
   }
 
-  return node;
+  return true;
 }
 
 
@@ -535,26 +549,26 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol)
   else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get()))
   {
     std::string fullPath = getFullFilePath(mesh->filename);
-    const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );
-
-    if(!model)
-    {
-      dtwarn << "[DartLoader::createShape] Assimp could not load a model from "
-             << "the file '" << fullPath << "'\n";
-      shape = nullptr;
-    }
-    else
+    if (fullPath.empty())
     {
-      shape = dynamics::ShapePtr(new dynamics::MeshShape(
-          Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model, fullPath));
+      dtwarn << "[DartLoader::createShape] Skipping URDF mesh with empty"
+                " filename. We are returning a nullptr.\n";
+      return nullptr;
     }
+
+    const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );
+    if(!model)
+      return nullptr; // MeshShape::loadMesh logs a warning
+
+    shape = dynamics::ShapePtr(new dynamics::MeshShape(
+      Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model, fullPath));
   }
   // Unknown geometry type
   else
   {
-    dtwarn << "[DartLoader::createShape] Unknown urdf Shape type "
-           << "(we only know of Sphere, Box, Cylinder, and Mesh). "
-           << "We are returning a nullptr." << std::endl;
+    dtwarn << "[DartLoader::createShape] Unknown URDF shape type"
+              " (we only know of Sphere, Box, Cylinder, and Mesh)."
+              " We are returning a nullptr.\n";
     return nullptr;
   }
 

dart/utils/urdf/DartLoader.h

@@ -92,7 +92,7 @@ class DartLoader {
     void parseWorldToEntityPaths(const std::string& _xml_string);
 
     dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model);
-    void createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent);
+    bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent);
 
     template <class VisualOrCollision>
     dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol);
@@ -103,8 +103,8 @@ class DartLoader {
         dynamics::BodyNode* _parent,
         dynamics::SkeletonPtr _skeleton);
 
-    dynamics::BodyNode::Properties createDartNodeProperties(
-        const urdf::Link* _lk);
+    bool createDartNodeProperties(
+        const urdf::Link* _lk, dynamics::BodyNode::Properties &properties);
 
     Eigen::Isometry3d toEigen(const urdf::Pose& _pose);
     Eigen::Vector3d toEigen(const urdf::Vector3& _vector);

data/urdf/test/invalid.urdf

@@ -0,0 +1 @@
+This is not a valid URDF file.

data/urdf/test/invalid_mesh.stl

@@ -0,0 +1 @@
+This is not a valid STL file.

data/urdf/test/invalid_mesh.urdf

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot name="invalid_mesh">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="invalid_mesh.stl"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>

data/urdf/test/missing_mesh.urdf

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot name="multipleshapes">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="this_file_does_not_exist.stl"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>

data/urdf/test/missing_package.urdf

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot name="missing_package">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="package://missing_package/foo.stl"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>

data/urdf/test/primitive_geometry.urdf

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot name="primitive_geometry">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <sphere radius="1"/>
+        <cylinder radius="1" length="1"/>
+        <box size="1 1 1"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>

unittests/testDartLoader.cpp

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2015, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Michael Koval <[email protected]>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <[email protected]> <[email protected]>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   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.
+ *   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 HOLDER 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.
+ */
+
+#include <iostream>
+#include <gtest/gtest.h>
+#include "dart/utils/urdf/DartLoader.h"
+
+using dart::utils::DartLoader;
+
+TEST(DartLoader, parseSkeleton_NonExistantPathReturnsNull)
+{
+  DartLoader loader;
+  EXPECT_EQ(nullptr,
+    loader.parseSkeleton(DART_DATA_PATH"skel/test/does_not_exist.urdf"));
+}
+
+TEST(DartLoader, parseSkeleton_InvalidUrdfReturnsNull)
+{
+  DartLoader loader;
+  EXPECT_EQ(nullptr,
+    loader.parseSkeleton(DART_DATA_PATH"urdf/test/invalid.urdf"));
+}
+
+TEST(DartLoader, parseSkeleton_MissingMeshReturnsNull)
+{
+  DartLoader loader;
+  EXPECT_EQ(nullptr,
+    loader.parseSkeleton(DART_DATA_PATH"urdf/test/missing_mesh.urdf"));
+}
+
+TEST(DartLoader, parseSkeleton_InvalidMeshReturnsNull)
+{
+  DartLoader loader;
+  EXPECT_EQ(nullptr,
+    loader.parseSkeleton(DART_DATA_PATH"urdf/test/invalid_mesh.urdf"));
+}
+
+TEST(DartLoader, parseSkeleton_MissingPackageReturnsNull)
+{
+  DartLoader loader;
+  EXPECT_EQ(nullptr,
+    loader.parseSkeleton(DART_DATA_PATH"urdf/test/missing_package.urdf"));
+}
+
+TEST(DartLoader, parseSkeleton_LoadsPrimitiveGeometry)
+{
+  DartLoader loader;
+  EXPECT_TRUE(nullptr !=
+    loader.parseSkeleton(DART_DATA_PATH"urdf/test/primitive_geometry.urdf"));
+}
+
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}