From fbeb9039e897ec7e98890b2b3df945bdd554f04f Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 17:14:56 -0500 Subject: [PATCH 01/55] Added the ResourceRetriever concept. --- dart/dynamics/MeshShape.cpp | 99 ++++++++++++++++++++----- dart/dynamics/MeshShape.h | 25 ++++++- dart/utils/LocalResourceRetriever.cpp | 41 ++++++++++ dart/utils/LocalResourceRetriever.h | 56 ++++++++++++++ dart/utils/PackageResourceRetriever.cpp | 80 ++++++++++++++++++++ dart/utils/PackageResourceRetriever.h | 90 ++++++++++++++++++++++ dart/utils/ResourceRetriever.cpp | 44 +++++++++++ dart/utils/ResourceRetriever.h | 81 ++++++++++++++++++++ dart/utils/SchemaResourceRetriever.cpp | 68 +++++++++++++++++ dart/utils/SchemaResourceRetriever.h | 66 +++++++++++++++++ dart/utils/urdf/DartLoader.cpp | 83 ++++++--------------- dart/utils/urdf/DartLoader.h | 17 ++++- 12 files changed, 668 insertions(+), 82 deletions(-) create mode 100644 dart/utils/LocalResourceRetriever.cpp create mode 100644 dart/utils/LocalResourceRetriever.h create mode 100644 dart/utils/PackageResourceRetriever.cpp create mode 100644 dart/utils/PackageResourceRetriever.h create mode 100644 dart/utils/ResourceRetriever.cpp create mode 100644 dart/utils/ResourceRetriever.h create mode 100644 dart/utils/SchemaResourceRetriever.cpp create mode 100644 dart/utils/SchemaResourceRetriever.h diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 666c0c8f17261..c9413e150acaf 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -36,6 +36,7 @@ */ #include "dart/dynamics/MeshShape.h" +#include "dart/utils/LocalResourceRetriever.h" #include #include @@ -129,9 +130,27 @@ aiMaterial::~aiMaterial() namespace dart { namespace dynamics { +static bool startsWith(const std::string &_target, const std::string &_prefix) +{ + return _target.size() >= _prefix.size() + && _target.substr(0, _prefix.size()) == _prefix; +} + +static std::string extractPathFromUri(const std::string &_uri) +{ + static const std::string fileSchema = "file://"; + + if (startsWith(_uri, fileSchema)) + return _uri.substr(fileSchema.size()); + else + return ""; +} + MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, - const std::string &_path) + const std::string &_path, bool _isUri, + const utils::ResourceRetrieverPtr& _resourceRetriever) : Shape(MESH), + mResourceRetriever(_resourceRetriever), mDisplayList(0), mScale(_scale) { @@ -139,7 +158,7 @@ MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, assert(_scale[1] > 0.0); assert(_scale[2] > 0.0); - setMesh(_mesh, _path); + //setMesh(_mesh, _path, _isUri, _resourceRetriever); } MeshShape::~MeshShape() { @@ -150,20 +169,42 @@ const aiScene* MeshShape::getMesh() const { return mMesh; } +const std::string &MeshShape::getMeshUri() const +{ + return mMeshUri; +} + const std::string &MeshShape::getMeshPath() const { return mMeshPath; } -void MeshShape::setMesh(const aiScene* _mesh, const std::string &_path) { +void MeshShape::setMesh( + const aiScene* _mesh, const std::string &_path, bool _isUri, + const utils::ResourceRetrieverPtr& _resourceRetriever) +{ mMesh = _mesh; if(nullptr == _mesh) { mMeshPath = ""; + mMeshUri = ""; + mResourceRetriever = nullptr; return; } - mMeshPath = _path; + if(_isUri) + { + mMeshUri = _path; + mMeshPath = extractPathFromUri(_path); + } + else + { + mMeshUri = "file://" + _path; + mMeshPath = _path; + } + + mResourceRetriever = _resourceRetriever; + _updateBoundingBoxDim(); computeVolume(); } @@ -258,26 +299,36 @@ void MeshShape::_updateBoundingBoxDim() { mBoundingBoxDim[2] = max_Z - min_Z; } -const aiScene* MeshShape::loadMesh(const std::string& _fileName) { +const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size) +{ + // Remove points and lines aiPropertyStore* propertyStore = aiCreatePropertyStore(); - // remove points and lines aiSetImportPropertyInteger(propertyStore, AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_POINT | aiPrimitiveType_LINE); - const aiScene* scene = - aiImportFileExWithProperties(_fileName.c_str(), - aiProcess_GenNormals | - aiProcess_Triangulate | - aiProcess_JoinIdenticalVertices | - aiProcess_SortByPType | - 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; + + const aiScene* scene = aiImportFileFromMemoryWithProperties( + reinterpret_cast(_data), _size, + aiProcess_GenNormals + | aiProcess_Triangulate + | aiProcess_JoinIdenticalVertices + | aiProcess_SortByPType + | aiProcess_OptimizeMeshes, + nullptr, propertyStore + ); + aiReleasePropertyStore(propertyStore); + if(!scene) + { + dterr << "[MeshShape::loadMesh] Failed loading mesh: " + << aiGetErrorString() << "\n"; + return nullptr; + } + + // TODO: Detect if this is a DAE file. If so, apply the same post-processing + // as we do below. +#if 0 // Assimp rotates collada files such that the up-axis (specified in the // collada file) aligns with assimp's y-axis. Here we are reverting this // rotation. We are only catching files with the .dae file ending here. We @@ -288,9 +339,21 @@ const aiScene* MeshShape::loadMesh(const std::string& _fileName) { scene->mRootNode->mTransformation = aiMatrix4x4(); } scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices); +#endif return scene; } +const aiScene* MeshShape::loadMesh(const utils::MemoryResource& _resource) +{ + return loadMesh(_resource.getData(), _resource.getSize()); +} + +const aiScene* MeshShape::loadMesh(const std::string& _fileName) +{ + utils::LocalResourceRetriever retriever; + return loadMesh(*retriever.retrieve("file://" + _fileName)); +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index c8de0bc36f8ac..3ec2a3570cb88 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -43,6 +43,7 @@ #include #include "dart/dynamics/Shape.h" +#include "dart/utils/ResourceRetriever.h" namespace dart { namespace dynamics { @@ -51,8 +52,10 @@ namespace dynamics { class MeshShape : public Shape { public: /// \brief Constructor. - MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, - const std::string &path = std::string()); + MeshShape( + const Eigen::Vector3d& _scale, const aiScene* _mesh, + const std::string &_path = std::string(), bool _isUri = false, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief Destructor. virtual ~MeshShape(); @@ -61,7 +64,13 @@ class MeshShape : public Shape { const aiScene* getMesh() const; /// \brief - void setMesh(const aiScene* _mesh, const std::string &path = std::string()); + void setMesh( + const aiScene* _mesh, + const std::string &path = std::string(), bool _isUri = false, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + + /// \brief URI to the mesh; an empty string if unavailable. + const std::string &getMeshUri() const; /// \brief Path to the mesh on disk; an empty string if unavailable. const std::string &getMeshPath() const; @@ -84,6 +93,8 @@ class MeshShape : public Shape { bool _default = true) const; /// \brief + static const aiScene* loadMesh(const uint8_t* _data, size_t _size); + static const aiScene* loadMesh(const utils::MemoryResource& _resource); static const aiScene* loadMesh(const std::string& _fileName); // Documentation inherited. @@ -101,9 +112,15 @@ class MeshShape : public Shape { /// \brief const aiScene* mMesh; - /// \brief + /// \brief URI the mesh, if available). + std::string mMeshUri; + + /// \brief Path the mesh on disk, if available. std::string mMeshPath; + /// \brief Optional method of loading resources by URI. + utils::ResourceRetrieverPtr mResourceRetriever; + /// \brief OpenGL DisplayList id for rendering int mDisplayList; diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp new file mode 100644 index 0000000000000..949be0a317918 --- /dev/null +++ b/dart/utils/LocalResourceRetriever.cpp @@ -0,0 +1,41 @@ +#include +#include "dart/common/Console.h" +#include "dart/utils/LocalResourceRetriever.h" + +namespace dart { +namespace utils { + +ConstMemoryResourcePtr LocalResourceRetriever::retrieve( + const std::string& _uri) +{ + static const std::string prefix = "file://"; + + // Only file:// URIs are local. + if(_uri.find(prefix) != 0) + return nullptr; + + const std::string localPath = _uri.substr(prefix.size()); + std::ifstream stream(localPath.c_str(), std::ios::binary); + + // Compute the length of the file. + std::streampos size = stream.tellg(); + stream.seekg(0, std::ios::end); + size = stream.tellg() - size; + stream.seekg(0, std::ios::beg); + + // Read the full contents of the file. + const auto resource = std::make_shared( + new uint8_t[size], size, true); + stream.read(reinterpret_cast(resource->getData()), size); + + if(!stream.good()) + { + dtwarn << "[LocalResourceRetriever::retrieve] Failed to read file '" + << localPath << "'.\n"; + return nullptr; + } + return resource; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/LocalResourceRetriever.h b/dart/utils/LocalResourceRetriever.h new file mode 100644 index 0000000000000..6ee1d971ed13e --- /dev/null +++ b/dart/utils/LocalResourceRetriever.h @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ +#define DART_UTILS_LOCALRESOURCERETRIEVER_H_ +#include "dart/utils/ResourceRetriever.h" + +namespace dart { +namespace utils { + +/// \brief Retrieve local resources specified by file:// URIs. +class LocalResourceRetriever : public virtual ResourceRetriever +{ +public: + virtual ~LocalResourceRetriever() = default; + ConstMemoryResourcePtr retrieve(const std::string& _uri) override; +}; + +typedef std::shared_ptr LocalResourceRetrieverPtr; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp new file mode 100644 index 0000000000000..72a50573f65fa --- /dev/null +++ b/dart/utils/PackageResourceRetriever.cpp @@ -0,0 +1,80 @@ +#include +#include +#include "dart/common/Console.h" +#include "dart/utils/LocalResourceRetriever.h" +#include "dart/utils/ResourceRetriever.h" +#include "dart/utils/PackageResourceRetriever.h" + +namespace dart { +namespace utils { + +PackageResourceRetriever::PackageResourceRetriever( + const ResourceRetrieverPtr& localRetriever) +{ + if (localRetriever) + mLocalRetriever = localRetriever; + else + mLocalRetriever = std::make_shared(); +} + +void PackageResourceRetriever::addPackageDirectory( + const std::string& _packageName, const std::string& _packageDirectory) +{ + // Strip a trailing slash. + std::string normalizedPackageDirectory; + if(!_packageDirectory.empty() && _packageDirectory.back() == '/') + normalizedPackageDirectory + = _packageDirectory.substr(0, _packageDirectory.size() - 1); + else + normalizedPackageDirectory = _packageDirectory; + + mPackageMap[_packageName].push_back(normalizedPackageDirectory); +} + +ConstMemoryResourcePtr PackageResourceRetriever::retrieve( + const std::string& _uri) +{ + static const std::string schema = "package://"; + + if (_uri.find(schema) != 0) + return nullptr; + + // Split the URI into package and relative path components. + const size_t packageIndex = schema.size(); + const size_t pathIndex = _uri.find_first_of('/', packageIndex); + + if(pathIndex == std::string::npos) + { + dtwarn << "[PackageResourceRetriever::retrieve] Unable to find package" + " name in URI '" << _uri << "'.\n"; + return nullptr; + } + + assert(pathIndex >= packageIndex); + const std::string packageName = _uri.substr(packageIndex, pathIndex - packageIndex); + const std::string relativePath = _uri.substr(pathIndex); + + // Lookup the corresponding package path. + const auto it = mPackageMap.find(packageName); + if(it == std::end(mPackageMap)) + { + dtwarn << "[PackageResourceResolver::retrieve] Unable to resolve path to" + " package '" << packageName << "' while retrieving '" << _uri + << "'. Did you call addPackageDirectory(~) with this package" + " name?\n"; + return nullptr; + } + + // Try each path, in sequence. + for(const std::string &packagePath : it->second) + { + const std::string localUri = "file://" + packagePath + relativePath; + std::cout << "requesting: " << localUri << std::endl; + if(const auto resource = mLocalRetriever->retrieve(localUri)) + return resource; + } + return nullptr; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h new file mode 100644 index 0000000000000..49fe065822d4a --- /dev/null +++ b/dart/utils/PackageResourceRetriever.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_PACKAGERESOURCERETRIEVER_H_ +#define DART_UTILS_PACKAGERESOURCERETRIEVER_H_ +#include +#include +#include "dart/utils/ResourceRetriever.h" + +namespace dart { +namespace utils { + +/// \brief Retrieve local resources specified by package:// URIs. +class PackageResourceRetriever : public virtual ResourceRetriever +{ +public: + PackageResourceRetriever( + const ResourceRetrieverPtr& localRetriever = nullptr); + + virtual ~PackageResourceRetriever() = default; + + /// Specify the directory of a ROS package. In your URDF files, you may see + /// strings with a package URI pattern such as: + /// + /// @code + /// "package://my_robot/meshes/mesh_for_my_robot.stl" + /// \______/ \______/\___________________________/ + /// | | | + /// package package file path with respect to + /// keyword name the package directory + /// @endcode + /// + /// For us to successfully parse a URDF, we need to be told what the path + /// to the package directory is, using addPackageDirectory(). In this case, + /// suppose the path to the my_robot package is /path/to/my_robot. Then you + /// should use addPackageDirectory("my_robot", "/path/to/my_robot"). + /// Altogether, this implies that a file named + /// "/path/to/my_robot/meshes/mesh_for_my_robot.stl" exists. Whatever you + /// specify as the package directory will end up replacing the 'package + /// keyword' and 'package name' components of the URI string. + /// + /// + void addPackageDirectory(const std::string& _packageName, + const std::string& _packageDirectory); + + virtual ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + +private: + ResourceRetrieverPtr mLocalRetriever; + std::unordered_map > mPackageMap; +}; + +typedef std::shared_ptr PackageResourceRetrieverPtr; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_PACKAGERESOURCERETRIEVER_H_ diff --git a/dart/utils/ResourceRetriever.cpp b/dart/utils/ResourceRetriever.cpp new file mode 100644 index 0000000000000..42bbf909e2af5 --- /dev/null +++ b/dart/utils/ResourceRetriever.cpp @@ -0,0 +1,44 @@ +#include +#include "ResourceRetriever.h" + +namespace dart { +namespace utils { + +MemoryResource::MemoryResource() + : mData(nullptr), + mSize(0), + mIsOwner(false) +{ +} + +MemoryResource::MemoryResource(uint8_t* _data, size_t _size, bool _isOwner) + : mData(_data), + mSize(_size), + mIsOwner(_isOwner) +{ + assert(_size == 0 || _data != nullptr); +} + +MemoryResource::~MemoryResource() +{ + if (mData && mIsOwner) + delete mData; +} + +uint8_t* MemoryResource::getData() +{ + return mData; +} + +const uint8_t* MemoryResource::getData() const +{ + return mData; +} + +size_t MemoryResource::getSize() const +{ + return mSize; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/ResourceRetriever.h b/dart/utils/ResourceRetriever.h new file mode 100644 index 0000000000000..730796c3147cc --- /dev/null +++ b/dart/utils/ResourceRetriever.h @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_RESOURCERETRIEVER_H_ +#define DART_UTILS_RESOURCERETRIEVER_H_ + +#include +#include +#include + +namespace dart { +namespace utils { + +struct MemoryResource { +public: + MemoryResource(); + MemoryResource(uint8_t* _data, size_t _size, bool _isOwner); + MemoryResource(const MemoryResource& _other) = delete; + ~MemoryResource(); + + MemoryResource& operator=(const MemoryResource& _other) = delete; + + uint8_t* getData(); + const uint8_t* getData() const; + + size_t getSize() const; + +private: + uint8_t *mData; + size_t mSize; + bool mIsOwner; +}; + +typedef std::shared_ptr MemoryResourcePtr; +typedef std::shared_ptr ConstMemoryResourcePtr; + + +struct ResourceRetriever { + virtual ~ResourceRetriever() = default; + virtual ConstMemoryResourcePtr retrieve(const std::string &_uri) = 0; +}; + +typedef std::shared_ptr ResourceRetrieverPtr; +typedef std::shared_ptr ConstResourceRetrieverPtr; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp new file mode 100644 index 0000000000000..9fc4c61a70546 --- /dev/null +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -0,0 +1,68 @@ +#include +#include "dart/common/Console.h" +#include "dart/utils/SchemaResourceRetriever.h" + +namespace dart { +namespace utils { + +bool SchemaResourceRetriever::addSchemaRetriever( + const std::string& _schema, const ResourceRetrieverPtr& _resourceRetriever) +{ + if(!_resourceRetriever) + { + dterr << "[SchemaResourceRetriever::addSchemaRetriever] Recieved nullptr" + " ResourceRetriever; skipping this entry.\n"; + return false; + } + + if(_schema.find("://") != std::string::npos) + { + dterr << "[SchemaResourceRetriever::addSchemaRetriever] Schema '" + << _schema << "' contains '://'. Did you mistakenly include the" + " '://' in the input of this function?\n"; + return false; + } + + mResourceRetrievers[_schema].push_back(_resourceRetriever); + return true; +} + +ConstMemoryResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) +{ + const auto schemaIndex = _uri.find("://"); + if(schemaIndex == std::string::npos) + { + dterr << "[SchemaResourceRetriever::retrieve] Failed to extract schema from" + " URI '" << _uri << "'. Is this a valid URI?\n"; + return nullptr; + } + + const std::string schema = _uri.substr(0, schemaIndex); + const auto resourceRetrieversIt = mResourceRetrievers.find(schema); + if (resourceRetrieversIt == std::end(mResourceRetrievers)) + { + dtwarn << "[SchemaResourceRetriever::retrieve] There are no resource" + " retrievers registered for the schema '" << schema << "'" + " that is necessary to retrieve URI '" << _uri << "'.\n"; + return nullptr; + } + + // Sequentially try each ResourceRetriever until one returns success. + const std::vector &retrievers + = resourceRetrieversIt->second; + + for (const ResourceRetrieverPtr resourceRetriever : retrievers) + { + if (ConstMemoryResourcePtr resource = resourceRetriever->retrieve(_uri)) + return resource; + } + + dtwarn << "[SchemaResourceRetriever::retrieve] All ResourceRetrievers" + " registered for schema '" << schema << "'" " failed to retrieve" + " URI '" << _uri << "' (tried " << retrievers.size() << ").\n"; + + return nullptr; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h new file mode 100644 index 0000000000000..06f9362f4a0f1 --- /dev/null +++ b/dart/utils/SchemaResourceRetriever.h @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_SCHEMARESOURCERETRIEVER_H_ +#define DART_UTILS_SCHEMARESOURCERETRIEVER_H_ +#include +#include +#include "dart/utils/ResourceRetriever.h" + +namespace dart { +namespace utils { + +class SchemaResourceRetriever : public virtual ResourceRetriever +{ +public: + virtual ~SchemaResourceRetriever() = default; + + bool addSchemaRetriever( + const std::string& _schema, + const ResourceRetrieverPtr& _resourceRetriever); + + ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + +private: + std::unordered_map > mResourceRetrievers; +}; + +typedef std::shared_ptr SchemaResourceRetrieverPtr; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_SCHEMARESOURCERETRIEVER_H_ diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 0680ddb1f57c5..5a46c4be4573d 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -1,7 +1,3 @@ -/** - * @file DartLoader.cpp - */ - #include "DartLoader.h" #include @@ -30,10 +26,21 @@ namespace dart { namespace utils { +DartLoader::DartLoader() + : mLocalRetriever(new utils::LocalResourceRetriever) + , mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)) + , mRetriever(new utils::SchemaResourceRetriever) +{ + using namespace std::placeholders; + + mRetriever->addSchemaRetriever("file", mLocalRetriever); + mRetriever->addSchemaRetriever("package", mPackageRetriever); +} + void DartLoader::addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory) { - mPackageDirectories[_packageName] = _packageDirectory; + mPackageRetriever->addPackageDirectory(_packageName, _packageDirectory); } dynamics::SkeletonPtr DartLoader::parseSkeleton(const std::string& _urdfFileName) { @@ -155,46 +162,6 @@ simulation::WorldPtr DartLoader::parseWorldString( return world; } -/** - * @function getFullFilePath - */ -std::string DartLoader::getFullFilePath(const std::string& _filename) const -{ - std::string fullpath = _filename; - size_t scheme = fullpath.find("package://"); - if(scheme < std::string::npos) - { - size_t authority_start = scheme+10; - size_t authority_end = fullpath.find("/", scheme+10); - size_t authority_length = authority_end - authority_start; - - std::map::const_iterator packageDirectory = - mPackageDirectories.find( - fullpath.substr(authority_start, authority_length)); - - 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"; - } - else - { - fullpath.erase(scheme, authority_end); - fullpath.insert(scheme, packageDirectory->second); - } - } - else - { - fullpath = mRootToSkelPath + fullpath; - } - - return fullpath; -} - /** * @function parseWorldToEntityPaths */ @@ -534,25 +501,23 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) // Mesh else if(urdf::Mesh* mesh = dynamic_cast(_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 - { - shape = dynamics::ShapePtr(new dynamics::MeshShape( - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model, fullPath)); + const utils::ConstMemoryResourcePtr resource = mRetriever->retrieve(mesh->filename); + if (!resource) { + return nullptr; } + + const aiScene* scene = dynamics::MeshShape::loadMesh(*resource); + if (!scene) + return nullptr; + + const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); + shape = std::make_shared( + scale, scene, mesh->filename, true, mRetriever); } // Unknown geometry type else { - dtwarn << "[DartLoader::createShape] Unknown urdf Shape type " + dtwarn << "[DartLoader::createShape] Unknown URDF Shape type " << "(we only know of Sphere, Box, Cylinder, and Mesh). " << "We are returning a nullptr." << std::endl; return nullptr; diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 670e2009bd3a6..66f837d49e06e 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -10,10 +10,15 @@ #include #include +#include "dart/common/Deprecated.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/simulation/World.h" +#include "dart/utils/ResourceRetriever.h" +#include "dart/utils/LocalResourceRetriever.h" +#include "dart/utils/PackageResourceRetriever.h" +#include "dart/utils/SchemaResourceRetriever.h" namespace urdf { @@ -46,6 +51,8 @@ namespace utils { class DartLoader { public: + /// Constructor with the default ResourceRetriever. + DartLoader(); /// Specify the directory of a ROS package. In your URDF files, you may see /// strings with a package URI pattern such as: @@ -66,6 +73,10 @@ class DartLoader { /// "/path/to/my_robot/meshes/mesh_for_my_robot.stl" exists. Whatever you /// specify as the package directory will end up replacing the 'package /// keyword' and 'package name' components of the URI string. + /// + /// DEPRECATED: This functionality has been moved into the + /// PackageResourceRetrievew class. + DEPRECATED(5.0) void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); @@ -88,7 +99,6 @@ class DartLoader { typedef std::shared_ptr BodyPropPtr; typedef std::shared_ptr JointPropPtr; - std::string getFullFilePath(const std::string& _filename) const; void parseWorldToEntityPaths(const std::string& _xml_string); dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model); @@ -110,11 +120,16 @@ class DartLoader { Eigen::Vector3d toEigen(const urdf::Vector3& _vector); std::string readFileToString(std::string _xmlFile); + ResourceRetrieverPtr mResourceRetriever; std::map mWorld_To_Entity_Paths; std::map mPackageDirectories; std::string mRootToSkelPath; std::string mRootToWorldPath; + + utils::LocalResourceRetrieverPtr mLocalRetriever; + utils::PackageResourceRetrieverPtr mPackageRetriever; + utils::SchemaResourceRetrieverPtr mRetriever; }; } From f2b619b0110d7aeafee4bd4a4cffb41bddc16128 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 17:39:37 -0500 Subject: [PATCH 02/55] Cleanup. --- dart/dynamics/MeshShape.cpp | 9 ++++----- dart/utils/PackageResourceRetriever.cpp | 1 - 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index c9413e150acaf..84f811c35b7a5 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -151,14 +151,14 @@ MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, const utils::ResourceRetrieverPtr& _resourceRetriever) : Shape(MESH), mResourceRetriever(_resourceRetriever), - mDisplayList(0), - mScale(_scale) + mDisplayList(0) { assert(_scale[0] > 0.0); assert(_scale[1] > 0.0); assert(_scale[2] > 0.0); - //setMesh(_mesh, _path, _isUri, _resourceRetriever); + setMesh(_mesh, _path, _isUri, _resourceRetriever); + setScale(_scale); } MeshShape::~MeshShape() { @@ -304,8 +304,7 @@ const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size) // Remove points and lines aiPropertyStore* propertyStore = aiCreatePropertyStore(); aiSetImportPropertyInteger(propertyStore, - AI_CONFIG_PP_SBP_REMOVE, - aiPrimitiveType_POINT | aiPrimitiveType_LINE); + AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_POINT | aiPrimitiveType_LINE); const aiScene* scene = aiImportFileFromMemoryWithProperties( reinterpret_cast(_data), _size, diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 72a50573f65fa..7f4a48b7d8310 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -69,7 +69,6 @@ ConstMemoryResourcePtr PackageResourceRetriever::retrieve( for(const std::string &packagePath : it->second) { const std::string localUri = "file://" + packagePath + relativePath; - std::cout << "requesting: " << localUri << std::endl; if(const auto resource = mLocalRetriever->retrieve(localUri)) return resource; } From f64234ffce2902ff6fdf7ca34bfb24a7d40f12ee Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 18:29:55 -0500 Subject: [PATCH 03/55] Added DAE rotation logic to MeshShape::loadMesh. --- dart/dynamics/MeshShape.cpp | 56 +++++++++++++++++++++++++--------- dart/dynamics/MeshShape.h | 6 ++-- dart/utils/urdf/DartLoader.cpp | 7 +++-- 3 files changed, 50 insertions(+), 19 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 84f811c35b7a5..226c96331e0e1 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -136,6 +136,15 @@ static bool startsWith(const std::string &_target, const std::string &_prefix) && _target.substr(0, _prefix.size()) == _prefix; } +static bool endsWith(const std::string &_target, const std::string &_suffix) +{ + if( _target.size() >= _suffix.size() ) + std::cout << "Testing: " << _target.substr(_target.size() - _suffix.size()) << std::endl; + + return _target.size() >= _suffix.size() + && _target.substr(_target.size() - _suffix.size()) == _suffix; +} + static std::string extractPathFromUri(const std::string &_uri) { static const std::string fileSchema = "file://"; @@ -299,8 +308,25 @@ void MeshShape::_updateBoundingBoxDim() { mBoundingBoxDim[2] = max_Z - min_Z; } -const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size) +const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size, + const std::string& _uri) { + // Extract the file extension. + std::string extension; + const size_t extensionIndex = _uri.find_last_of('.'); + if(extensionIndex != std::string::npos) + extension = _uri.substr(extensionIndex); + + std::transform(std::begin(extension), std::end(extension), + std::begin(extension), ::tolower); + + // Use the file extension as a "format hint" for Assimp. + const char *achFormatHint; + if(!extension.empty()) + achFormatHint = extension.c_str() + 1; // strip the '.' + else + achFormatHint = nullptr; + // Remove points and lines aiPropertyStore* propertyStore = aiCreatePropertyStore(); aiSetImportPropertyInteger(propertyStore, @@ -313,45 +339,47 @@ const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size) | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeMeshes, - nullptr, propertyStore + achFormatHint, propertyStore ); aiReleasePropertyStore(propertyStore); if(!scene) { - dterr << "[MeshShape::loadMesh] Failed loading mesh: " - << aiGetErrorString() << "\n"; + const char* assimpMessage = aiGetErrorString(); + const char* message + = (assimpMessage) ? assimpMessage : "An unknown error has occurred."; + dterr << "[MeshShape::loadMesh] Failed loading mesh: " << message << '\n'; return nullptr; } - // TODO: Detect if this is a DAE file. If so, apply the same post-processing - // as we do below. -#if 0 // Assimp rotates collada files such that the up-axis (specified in the // collada file) aligns with assimp's y-axis. Here we are reverting this // rotation. We are only catching files with the .dae file ending here. We // might miss files with an .xml file ending, which would need to be looked // into to figure out whether they are collada files. - if (_fileName.length() >= 4 - && _fileName.substr(_fileName.length() - 4, 4) == ".dae") { + if(extension == ".dae" || extension == ".zae") scene->mRootNode->mTransformation = aiMatrix4x4(); - } + + // Pre-transform the verticies. Assimp states that this post-processing step + // cannot fail, so we'll assert if it returns nullptr. scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices); -#endif + assert(scene); return scene; } -const aiScene* MeshShape::loadMesh(const utils::MemoryResource& _resource) +const aiScene* MeshShape::loadMesh(const utils::MemoryResource& _resource, + const std::string& _uri) { - return loadMesh(_resource.getData(), _resource.getSize()); + return loadMesh(_resource.getData(), _resource.getSize(), _uri); } const aiScene* MeshShape::loadMesh(const std::string& _fileName) { utils::LocalResourceRetriever retriever; - return loadMesh(*retriever.retrieve("file://" + _fileName)); + const std::string uri = "file://" + _fileName; + return loadMesh(*retriever.retrieve(uri), uri); } } // namespace dynamics diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 3ec2a3570cb88..e1312401afc62 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -93,8 +93,10 @@ class MeshShape : public Shape { bool _default = true) const; /// \brief - static const aiScene* loadMesh(const uint8_t* _data, size_t _size); - static const aiScene* loadMesh(const utils::MemoryResource& _resource); + static const aiScene* loadMesh(const uint8_t* _data, size_t _size, + const std::string& _uri = std::string()); + static const aiScene* loadMesh(const utils::MemoryResource& _resource, + const std::string& _uri = std::string()); static const aiScene* loadMesh(const std::string& _fileName); // Documentation inherited. diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 6d7f1025c3b84..24b68ed61b967 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -514,18 +514,19 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) // Mesh else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { - const utils::ConstMemoryResourcePtr resource = mRetriever->retrieve(mesh->filename); + const std::string& uri = mesh->filename; + const utils::ConstMemoryResourcePtr resource = mRetriever->retrieve(uri); if (!resource) { return nullptr; } - const aiScene* scene = dynamics::MeshShape::loadMesh(*resource); + const aiScene* scene = dynamics::MeshShape::loadMesh(*resource, uri); if (!scene) return nullptr; const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shape = std::make_shared( - scale, scene, mesh->filename, true, mRetriever); + scale, scene, uri, true, mRetriever); } // Unknown geometry type else From e896ccff9d1da3fc96d40ff947691d4f06752ca8 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 20:17:52 -0500 Subject: [PATCH 04/55] Added ResourceRetriever::exists. --- dart/utils/LocalResourceRetriever.cpp | 17 ++++-- dart/utils/LocalResourceRetriever.h | 2 + dart/utils/PackageResourceRetriever.cpp | 78 +++++++++++++++++-------- dart/utils/PackageResourceRetriever.h | 8 ++- dart/utils/ResourceRetriever.h | 7 ++- dart/utils/SchemaResourceRetriever.cpp | 63 +++++++++++++------- dart/utils/SchemaResourceRetriever.h | 5 ++ 7 files changed, 127 insertions(+), 53 deletions(-) diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index 949be0a317918..dbd70c38aa6ee 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -1,20 +1,29 @@ #include +#include #include "dart/common/Console.h" #include "dart/utils/LocalResourceRetriever.h" +static const std::string FILE_SCHEMA = "file://"; + namespace dart { namespace utils { +bool LocalResourceRetriever::exists(const std::string& _uri) +{ + if(_uri.size() >= FILE_SCHEMA.size()) + return boost::filesystem::exists(_uri.substr(FILE_SCHEMA.size())); + else + return false; +} + ConstMemoryResourcePtr LocalResourceRetriever::retrieve( const std::string& _uri) { - static const std::string prefix = "file://"; - // Only file:// URIs are local. - if(_uri.find(prefix) != 0) + if(_uri.find(FILE_SCHEMA) != 0) return nullptr; - const std::string localPath = _uri.substr(prefix.size()); + const std::string localPath = _uri.substr(FILE_SCHEMA.size()); std::ifstream stream(localPath.c_str(), std::ios::binary); // Compute the length of the file. diff --git a/dart/utils/LocalResourceRetriever.h b/dart/utils/LocalResourceRetriever.h index 6ee1d971ed13e..60d5bc71025c8 100644 --- a/dart/utils/LocalResourceRetriever.h +++ b/dart/utils/LocalResourceRetriever.h @@ -45,6 +45,8 @@ class LocalResourceRetriever : public virtual ResourceRetriever { public: virtual ~LocalResourceRetriever() = default; + + bool exists(const std::string& _uri) override; ConstMemoryResourcePtr retrieve(const std::string& _uri) override; }; diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 7f4a48b7d8310..47a9ab8a6d34c 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -31,13 +31,62 @@ void PackageResourceRetriever::addPackageDirectory( mPackageMap[_packageName].push_back(normalizedPackageDirectory); } +bool PackageResourceRetriever::exists(const std::string& _uri) +{ + std::string packageName, relativePath; + if (!resolvePackageUri(_uri, packageName, relativePath)) + return false; + + for(const std::string &packagePath : getPackagePaths(packageName)) + { + if (mLocalRetriever->exists(_uri)) + return true; + } + return false; +} + ConstMemoryResourcePtr PackageResourceRetriever::retrieve( const std::string& _uri) +{ + std::string packageName, relativePath; + if (!resolvePackageUri(_uri, packageName, relativePath)) + return nullptr; + + for(const std::string &packagePath : getPackagePaths(packageName)) + { + const std::string localUri = "file://" + packagePath + relativePath; + if(const auto resource = mLocalRetriever->retrieve(localUri)) + return resource; + } + return nullptr; +} + +const std::vector& PackageResourceRetriever::getPackagePaths( + const std::string& _packageName) +{ + static const std::vector empty_placeholder; + + // Lookup the corresponding package path. + const auto it = mPackageMap.find(_packageName); + if(it != std::end(mPackageMap)) + return it->second; + else + { + dtwarn << "[PackageResourceResolver::retrieve] Unable to resolve path to" + " package '" << _packageName << "'. Did you call" + " addPackageDirectory(~) for this package name?\n"; + return empty_placeholder; + } +} + +bool PackageResourceRetriever::resolvePackageUri( + const std::string& _uri, std::string& _packageName, + std::string& _relativePath) { static const std::string schema = "package://"; if (_uri.find(schema) != 0) - return nullptr; + return false; // Split the URI into package and relative path components. const size_t packageIndex = schema.size(); @@ -47,32 +96,13 @@ ConstMemoryResourcePtr PackageResourceRetriever::retrieve( { dtwarn << "[PackageResourceRetriever::retrieve] Unable to find package" " name in URI '" << _uri << "'.\n"; - return nullptr; + return false; } assert(pathIndex >= packageIndex); - const std::string packageName = _uri.substr(packageIndex, pathIndex - packageIndex); - const std::string relativePath = _uri.substr(pathIndex); - - // Lookup the corresponding package path. - const auto it = mPackageMap.find(packageName); - if(it == std::end(mPackageMap)) - { - dtwarn << "[PackageResourceResolver::retrieve] Unable to resolve path to" - " package '" << packageName << "' while retrieving '" << _uri - << "'. Did you call addPackageDirectory(~) with this package" - " name?\n"; - return nullptr; - } - - // Try each path, in sequence. - for(const std::string &packagePath : it->second) - { - const std::string localUri = "file://" + packagePath + relativePath; - if(const auto resource = mLocalRetriever->retrieve(localUri)) - return resource; - } - return nullptr; + _packageName = _uri.substr(packageIndex, pathIndex - packageIndex); + _relativePath = _uri.substr(pathIndex); + return true; } } // namespace utils diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index 49fe065822d4a..fb7f7bf6216e8 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -75,11 +75,17 @@ class PackageResourceRetriever : public virtual ResourceRetriever void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); - virtual ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + bool exists(const std::string& _uri) override; + ConstMemoryResourcePtr retrieve(const std::string& _uri) override; private: ResourceRetrieverPtr mLocalRetriever; std::unordered_map > mPackageMap; + + const std::vector& getPackagePaths( + const std::string& _packageName); + bool resolvePackageUri(const std::string& _uri, + std::string& _packageName, std::string& _relativePath); }; typedef std::shared_ptr PackageResourceRetrieverPtr; diff --git a/dart/utils/ResourceRetriever.h b/dart/utils/ResourceRetriever.h index 730796c3147cc..39ef30fcdd784 100644 --- a/dart/utils/ResourceRetriever.h +++ b/dart/utils/ResourceRetriever.h @@ -9,8 +9,7 @@ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman * * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or + * 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 @@ -69,7 +68,9 @@ typedef std::shared_ptr ConstMemoryResourcePtr; struct ResourceRetriever { virtual ~ResourceRetriever() = default; - virtual ConstMemoryResourcePtr retrieve(const std::string &_uri) = 0; + + virtual bool exists(const std::string& _uri) = 0; + virtual ConstMemoryResourcePtr retrieve(const std::string& _uri) = 0; }; typedef std::shared_ptr ResourceRetrieverPtr; diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index 9fc4c61a70546..14742ed7bab2f 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -27,41 +27,62 @@ bool SchemaResourceRetriever::addSchemaRetriever( return true; } +bool SchemaResourceRetriever::exists(const std::string& _uri) +{ + for(const ResourceRetrieverPtr& resourceRetriever : getRetrievers(_uri)) + { + if(resourceRetriever->exists(_uri)) + return true; + } + return false; +} + ConstMemoryResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) { - const auto schemaIndex = _uri.find("://"); - if(schemaIndex == std::string::npos) + const std::vector &retrievers = getRetrievers(_uri); + for(const ResourceRetrieverPtr& resourceRetriever : retrievers) { - dterr << "[SchemaResourceRetriever::retrieve] Failed to extract schema from" - " URI '" << _uri << "'. Is this a valid URI?\n"; - return nullptr; + if(ConstMemoryResourcePtr resource = resourceRetriever->retrieve(_uri)) + return resource; } - const std::string schema = _uri.substr(0, schemaIndex); - const auto resourceRetrieversIt = mResourceRetrievers.find(schema); - if (resourceRetrieversIt == std::end(mResourceRetrievers)) + dtwarn << "[SchemaResourceRetriever::retrieve] All ResourceRetrievers" + " registered for this schema failed to retrieve the URI '" + << _uri << "' (tried " << retrievers.size() << ").\n"; + + return nullptr; +} + +const std::vector& SchemaResourceRetriever::getRetrievers( + const std::string& _uri) +{ + static const std::vector empty_placeholder; + + const std::string schema = getSchema(_uri); + + const auto it = mResourceRetrievers.find(schema); + if(it != std::end(mResourceRetrievers)) + return it->second; + else { dtwarn << "[SchemaResourceRetriever::retrieve] There are no resource" " retrievers registered for the schema '" << schema << "'" " that is necessary to retrieve URI '" << _uri << "'.\n"; - return nullptr; + return empty_placeholder; } +} - // Sequentially try each ResourceRetriever until one returns success. - const std::vector &retrievers - = resourceRetrieversIt->second; - - for (const ResourceRetrieverPtr resourceRetriever : retrievers) +std::string SchemaResourceRetriever::getSchema(const std::string& _uri) +{ + const auto schemaIndex = _uri.find("://"); + if(schemaIndex == std::string::npos) { - if (ConstMemoryResourcePtr resource = resourceRetriever->retrieve(_uri)) - return resource; + dterr << "[SchemaResourceRetriever::retrieve] Failed to extract schema from" + " URI '" << _uri << "'. Is this a valid URI?\n"; + return nullptr; } - dtwarn << "[SchemaResourceRetriever::retrieve] All ResourceRetrievers" - " registered for schema '" << schema << "'" " failed to retrieve" - " URI '" << _uri << "' (tried " << retrievers.size() << ").\n"; - - return nullptr; + return _uri.substr(0, schemaIndex); } } // namespace utils diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index 06f9362f4a0f1..5843b821b8d6c 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -51,11 +51,16 @@ class SchemaResourceRetriever : public virtual ResourceRetriever const std::string& _schema, const ResourceRetrieverPtr& _resourceRetriever); + bool exists(const std::string& _uri) override; ConstMemoryResourcePtr retrieve(const std::string& _uri) override; private: std::unordered_map > mResourceRetrievers; + + const std::vector& getRetrievers( + const std::string& _uri); + std::string getSchema(const std::string& _uri); }; typedef std::shared_ptr SchemaResourceRetrieverPtr; From 755c00d9dc5e04530d17846a61f0e2ff371487c1 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 13 Jul 2015 23:22:42 -0500 Subject: [PATCH 05/55] Switched to a stream-based URI. --- dart/dynamics/MeshShape.cpp | 78 ++++---- dart/dynamics/MeshShape.h | 14 +- dart/utils/AssimpInputResourceAdaptor.cpp | 205 ++++++++++++++++++++++ dart/utils/AssimpInputResourceAdaptor.h | 130 ++++++++++++++ dart/utils/LocalResource.cpp | 134 ++++++++++++++ dart/utils/LocalResource.h | 71 ++++++++ dart/utils/LocalResourceRetriever.cpp | 23 +-- dart/utils/LocalResourceRetriever.h | 2 +- dart/utils/PackageResourceRetriever.cpp | 4 +- dart/utils/PackageResourceRetriever.h | 2 +- dart/utils/Resource.h | 71 ++++++++ dart/utils/ResourceRetriever.h | 9 +- dart/utils/SchemaResourceRetriever.cpp | 4 +- dart/utils/SchemaResourceRetriever.h | 2 +- dart/utils/urdf/DartLoader.cpp | 7 +- 15 files changed, 672 insertions(+), 84 deletions(-) create mode 100644 dart/utils/AssimpInputResourceAdaptor.cpp create mode 100644 dart/utils/AssimpInputResourceAdaptor.h create mode 100644 dart/utils/LocalResource.cpp create mode 100644 dart/utils/LocalResource.h create mode 100644 dart/utils/Resource.h diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 226c96331e0e1..1e75887d4ad02 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -48,6 +48,8 @@ #include "dart/renderer/RenderInterface.h" #include "dart/common/Console.h" +#include "dart/utils/AssimpInputResourceAdaptor.h" +#include "dart/utils/LocalResourceRetriever.h" // We define our own constructor for aiScene, because it seems to be missing // from the standard assimp library @@ -308,48 +310,40 @@ void MeshShape::_updateBoundingBoxDim() { mBoundingBoxDim[2] = max_Z - min_Z; } -const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size, - const std::string& _uri) +const aiScene* MeshShape::loadMesh( + const std::string& _uri, const utils::ResourceRetrieverPtr& _retriever) { - // Extract the file extension. - std::string extension; - const size_t extensionIndex = _uri.find_last_of('.'); - if(extensionIndex != std::string::npos) - extension = _uri.substr(extensionIndex); - - std::transform(std::begin(extension), std::end(extension), - std::begin(extension), ::tolower); - - // Use the file extension as a "format hint" for Assimp. - const char *achFormatHint; - if(!extension.empty()) - achFormatHint = extension.c_str() + 1; // strip the '.' - else - achFormatHint = nullptr; - - // Remove points and lines + // Remove points and lines from the import. aiPropertyStore* propertyStore = aiCreatePropertyStore(); aiSetImportPropertyInteger(propertyStore, - AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_POINT | aiPrimitiveType_LINE); + AI_CONFIG_PP_SBP_REMOVE, + aiPrimitiveType_POINT + | aiPrimitiveType_LINE + ); + + // Wrap ResourceRetriever in an IOSystem from Assimp's C++ API. Then wrap + // the IOSystem in an aiFileIO from Assimp's C API. Yes, this API is + // completely ridiculous... + utils::AssimpInputResourceRetrieverAdaptor systemIO(_retriever); + aiFileIO fileIO = utils::createFileIO(&systemIO); - const aiScene* scene = aiImportFileFromMemoryWithProperties( - reinterpret_cast(_data), _size, + // Import the file. + const aiScene* scene = aiImportFileExWithProperties( + _uri.c_str(), aiProcess_GenNormals | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeMeshes, - achFormatHint, propertyStore + &fileIO, + propertyStore ); - aiReleasePropertyStore(propertyStore); - + // If succeeded, store the importer in the scene to keep it alive. This is + // necessary because the importer owns the memory that it allocates. if(!scene) { - const char* assimpMessage = aiGetErrorString(); - const char* message - = (assimpMessage) ? assimpMessage : "An unknown error has occurred."; - dterr << "[MeshShape::loadMesh] Failed loading mesh: " << message << '\n'; + dtwarn << "[MeshShape::loadMesh] Failed loading mesh '" << _uri << "'.\n"; return nullptr; } @@ -358,28 +352,30 @@ const aiScene* MeshShape::loadMesh(const uint8_t* _data, size_t _size, // rotation. We are only catching files with the .dae file ending here. We // might miss files with an .xml file ending, which would need to be looked // into to figure out whether they are collada files. + std::string extension; + const size_t extensionIndex = _uri.find_last_of('.'); + if(extensionIndex != std::string::npos) + extension = _uri.substr(extensionIndex); + + std::transform(std::begin(extension), std::end(extension), + std::begin(extension), ::tolower); + if(extension == ".dae" || extension == ".zae") scene->mRootNode->mTransformation = aiMatrix4x4(); - // Pre-transform the verticies. Assimp states that this post-processing step - // cannot fail, so we'll assert if it returns nullptr. + // Finally, pre-transform the vertices. We can't do this as part of the + // import process, because we may have changed mTransformation above. scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices); - assert(scene); + if(!scene) + dtwarn << "[MeshShape::loadMesh] Failed pre-transforming vertices.\n"; return scene; } -const aiScene* MeshShape::loadMesh(const utils::MemoryResource& _resource, - const std::string& _uri) -{ - return loadMesh(_resource.getData(), _resource.getSize(), _uri); -} - const aiScene* MeshShape::loadMesh(const std::string& _fileName) { - utils::LocalResourceRetriever retriever; - const std::string uri = "file://" + _fileName; - return loadMesh(*retriever.retrieve(uri), uri); + const auto retriever = std::make_shared(); + return loadMesh("file://" + _fileName, retriever); } } // namespace dynamics diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index e1312401afc62..48ed37d18ff9b 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -45,6 +45,12 @@ #include "dart/dynamics/Shape.h" #include "dart/utils/ResourceRetriever.h" +namespace Assimp { + +class IOSystem; + +} // namespace Assimp + namespace dart { namespace dynamics { @@ -93,12 +99,12 @@ class MeshShape : public Shape { bool _default = true) const; /// \brief - static const aiScene* loadMesh(const uint8_t* _data, size_t _size, - const std::string& _uri = std::string()); - static const aiScene* loadMesh(const utils::MemoryResource& _resource, - const std::string& _uri = std::string()); static const aiScene* loadMesh(const std::string& _fileName); + /// \brief + static const aiScene* loadMesh( + const std::string& _uri, const utils::ResourceRetrieverPtr& _retriever); + // Documentation inherited. virtual Eigen::Matrix3d computeInertia(double _mass) const; diff --git a/dart/utils/AssimpInputResourceAdaptor.cpp b/dart/utils/AssimpInputResourceAdaptor.cpp new file mode 100644 index 0000000000000..eae1cc17dc163 --- /dev/null +++ b/dart/utils/AssimpInputResourceAdaptor.cpp @@ -0,0 +1,205 @@ +#include +#include +#include +#include "dart/common/Console.h" +#include "AssimpInputResourceAdaptor.h" + +namespace dart { +namespace utils { + +/* + * AssimpInputResourceRetrieverWrapper + */ +AssimpInputResourceRetrieverAdaptor::AssimpInputResourceRetrieverAdaptor( + const ResourceRetrieverPtr& _resourceRetriever) + : mResourceRetriever(_resourceRetriever) +{ +} + +bool AssimpInputResourceRetrieverAdaptor::Exists(const char* pFile) const +{ + return mResourceRetriever->exists(pFile); +} + +char AssimpInputResourceRetrieverAdaptor::getOsSeparator() const +{ + // URIs always use forward slash as a delimeter. + return '/'; +} + +Assimp::IOStream* AssimpInputResourceRetrieverAdaptor::Open( + const char* pFile, const char* pMode) +{ + // TODO: How do we support text mode? + if(pMode != std::string("r") && pMode != std::string("rb") + && pMode != std::string("rt")) + { + dtwarn << "[AssimpInputResourceRetrieverAdaptor::Open] Unsupported mode '" + << pMode << "'. Only 'r', 'rb', and 'rt' are supported.\n"; + return nullptr; + } + + if(const ResourcePtr resource = mResourceRetriever->retrieve(pFile)) + return new AssimpInputResourceAdaptor(resource); + else + return nullptr; +} + +void AssimpInputResourceRetrieverAdaptor::Close(Assimp::IOStream* pFile) +{ + if(pFile) + delete pFile; +} + + +/* + * AssimpInputResourceAdaptor + */ +AssimpInputResourceAdaptor::AssimpInputResourceAdaptor( + const ResourcePtr& _resource) + : mResource(_resource) +{ + assert(_resource); +} + +size_t AssimpInputResourceAdaptor::Read( + void* pvBuffer, size_t psize, size_t pCount) +{ + return mResource->read(pvBuffer, psize, pCount); +} + +size_t AssimpInputResourceAdaptor::Write( + const void* pvBuffer, size_t pSize, size_t pCount) +{ + dterr << "[AssimpInputResourceAdaptor::Write] Write is not implemented. This" + " is a read-only stream.\n"; + return 0; +} + +aiReturn AssimpInputResourceAdaptor::Seek(size_t pOffset, aiOrigin pOrigin) +{ + Resource::SeekType origin; + switch(pOrigin) + { + case aiOrigin_CUR: + origin = Resource::SEEKTYPE_CUR; + break; + + case aiOrigin_END: + origin = Resource::SEEKTYPE_END; + break; + + case aiOrigin_SET: + origin = Resource::SEEKTYPE_SET; + break; + + default: + dterr << "[AssimpInputResourceAdaptor::Seek] Invalid origin. Expected" + " aiOrigin_CUR, aiOrigin_END, or aiOrigin_SET.\n"; + return aiReturn_FAILURE; + } + + if(mResource->seek(pOffset, origin)) + return aiReturn_SUCCESS; + else + return aiReturn_FAILURE; +} + +size_t AssimpInputResourceAdaptor::Tell() const +{ + return mResource->tell(); +} + +size_t AssimpInputResourceAdaptor::FileSize() const +{ + return mResource->getFileSize(); +} + +void AssimpInputResourceAdaptor::Flush() +{ + dterr << "[AssimpInputResourceAdaptor::Flush] Flush is not implemented. This" + " is a read-only stream.\n"; +} + + +/* + * C API + */ +namespace { + +inline Assimp::IOSystem *getIOSystem(aiFileIO *_io) +{ + return reinterpret_cast(_io->UserData); +} + +inline Assimp::IOStream *getIOStream(aiFile *_file) +{ + return reinterpret_cast(_file->UserData); +} + +void fileFlushProc(aiFile *_file) +{ + getIOStream(_file)->Flush(); +} + +size_t fileReadProc(aiFile *_file, char *_buffer, size_t _size, size_t _count) +{ + return getIOStream(_file)->Read(_buffer, _size, _count); +} + +aiReturn fileSeekProc(aiFile *_file, size_t _offset, aiOrigin _origin) +{ + return getIOStream(_file)->Seek(_offset, _origin); +} + +size_t fileSizeProc(aiFile *_file) +{ + return getIOStream(_file)->FileSize(); +} + +size_t fileTellProc(aiFile *_file) +{ + return getIOStream(_file)->Tell(); +} + +size_t fileWriteProc( + aiFile *_file, const char *_buffer, size_t _size, size_t _count) +{ + return getIOStream(_file)->Write(_buffer, _size, _count); +} + +aiFile *fileOpenProc(aiFileIO* _io, const char* _path, const char* _mode) +{ + Assimp::IOStream* stream = getIOSystem(_io)->Open(_path, _mode); + if(!stream) + return nullptr; + + aiFile* out = new aiFile; + out->FileSizeProc = &fileSizeProc; + out->FlushProc = &fileFlushProc; + out->ReadProc = &fileReadProc; + out->SeekProc = &fileSeekProc; + out->TellProc = &fileTellProc; + out->WriteProc = &fileWriteProc; + out->UserData = reinterpret_cast(stream); + return out; +} + +void fileCloseProc(aiFileIO *_io, aiFile *_file) +{ + getIOSystem(_io)->Close(getIOStream(_file)); +} + +} + +aiFileIO createFileIO(Assimp::IOSystem* _system) +{ + aiFileIO out; + out.OpenProc = &fileOpenProc; + out.CloseProc = &fileCloseProc; + out.UserData = reinterpret_cast(_system); + return out; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/AssimpInputResourceAdaptor.h b/dart/utils/AssimpInputResourceAdaptor.h new file mode 100644 index 0000000000000..9b83546907615 --- /dev/null +++ b/dart/utils/AssimpInputResourceAdaptor.h @@ -0,0 +1,130 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ +#define DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ +#include +#include +#include +#include "dart/utils/Resource.h" +#include "dart/utils/ResourceRetriever.h" + +namespace dart { +namespace utils { + +class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem +{ +public: + AssimpInputResourceRetrieverAdaptor( + const ResourceRetrieverPtr& _resourceRetriever); + virtual ~AssimpInputResourceRetrieverAdaptor() = default; + + /// \brief Tests for the existence of a file at the given path. + /// + /// \param pFile Path to the file + /// \return true if there is a file with this path, else false. + bool Exists(const char* pFile) const override; + + /// \brief Returns the system specific directory separator + /// + /// \return System specific directory separator + char getOsSeparator() const override; + + /// \brief Open a new file with a given path. + /// + /// When the access to the file is finished, call Close() to release + /// all associated resources (or the virtual dtor of the IOStream). + /// + /// \param pFile Path to the file + /// \param pMode Desired file I/O mode. Required are: "wb", "w", "wt", + /// "rb", "r", "rt". + /// \return New IOStream interface allowing the lib to access + /// the underlying file. + Assimp::IOStream* Open(const char* pFile, const char* pMode = "rb") override; + + /// \brief Closes the given file and releases all resources + /// associated with it. + /// \param pFile The file instance previously created by Open(). + void Close(Assimp::IOStream* pFile) override; + +private: + ResourceRetrieverPtr mResourceRetriever; +}; + +class AssimpInputResourceAdaptor : public Assimp::IOStream +{ +public: + AssimpInputResourceAdaptor(const ResourcePtr& _resource); + virtual ~AssimpInputResourceAdaptor() = default; + + /// \brief Read from the file + /// + /// See fread() for more details + size_t Read(void* pvBuffer, size_t pSize, size_t pCount) override; + + /// \brief Not implemented. This is a read-only stream. + size_t Write(const void* pvBuffer, size_t pSize, size_t pCount) override; + + /// \brief Set the read/write cursor of the file + /// + /// Note that the offset is _negative_ for aiOrigin_END. + /// See fseek() for more details + aiReturn Seek(size_t pOffset, aiOrigin pOrigin) override; + + /// \brief Get the current position of the read/write cursor + /// + /// See ftell() for more details + size_t Tell() const override; + + /// \brief Returns filesize + /// + /// Returns the filesize. + size_t FileSize() const override; + + /// \brief Not implemented. This is a read-only stream. + void Flush() override; + +private: + ResourcePtr mResource; +}; + + +aiFileIO createFileIO(Assimp::IOSystem* adaptor); +aiFile createFile(Assimp::IOStream* adaptor); + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ diff --git a/dart/utils/LocalResource.cpp b/dart/utils/LocalResource.cpp new file mode 100644 index 0000000000000..908c82f07a4ca --- /dev/null +++ b/dart/utils/LocalResource.cpp @@ -0,0 +1,134 @@ +#include +#include +#include "LocalResource.h" +#include "dart/common/Console.h" + +namespace dart { +namespace utils { + +LocalResource::LocalResource(const std::string& _path) + : mFile(std::fopen(_path.c_str(), "rb")) +{ + if(!mFile) + { + dterr << "[LocalResource::constructor] Failed opening file '" << _path + << "' for reading: " << std::strerror(errno) << "\n"; + } +} + +LocalResource::~LocalResource() +{ + if (!mFile) + return; + + if (std::fclose(mFile) == EOF) + { + dterr << "[LocalResource::destructor] Failed closing file.\n"; + } +} + +size_t LocalResource::getFileSize() +{ + if(!mFile) + return 0; + + const long offset = std::ftell(mFile); + if(offset == -1L) + { + dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + " getting current offset: " << std::strerror(errno) << "\n"; + return 0; + } + + // The SEEK_END option is not required by the C standard. However, it is + // required by POSIX. + // TODO: Does this work on Windows? + if(std::fseek(mFile, 0, SEEK_END) || std::ferror(mFile)) + { + dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + " seeking to the end of the file.\n"; + return 0; + } + + const long size = std::ftell(mFile); + if(size == -1L) + { + dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + " getting end of file offset: " << std::strerror(errno) << "\n"; + return 0; + } + + if(std::fseek(mFile, offset, SEEK_SET) || std::ferror(mFile)) + { + dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + " seeking to the current position.\n"; + return 0; + } + + return size; +} + +size_t LocalResource::tell() +{ + if(!mFile) + return 0; + + const long offset = std::ftell(mFile); + if (offset == -1L) + { + dterr << "[LocalResource::tell] Failed" + " seeking to the current position.\n"; + } + + // We return -1 to match the beahvior of DefaultIoStream in Assimp. + return offset; +} + +bool LocalResource::seek(size_t _offset, SeekType _mode) +{ + int origin; + switch(_mode) + { + case Resource::SEEKTYPE_CUR: + origin = SEEK_CUR; + break; + + case Resource::SEEKTYPE_END: + origin = SEEK_END; + break; + + case Resource::SEEKTYPE_SET: + origin = SEEK_SET; + break; + + default: + dterr << "[LocalResource::Seek] Invalid origin. Expected" + " SEEKTYPE_CUR, SEEKTYPE_END, or SEEKTYPE_SET.\n"; + return false; + } + + if (!std::fseek(mFile, _offset, origin) && !std::ferror(mFile)) + return true; + else + { + dtwarn << "[LocalResource::seek] Seeking failed.\n"; + return false; + } +} + +size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) +{ + if (!mFile) + return 0; + + const size_t result = std::fread(_buffer, _size, _count, mFile); + if (std::ferror(mFile)) + { + dterr << "[LocalResource::tell] Failed reading file.\n"; + } + return result; +} + +} // namespace utils +} // namespace dart + diff --git a/dart/utils/LocalResource.h b/dart/utils/LocalResource.h new file mode 100644 index 0000000000000..77214b837a637 --- /dev/null +++ b/dart/utils/LocalResource.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_LOCALRESOURCE_H_ +#define DART_UTILS_LOCALRESOURCE_H_ +#include "Resource.h" + +namespace dart { +namespace utils { + +class LocalResource : public virtual Resource +{ +public: + LocalResource(const std::string& _path); + virtual ~LocalResource(); + + LocalResource(const LocalResource& _other) = delete; + LocalResource& operator=(const LocalResource& _other) = delete; + + // Documentation inherited. + size_t getFileSize() override; + + // Documentation inherited. + size_t tell() override; + + // Documentation inherited. + bool seek(size_t _origin, SeekType _mode) override; + + // Documentation inherited. + size_t read(void *_buffer, size_t _size, size_t _count) override; + +private: + std::FILE* mFile; +}; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index dbd70c38aa6ee..b066c4e6fc631 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -2,6 +2,7 @@ #include #include "dart/common/Console.h" #include "dart/utils/LocalResourceRetriever.h" +#include "LocalResource.h" static const std::string FILE_SCHEMA = "file://"; @@ -16,8 +17,7 @@ bool LocalResourceRetriever::exists(const std::string& _uri) return false; } -ConstMemoryResourcePtr LocalResourceRetriever::retrieve( - const std::string& _uri) +ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) { // Only file:// URIs are local. if(_uri.find(FILE_SCHEMA) != 0) @@ -26,24 +26,7 @@ ConstMemoryResourcePtr LocalResourceRetriever::retrieve( const std::string localPath = _uri.substr(FILE_SCHEMA.size()); std::ifstream stream(localPath.c_str(), std::ios::binary); - // Compute the length of the file. - std::streampos size = stream.tellg(); - stream.seekg(0, std::ios::end); - size = stream.tellg() - size; - stream.seekg(0, std::ios::beg); - - // Read the full contents of the file. - const auto resource = std::make_shared( - new uint8_t[size], size, true); - stream.read(reinterpret_cast(resource->getData()), size); - - if(!stream.good()) - { - dtwarn << "[LocalResourceRetriever::retrieve] Failed to read file '" - << localPath << "'.\n"; - return nullptr; - } - return resource; + return std::make_shared(localPath); } } // namespace utils diff --git a/dart/utils/LocalResourceRetriever.h b/dart/utils/LocalResourceRetriever.h index 60d5bc71025c8..518a06edfa339 100644 --- a/dart/utils/LocalResourceRetriever.h +++ b/dart/utils/LocalResourceRetriever.h @@ -47,7 +47,7 @@ class LocalResourceRetriever : public virtual ResourceRetriever virtual ~LocalResourceRetriever() = default; bool exists(const std::string& _uri) override; - ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + ResourcePtr retrieve(const std::string& _uri) override; }; typedef std::shared_ptr LocalResourceRetrieverPtr; diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 47a9ab8a6d34c..33ff94197e1ba 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -4,7 +4,6 @@ #include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/ResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" - namespace dart { namespace utils { @@ -45,8 +44,7 @@ bool PackageResourceRetriever::exists(const std::string& _uri) return false; } -ConstMemoryResourcePtr PackageResourceRetriever::retrieve( - const std::string& _uri) +ResourcePtr PackageResourceRetriever::retrieve(const std::string& _uri) { std::string packageName, relativePath; if (!resolvePackageUri(_uri, packageName, relativePath)) diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index fb7f7bf6216e8..be28f68bc874d 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -76,7 +76,7 @@ class PackageResourceRetriever : public virtual ResourceRetriever const std::string& _packageDirectory); bool exists(const std::string& _uri) override; - ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + ResourcePtr retrieve(const std::string& _uri) override; private: ResourceRetrieverPtr mLocalRetriever; diff --git a/dart/utils/Resource.h b/dart/utils/Resource.h new file mode 100644 index 0000000000000..9adf7867f524e --- /dev/null +++ b/dart/utils/Resource.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_RESOURCE_H_ +#define DART_UTILS_RESOURCE_H_ +#include +#include + +namespace dart { +namespace utils { + +class Resource +{ +public: + enum SeekType { + SEEKTYPE_CUR, + SEEKTYPE_END, + SEEKTYPE_SET + }; + + static const size_t SIZE_UNKNOWN = -1; + + virtual ~Resource() = default; + + virtual size_t getFileSize() = 0; + + virtual size_t tell() = 0; + + virtual bool seek(size_t _offset, SeekType _origin) = 0; + + virtual size_t read(void *_buffer, size_t _size, size_t _count) = 0; +}; + +typedef std::shared_ptr ResourcePtr; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ diff --git a/dart/utils/ResourceRetriever.h b/dart/utils/ResourceRetriever.h index 39ef30fcdd784..d39745c249ac9 100644 --- a/dart/utils/ResourceRetriever.h +++ b/dart/utils/ResourceRetriever.h @@ -36,8 +36,8 @@ #define DART_UTILS_RESOURCERETRIEVER_H_ #include -#include #include +#include "Resource.h" namespace dart { namespace utils { @@ -70,13 +70,12 @@ struct ResourceRetriever { virtual ~ResourceRetriever() = default; virtual bool exists(const std::string& _uri) = 0; - virtual ConstMemoryResourcePtr retrieve(const std::string& _uri) = 0; + virtual ResourcePtr retrieve(const std::string& _uri) = 0; }; typedef std::shared_ptr ResourceRetrieverPtr; -typedef std::shared_ptr ConstResourceRetrieverPtr; -} // namespace utils -} // namespace dart +} // namespace utils +} // namespace dart #endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index 14742ed7bab2f..c8c5c61579e00 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -37,12 +37,12 @@ bool SchemaResourceRetriever::exists(const std::string& _uri) return false; } -ConstMemoryResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) +ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) { const std::vector &retrievers = getRetrievers(_uri); for(const ResourceRetrieverPtr& resourceRetriever : retrievers) { - if(ConstMemoryResourcePtr resource = resourceRetriever->retrieve(_uri)) + if(ResourcePtr resource = resourceRetriever->retrieve(_uri)) return resource; } diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index 5843b821b8d6c..8eac46d64b8bd 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -52,7 +52,7 @@ class SchemaResourceRetriever : public virtual ResourceRetriever const ResourceRetrieverPtr& _resourceRetriever); bool exists(const std::string& _uri) override; - ConstMemoryResourcePtr retrieve(const std::string& _uri) override; + ResourcePtr retrieve(const std::string& _uri) override; private: std::unordered_map(_vizOrCol->geometry.get())) { const std::string& uri = mesh->filename; - const utils::ConstMemoryResourcePtr resource = mRetriever->retrieve(uri); - if (!resource) { - return nullptr; - } - - const aiScene* scene = dynamics::MeshShape::loadMesh(*resource, uri); + const aiScene* scene = dynamics::MeshShape::loadMesh(uri, mRetriever); if (!scene) return nullptr; From 0ae6658439f61bb1e3e5e623fe2eaf4ee3a722b4 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 13 Jul 2015 23:25:02 -0500 Subject: [PATCH 06/55] Removed MemoryResource. --- dart/utils/ResourceRetriever.cpp | 44 -------------------------------- dart/utils/ResourceRetriever.h | 24 ----------------- 2 files changed, 68 deletions(-) delete mode 100644 dart/utils/ResourceRetriever.cpp diff --git a/dart/utils/ResourceRetriever.cpp b/dart/utils/ResourceRetriever.cpp deleted file mode 100644 index 42bbf909e2af5..0000000000000 --- a/dart/utils/ResourceRetriever.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include -#include "ResourceRetriever.h" - -namespace dart { -namespace utils { - -MemoryResource::MemoryResource() - : mData(nullptr), - mSize(0), - mIsOwner(false) -{ -} - -MemoryResource::MemoryResource(uint8_t* _data, size_t _size, bool _isOwner) - : mData(_data), - mSize(_size), - mIsOwner(_isOwner) -{ - assert(_size == 0 || _data != nullptr); -} - -MemoryResource::~MemoryResource() -{ - if (mData && mIsOwner) - delete mData; -} - -uint8_t* MemoryResource::getData() -{ - return mData; -} - -const uint8_t* MemoryResource::getData() const -{ - return mData; -} - -size_t MemoryResource::getSize() const -{ - return mSize; -} - -} // namespace utils -} // namespace dart diff --git a/dart/utils/ResourceRetriever.h b/dart/utils/ResourceRetriever.h index d39745c249ac9..571bf69cb489a 100644 --- a/dart/utils/ResourceRetriever.h +++ b/dart/utils/ResourceRetriever.h @@ -42,30 +42,6 @@ namespace dart { namespace utils { -struct MemoryResource { -public: - MemoryResource(); - MemoryResource(uint8_t* _data, size_t _size, bool _isOwner); - MemoryResource(const MemoryResource& _other) = delete; - ~MemoryResource(); - - MemoryResource& operator=(const MemoryResource& _other) = delete; - - uint8_t* getData(); - const uint8_t* getData() const; - - size_t getSize() const; - -private: - uint8_t *mData; - size_t mSize; - bool mIsOwner; -}; - -typedef std::shared_ptr MemoryResourcePtr; -typedef std::shared_ptr ConstMemoryResourcePtr; - - struct ResourceRetriever { virtual ~ResourceRetriever() = default; From 02028bcb1ec4c7013fc77001fb8a51b9581e76db Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 14 Jul 2015 00:12:05 -0500 Subject: [PATCH 07/55] Refactoring in preparation for static. --- dart/utils/urdf/DartLoader.cpp | 92 +++++++++++++++++++++++++--------- dart/utils/urdf/DartLoader.h | 23 +++++++-- 2 files changed, 86 insertions(+), 29 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index ae9fef9c8d8a8..2aba46a054f43 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -43,6 +43,30 @@ void DartLoader::addPackageDirectory(const std::string& _packageName, mPackageRetriever->addPackageDirectory(_packageName, _packageDirectory); } +dynamics::SkeletonPtr DartLoader::readSkeleton(const std::string& _uri, + const utils::ResourceRetrieverPtr& _resourceRetriever) +{ + using ModelInterfacePtr = boost::shared_ptr; + + const ResourcePtr resource = _resourceRetriever->retrieve(_uri); + const size_t size = resource->getFileSize(); + + // std::string is guaranteed to be contiguous in C++11. + std::string buffer; + buffer.resize(size); + resource->read(&buffer.front(), size, 1); + + // Use urdfdom to load the URDF file. + const ModelInterfacePtr urdfInterface = urdf::parseURDF(buffer); + if(!urdfInterface) + { + dtwarn << "[DartLoader::readSkeleton] Failed loading URDF file '" + << _uri << "'.\n"; + return nullptr; + } + return modelInterfaceToSkeleton(urdfInterface.get(), _resourceRetriever); +} + dynamics::SkeletonPtr DartLoader::parseSkeleton(const std::string& _urdfFileName) { std::string urdfString = readFileToString(_urdfFileName); @@ -59,7 +83,7 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton(const std::string& _urdfFileName std::replace(mRootToSkelPath.begin(), mRootToSkelPath.end(), '\\' , '/' ); mRootToSkelPath = mRootToSkelPath.substr(0, mRootToSkelPath.rfind("/") + 1); - return parseSkeletonString(urdfString, mRootToSkelPath);; + return parseSkeletonString(urdfString, mRootToSkelPath); } dynamics::SkeletonPtr DartLoader::parseSkeletonString( @@ -78,7 +102,7 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( if(!skeletonModelPtr) return nullptr; - return modelInterfaceToSkeleton(skeletonModelPtr.get()); + return modelInterfaceToSkeleton(skeletonModelPtr.get(), mRetriever); } simulation::WorldPtr DartLoader::parseWorld(const std::string& _urdfFileName) @@ -138,7 +162,8 @@ simulation::WorldPtr DartLoader::parseWorldString( } mRootToSkelPath = mRootToWorldPath + it->second; - dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get()); + dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( + worldInterface->models[i].model.get(), mRetriever); if(!skeleton) { @@ -231,8 +256,10 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) * @function modelInterfaceToSkeleton * @brief Read the ModelInterface and spits out a Skeleton object */ -dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInterface* _model) { - +dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( + const urdf::ModelInterface* _model, + const utils::ResourceRetrieverPtr& _resourceRetriever) +{ dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(_model->getName()); dynamics::BodyNode* rootNode = nullptr; @@ -247,11 +274,12 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte { root = root->child_links[0].get(); dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties)) + if (!createDartNodeProperties(root, &rootProperties, _resourceRetriever)) return nullptr; rootNode = createDartJointAndNode( - root->parent_joint.get(), rootProperties, nullptr, skeleton); + root->parent_joint.get(), rootProperties, nullptr, skeleton, + _resourceRetriever); if(nullptr == rootNode) { dterr << "[DartLoader::modelInterfaceToSkeleton] Failed to create root node!\n"; @@ -262,7 +290,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte else { dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties)) + if (!createDartNodeProperties(root, &rootProperties, _resourceRetriever)) return nullptr; std::pair pair = @@ -276,7 +304,8 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte for(size_t i = 0; i < root->child_links.size(); i++) { - if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), *rootNode)) + if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), + *rootNode, _resourceRetriever)) return nullptr; } @@ -285,23 +314,27 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte } bool DartLoader::createSkeletonRecursive( - dynamics::SkeletonPtr _skel, - const urdf::Link* _lk, - dynamics::BodyNode& _parentNode) + dynamics::SkeletonPtr _skel, + const urdf::Link* _lk, + dynamics::BodyNode& _parentNode, + const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::BodyNode::Properties properties; - if (!createDartNodeProperties(_lk, &properties)) + if (!createDartNodeProperties(_lk, &properties, _resourceRetriever)) return false; dynamics::BodyNode* node = createDartJointAndNode( - _lk->parent_joint.get(), properties, &_parentNode, _skel); + _lk->parent_joint.get(), properties, &_parentNode, _skel, + _resourceRetriever); + if(!node) return false; for(size_t i = 0; i < _lk->child_links.size(); ++i) { - if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), *node)) - return false; + if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), *node, + _resourceRetriever)) + return false; } return true; } @@ -346,7 +379,8 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( const urdf::Joint* _jt, const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, - dynamics::SkeletonPtr _skeleton) + dynamics::SkeletonPtr _skeleton, + const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::Joint::Properties basicProperties; @@ -433,7 +467,9 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( * @function createDartNode */ bool DartLoader::createDartNodeProperties( - const urdf::Link* _lk, dynamics::BodyNode::Properties *node) + const urdf::Link* _lk, + dynamics::BodyNode::Properties *node, + const utils::ResourceRetrieverPtr& _resourceRetriever) { node->mName = _lk->name; @@ -458,7 +494,8 @@ bool DartLoader::createDartNodeProperties( // Set visual information for(size_t i = 0; i < _lk->visual_array.size(); i++) { - if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get())) + if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get(), + _resourceRetriever)) node->mVizShapes.push_back(shape); else return false; @@ -466,7 +503,8 @@ bool DartLoader::createDartNodeProperties( // Set collision information for(size_t i = 0; i < _lk->collision_array.size(); i++) { - if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) + if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get(), + _resourceRetriever)) node->mColShapes.push_back(shape); else return false; @@ -489,7 +527,9 @@ void setMaterial(dynamics::ShapePtr _shape, const urdf::Collision* _col) { * @function createShape */ template -dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) +dynamics::ShapePtr DartLoader::createShape( + const VisualOrCollision* _vizOrCol, + const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::ShapePtr shape; @@ -515,7 +555,7 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { const std::string& uri = mesh->filename; - const aiScene* scene = dynamics::MeshShape::loadMesh(uri, mRetriever); + const aiScene* scene = dynamics::MeshShape::loadMesh(uri, _resourceRetriever); if (!scene) return nullptr; @@ -537,8 +577,12 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) return shape; } -template dynamics::ShapePtr DartLoader::createShape(const urdf::Visual* _vizOrCol); -template dynamics::ShapePtr DartLoader::createShape(const urdf::Collision* _vizOrCol); +template dynamics::ShapePtr DartLoader::createShape( + const urdf::Visual* _vizOrCol, + const utils::ResourceRetrieverPtr& _resourceRetriever); +template dynamics::ShapePtr DartLoader::createShape( + const urdf::Collision* _vizOrCol, + const utils::ResourceRetrieverPtr& _resourceRetriever); /** * @function pose2Affine3d diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 16b5503744cb6..96d15174776c0 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -80,6 +80,9 @@ class DartLoader { void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); + dynamics::SkeletonPtr readSkeleton(const std::string& _uri, + const utils::ResourceRetrieverPtr& _resourceRetriever); + /// Parse a file to produce a Skeleton dynamics::SkeletonPtr parseSkeleton(const std::string& _urdfFileName); @@ -101,20 +104,30 @@ class DartLoader { void parseWorldToEntityPaths(const std::string& _xml_string); - dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model); - bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent); + dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( + const urdf::ModelInterface* _model, + const utils::ResourceRetrieverPtr& _resourceRetriever); + + bool createSkeletonRecursive( + dynamics::SkeletonPtr _skel, + const urdf::Link* _lk, + dynamics::BodyNode& _parent, + const utils::ResourceRetrieverPtr& _resourceRetriever); template - dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol); + dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol, + const utils::ResourceRetrieverPtr& _resourceRetriever); dynamics::BodyNode* createDartJointAndNode( const urdf::Joint* _jt, const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, - dynamics::SkeletonPtr _skeleton); + dynamics::SkeletonPtr _skeleton, + const utils::ResourceRetrieverPtr& _resourceRetriever); bool createDartNodeProperties( - const urdf::Link* _lk, dynamics::BodyNode::Properties *properties); + const urdf::Link* _lk, dynamics::BodyNode::Properties *properties, + const utils::ResourceRetrieverPtr& _resourceRetriever); Eigen::Isometry3d toEigen(const urdf::Pose& _pose); Eigen::Vector3d toEigen(const urdf::Vector3& _vector); From 2b2121cc23f7b0d5a5edcb50d5a6a943631589ea Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Fri, 17 Jul 2015 22:44:39 -0500 Subject: [PATCH 08/55] Added tests for LocalResourceRetriever. --- dart/utils/LocalResource.cpp | 23 ++-- dart/utils/LocalResource.h | 3 + dart/utils/LocalResourceRetriever.cpp | 39 ++++-- data/test/hello_world.txt | 1 + unittests/testLocalResourceRetriever.cpp | 160 +++++++++++++++++++++++ 5 files changed, 205 insertions(+), 21 deletions(-) create mode 100644 data/test/hello_world.txt create mode 100644 unittests/testLocalResourceRetriever.cpp diff --git a/dart/utils/LocalResource.cpp b/dart/utils/LocalResource.cpp index 908c82f07a4ca..ff42748d9e402 100644 --- a/dart/utils/LocalResource.cpp +++ b/dart/utils/LocalResource.cpp @@ -11,7 +11,7 @@ LocalResource::LocalResource(const std::string& _path) { if(!mFile) { - dterr << "[LocalResource::constructor] Failed opening file '" << _path + dtwarn << "[LocalResource::constructor] Failed opening file '" << _path << "' for reading: " << std::strerror(errno) << "\n"; } } @@ -23,10 +23,15 @@ LocalResource::~LocalResource() if (std::fclose(mFile) == EOF) { - dterr << "[LocalResource::destructor] Failed closing file.\n"; + dtwarn << "[LocalResource::destructor] Failed closing file.\n"; } } +bool LocalResource::isGood() const +{ + return !!mFile; +} + size_t LocalResource::getFileSize() { if(!mFile) @@ -35,7 +40,7 @@ size_t LocalResource::getFileSize() const long offset = std::ftell(mFile); if(offset == -1L) { - dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" " getting current offset: " << std::strerror(errno) << "\n"; return 0; } @@ -45,7 +50,7 @@ size_t LocalResource::getFileSize() // TODO: Does this work on Windows? if(std::fseek(mFile, 0, SEEK_END) || std::ferror(mFile)) { - dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" " seeking to the end of the file.\n"; return 0; } @@ -53,14 +58,14 @@ size_t LocalResource::getFileSize() const long size = std::ftell(mFile); if(size == -1L) { - dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" " getting end of file offset: " << std::strerror(errno) << "\n"; return 0; } if(std::fseek(mFile, offset, SEEK_SET) || std::ferror(mFile)) { - dterr << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" " seeking to the current position.\n"; return 0; } @@ -76,7 +81,7 @@ size_t LocalResource::tell() const long offset = std::ftell(mFile); if (offset == -1L) { - dterr << "[LocalResource::tell] Failed" + dtwarn << "[LocalResource::tell] Failed" " seeking to the current position.\n"; } @@ -102,7 +107,7 @@ bool LocalResource::seek(size_t _offset, SeekType _mode) break; default: - dterr << "[LocalResource::Seek] Invalid origin. Expected" + dtwarn << "[LocalResource::Seek] Invalid origin. Expected" " SEEKTYPE_CUR, SEEKTYPE_END, or SEEKTYPE_SET.\n"; return false; } @@ -124,7 +129,7 @@ size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) const size_t result = std::fread(_buffer, _size, _count, mFile); if (std::ferror(mFile)) { - dterr << "[LocalResource::tell] Failed reading file.\n"; + dtwarn << "[LocalResource::tell] Failed reading file.\n"; } return result; } diff --git a/dart/utils/LocalResource.h b/dart/utils/LocalResource.h index 77214b837a637..b11ff59e56ba3 100644 --- a/dart/utils/LocalResource.h +++ b/dart/utils/LocalResource.h @@ -49,6 +49,9 @@ class LocalResource : public virtual Resource LocalResource(const LocalResource& _other) = delete; LocalResource& operator=(const LocalResource& _other) = delete; + /// Return if the resource is open and in a valid state. + bool isGood() const; + // Documentation inherited. size_t getFileSize() override; diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index b066c4e6fc631..9db060b86aa13 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -1,32 +1,47 @@ +#include #include -#include #include "dart/common/Console.h" #include "dart/utils/LocalResourceRetriever.h" #include "LocalResource.h" -static const std::string FILE_SCHEMA = "file://"; +static std::string getPath(const std::string& _uriOrPath) +{ + static const std::string file_schema = "file://"; + + // There is no schema, so we'll interpret the input as a path. + if(_uriOrPath.find_first_of(':') == std::string::npos) + return _uriOrPath; + // This is a file:// URI, so we'll strip off the schema. + else if(_uriOrPath.size() > file_schema.size() + && _uriOrPath.substr(0, file_schema.size()) == file_schema) + return _uriOrPath.substr(file_schema.size()); + else + return ""; +} + namespace dart { namespace utils { bool LocalResourceRetriever::exists(const std::string& _uri) { - if(_uri.size() >= FILE_SCHEMA.size()) - return boost::filesystem::exists(_uri.substr(FILE_SCHEMA.size())); - else - return false; + // Open and close the file to check if it exists. It would be more efficient + // to stat() it, but that is not portable. + const std::string path = getPath(_uri); + return !path.empty() && std::ifstream(path, std::ios::binary).good(); } ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) { - // Only file:// URIs are local. - if(_uri.find(FILE_SCHEMA) != 0) + const std::string path = getPath(_uri); + if(path.empty()) return nullptr; - const std::string localPath = _uri.substr(FILE_SCHEMA.size()); - std::ifstream stream(localPath.c_str(), std::ios::binary); - - return std::make_shared(localPath); + const auto resource = std::make_shared(path); + if(resource->isGood()) + return resource; + else + return nullptr; } } // namespace utils diff --git a/data/test/hello_world.txt b/data/test/hello_world.txt new file mode 100644 index 0000000000000..557db03de997c --- /dev/null +++ b/data/test/hello_world.txt @@ -0,0 +1 @@ +Hello World diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp new file mode 100644 index 0000000000000..deb9a33c915bd --- /dev/null +++ b/unittests/testLocalResourceRetriever.cpp @@ -0,0 +1,160 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 +#include "dart/utils/LocalResourceRetriever.h" +#include "TestHelpers.h" + +using dart::utils::Resource; +using dart::utils::LocalResourceRetriever; + +TEST(LocalResourceRetriever, exists_UnsupportedUri_ReturnsFalse) +{ + LocalResourceRetriever retriever; + EXPECT_FALSE(retriever.exists("unknown://test")); +} + +TEST(LocalResourceRetriever, exists_FileUriDoesNotExist_ReturnsFalse) +{ + LocalResourceRetriever retriever; + EXPECT_FALSE(retriever.exists(DART_DATA_PATH"does/not/exist")); +} + +TEST(LocalResourceRetriever, exists_PathDoesNotExist_ReturnsFalse) +{ + LocalResourceRetriever retriever; + EXPECT_FALSE(retriever.exists(DART_DATA_PATH"does/not/exist")); +} + +TEST(LocalResourceRetriever, exists_FileUriDoesExists_ReturnsTrue) +{ + LocalResourceRetriever retriever; + EXPECT_TRUE(retriever.exists("file://" DART_DATA_PATH "skel/cube.skel")); +} + +TEST(LocalResourceRetriever, exists_FileUriDoesNotExists_ReturnsTrue) +{ + LocalResourceRetriever retriever; + EXPECT_TRUE(retriever.exists(DART_DATA_PATH"skel/cube.skel")); +} + +TEST(LocalResourceRetriever, retrieve_UnsupportedUri_ReturnsNull) +{ + LocalResourceRetriever retriever; + EXPECT_EQ(nullptr, retriever.retrieve("unknown://test")); +} + +TEST(LocalResourceRetriever, retrieve_FileUriDoesNotExist_ReturnsNull) +{ + LocalResourceRetriever retriever; + EXPECT_EQ(nullptr, retriever.retrieve(DART_DATA_PATH"does/not/exist")); +} + +TEST(LocalResourceRetriever, retrieve_PathDoesNotExist_ReturnsNull) +{ + LocalResourceRetriever retriever; + EXPECT_EQ(nullptr, retriever.retrieve(DART_DATA_PATH"does/not/exist")); +} + +TEST(LocalResourceRetriever, retrieve_FileUri) +{ + LocalResourceRetriever retriever; + auto resource = retriever.retrieve("file://" DART_DATA_PATH "test/hello_world.txt"); + ASSERT_TRUE(resource != nullptr); +} + +TEST(LocalResourceRetriever, retrieve_Path) +{ + LocalResourceRetriever retriever; + auto resource = retriever.retrieve(DART_DATA_PATH "test/hello_world.txt"); + ASSERT_TRUE(resource != nullptr); +} + +TEST(LocalResourceRetriever, retrieve_ResourceOperations) +{ + const std::string content = "Hello World\n"; + std::vector buffer(100, '\0'); + + LocalResourceRetriever retriever; + auto resource = retriever.retrieve(DART_DATA_PATH "test/hello_world.txt"); + ASSERT_TRUE(resource != nullptr); + + EXPECT_EQ(content.size(), resource->getFileSize()); + + // Relative seek. + ASSERT_TRUE(resource->seek(2, Resource::SEEKTYPE_CUR)); + EXPECT_EQ(2, resource->tell()); + + // Absolute seek. + ASSERT_TRUE(resource->seek(5, Resource::SEEKTYPE_SET)); + EXPECT_EQ(5, resource->tell()); + + // Seek to the end of the file. + ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_END)); + EXPECT_EQ(content.size(), resource->tell()); + + // TODO: SEEKTYPE_END should require negative input. + ASSERT_TRUE(resource->seek(3, Resource::SEEKTYPE_END)); + EXPECT_EQ(content.size() - 3, resource->tell()); + + // Reading a block that's too large should do nothing. + ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); + ASSERT_EQ(0, resource->read(buffer.data(), content.size() + 1, 1)); + + // Reading should only return full blocks. + buffer.assign('\0', buffer.size()); + ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); + ASSERT_EQ(1, resource->read(buffer.data(), 8, 1)); + EXPECT_STREQ(content.substr(0, 8).c_str(), buffer.data()); + + // Reading multiple blocks + buffer.assign('\0', buffer.size()); + ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); + ASSERT_EQ(2, resource->read(buffer.data(), 4, 2)); + EXPECT_STREQ(content.substr(0, 8).c_str(), buffer.data()); + + // Reading the whole file at once. + buffer.assign('\0', buffer.size()); + ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); + ASSERT_EQ(1, resource->read(buffer.data(), content.size(), 1)); + EXPECT_STREQ(content.c_str(), buffer.data()); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 82993bce192a16a2564491d1ed6d061057485962 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Fri, 17 Jul 2015 23:27:50 -0500 Subject: [PATCH 09/55] Added tests for PackageResourceRetriever. --- dart/utils/PackageResourceRetriever.cpp | 4 +- unittests/testPackageResourceRetriever.cpp | 277 +++++++++++++++++++++ 2 files changed, 280 insertions(+), 1 deletion(-) create mode 100644 unittests/testPackageResourceRetriever.cpp diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 33ff94197e1ba..697bc409d82fb 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "dart/common/Console.h" #include "dart/utils/LocalResourceRetriever.h" @@ -38,7 +39,8 @@ bool PackageResourceRetriever::exists(const std::string& _uri) for(const std::string &packagePath : getPackagePaths(packageName)) { - if (mLocalRetriever->exists(_uri)) + const std::string localUri = "file://" + packagePath + relativePath; + if (mLocalRetriever->exists(localUri)) return true; } return false; diff --git a/unittests/testPackageResourceRetriever.cpp b/unittests/testPackageResourceRetriever.cpp new file mode 100644 index 0000000000000..dc4146bae0750 --- /dev/null +++ b/unittests/testPackageResourceRetriever.cpp @@ -0,0 +1,277 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 +#include "dart/utils/PackageResourceRetriever.h" +#include "TestHelpers.h" + +using dart::utils::Resource; +using dart::utils::ResourcePtr; +using dart::utils::ResourceRetriever; +using dart::utils::PackageResourceRetriever; + +namespace { + +struct TestResource : public Resource +{ + size_t getFileSize() override + { + return 0; + } + + size_t tell() override + { + return 0; + } + + bool seek(size_t _offset, SeekType _origin) override + { + return false; + } + + size_t read(void *_buffer, size_t _size, size_t _count) override + { + return 0; + } +}; + +struct PresentResourceRetriever : public ResourceRetriever +{ + bool exists(const std::string& _uri) override + { + mExists.push_back(_uri); + return true; + } + + ResourcePtr retrieve(const std::string& _uri) override + { + mRetrieve.push_back(_uri); + return std::make_shared(); + } + + std::vector mExists; + std::vector mRetrieve; +}; + +struct AbsentResourceRetriever : public ResourceRetriever +{ + bool exists(const std::string& _uri) override + { + mExists.push_back(_uri); + return false; + } + + ResourcePtr retrieve(const std::string& _uri) override + { + mRetrieve.push_back(_uri); + return nullptr; + } + + std::vector mExists; + std::vector mRetrieve; +}; + +} + +TEST(PackageResourceRetriever, exists_UnableToResolve_ReturnsFalse) +{ + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + + EXPECT_FALSE(retriever.exists("package://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, exists_DelegateFails_ReturnsFalse) +{ + // GTest breaks the string concatenation. + const char* expected = "file://" DART_DATA_PATH"test/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test"); + + EXPECT_FALSE(retriever.exists("package://test/foo")); + ASSERT_EQ(1, mockRetriever->mExists.size()); + EXPECT_EQ(expected, mockRetriever->mExists.front()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, exists_UnsupportedUri_ReturnsFalse) +{ + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test"); + + EXPECT_FALSE(retriever.exists("foo://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, exists_StripsTrailingSlash) +{ + const char* expected = "file://" DART_DATA_PATH"test/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test/"); + + EXPECT_TRUE(retriever.exists("package://test/foo")); + ASSERT_EQ(1, mockRetriever->mExists.size()); + EXPECT_EQ(expected, mockRetriever->mExists.front()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, exists_FirstUriSucceeds) +{ + const char* expected = "file://" DART_DATA_PATH"test1/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test1"); + retriever.addPackageDirectory("test", DART_DATA_PATH"test2"); + + EXPECT_TRUE(retriever.exists("package://test/foo")); + ASSERT_EQ(1, mockRetriever->mExists.size()); + EXPECT_EQ(expected, mockRetriever->mExists.front()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, exists_FallsBackOnSecondUri) +{ + const char* expected1 = "file://" DART_DATA_PATH"test1/foo"; + const char* expected2 = "file://" DART_DATA_PATH"test2/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test1"); + retriever.addPackageDirectory("test", DART_DATA_PATH"test2"); + + EXPECT_FALSE(retriever.exists("package://test/foo")); + ASSERT_EQ(2, mockRetriever->mExists.size()); + EXPECT_EQ(expected1, mockRetriever->mExists[0]); + EXPECT_EQ(expected2, mockRetriever->mExists[1]); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, retrieve_UnableToResolve_ReturnsNull) +{ + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + + EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, retrieve_DelegateFails_ReturnsNull) +{ + // GTest breaks the string concatenation. + const char* expected = "file://" DART_DATA_PATH"test/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test"); + + EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + ASSERT_EQ(1, mockRetriever->mRetrieve.size()); + EXPECT_EQ(expected, mockRetriever->mRetrieve.front()); +} + +TEST(PackageResourceRetriever, retrieve_UnsupportedUri_ReturnsNull) +{ + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test"); + + EXPECT_EQ(nullptr, retriever.retrieve("foo://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + EXPECT_TRUE(mockRetriever->mRetrieve.empty()); +} + +TEST(PackageResourceRetriever, retrieve_StripsTrailingSlash) +{ + const char* expected = "file://" DART_DATA_PATH"test/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test/"); + + EXPECT_TRUE(retriever.retrieve("package://test/foo") != nullptr); + EXPECT_TRUE(mockRetriever->mExists.empty()); + ASSERT_EQ(1, mockRetriever->mRetrieve.size()); + EXPECT_EQ(expected, mockRetriever->mRetrieve.front()); +} + +TEST(PackageResourceRetriever, retrieve_FirstUriSucceeds) +{ + const char* expected = "file://" DART_DATA_PATH"test1/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test1"); + retriever.addPackageDirectory("test", DART_DATA_PATH"test2"); + + EXPECT_TRUE(retriever.retrieve("package://test/foo") != nullptr); + EXPECT_TRUE(mockRetriever->mExists.empty()); + ASSERT_EQ(1, mockRetriever->mRetrieve.size()); + EXPECT_EQ(expected, mockRetriever->mRetrieve.front()); +} + +TEST(PackageResourceRetriever, retrieve_FallsBackOnSecondUri) +{ + const char* expected1 = "file://" DART_DATA_PATH"test1/foo"; + const char* expected2 = "file://" DART_DATA_PATH"test2/foo"; + + auto mockRetriever = std::make_shared(); + PackageResourceRetriever retriever(mockRetriever); + retriever.addPackageDirectory("test", DART_DATA_PATH"test1"); + retriever.addPackageDirectory("test", DART_DATA_PATH"test2"); + + EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); + EXPECT_TRUE(mockRetriever->mExists.empty()); + ASSERT_EQ(2, mockRetriever->mRetrieve.size()); + EXPECT_EQ(expected1, mockRetriever->mRetrieve[0]); + EXPECT_EQ(expected2, mockRetriever->mRetrieve[1]); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 6bf445b4e613d12f3197e90c6dc6678fa70f5f3a Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sat, 18 Jul 2015 01:03:44 -0500 Subject: [PATCH 10/55] Refactoring DartLoader to use ResourceRetriever. --- dart/utils/urdf/DartLoader.cpp | 151 ++++++++++++----------- dart/utils/urdf/DartLoader.h | 33 +++-- unittests/testLocalResourceRetriever.cpp | 6 +- 3 files changed, 99 insertions(+), 91 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 2aba46a054f43..b9634a2b06313 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -23,6 +23,17 @@ #include "dart/simulation/World.h" #include "dart/utils/urdf/urdf_world_parser.h" +using ModelInterfacePtr = boost::shared_ptr; + +static std::string getBasePath(const std::string& _uri) +{ + const size_t index = _uri.find_last_of('/'); + if (index != std::string::npos) + return _uri.substr(0, index); + else + return ""; +} + namespace dart { namespace utils { @@ -43,51 +54,36 @@ void DartLoader::addPackageDirectory(const std::string& _packageName, mPackageRetriever->addPackageDirectory(_packageName, _packageDirectory); } -dynamics::SkeletonPtr DartLoader::readSkeleton(const std::string& _uri, +dynamics::SkeletonPtr DartLoader::parseSkeleton( + const std::string& _uri, const utils::ResourceRetrieverPtr& _resourceRetriever) { - using ModelInterfacePtr = boost::shared_ptr; + const utils::ResourceRetrieverPtr resourceRetriever + = getResourceRetriever(_resourceRetriever); - const ResourcePtr resource = _resourceRetriever->retrieve(_uri); - const size_t size = resource->getFileSize(); + // TODO: Support for Windows-style paths? + mRootToSkelPath = getBasePath(_uri); - // std::string is guaranteed to be contiguous in C++11. - std::string buffer; - buffer.resize(size); - resource->read(&buffer.front(), size, 1); + std::string content; + if (!readFileToString(resourceRetriever, _uri, content)) + return nullptr; // Use urdfdom to load the URDF file. - const ModelInterfacePtr urdfInterface = urdf::parseURDF(buffer); + const ModelInterfacePtr urdfInterface = urdf::parseURDF(content); if(!urdfInterface) { dtwarn << "[DartLoader::readSkeleton] Failed loading URDF file '" << _uri << "'.\n"; return nullptr; } - return modelInterfaceToSkeleton(urdfInterface.get(), _resourceRetriever); -} - -dynamics::SkeletonPtr DartLoader::parseSkeleton(const std::string& _urdfFileName) { - std::string urdfString = readFileToString(_urdfFileName); - - if(urdfString.empty()) - { - dtwarn << "[DartLoder::parseSkeleton] A blank or nonexistent file cannot " - << "be parsed into a Skeleton. Returning a nullptr\n"; - return nullptr; - } - - // Change path to a Unix-style path if given a Windows one - // Windows can handle Unix-style paths (apparently) - mRootToSkelPath = _urdfFileName; - std::replace(mRootToSkelPath.begin(), mRootToSkelPath.end(), '\\' , '/' ); - mRootToSkelPath = mRootToSkelPath.substr(0, mRootToSkelPath.rfind("/") + 1); - return parseSkeletonString(urdfString, mRootToSkelPath); + // TODO: Pass the ResourceRetriever down here. + return modelInterfaceToSkeleton(urdfInterface.get(), resourceRetriever); } dynamics::SkeletonPtr DartLoader::parseSkeletonString( - const std::string& _urdfString, const std::string& _urdfFileDirectory) + const std::string& _urdfString, const std::string& _baseUri, + const utils::ResourceRetrieverPtr& _resourceRetriever) { if(_urdfString.empty()) { @@ -96,37 +92,41 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( return nullptr; } - mRootToSkelPath = _urdfFileDirectory; + mRootToSkelPath = _baseUri; - boost::shared_ptr skeletonModelPtr = urdf::parseURDF(_urdfString); - if(!skeletonModelPtr) - return nullptr; + ModelInterfacePtr urdfInterface = urdf::parseURDF(_urdfString); + if(!urdfInterface) + { + dtwarn << "[DartLoader::parseSkeletonString] Failed loading URDF.\n"; + return nullptr; + } - return modelInterfaceToSkeleton(skeletonModelPtr.get(), mRetriever); + // TODO: Pass the ResourceRetriever down here. + return modelInterfaceToSkeleton( + urdfInterface.get(), getResourceRetriever(_resourceRetriever)); } -simulation::WorldPtr DartLoader::parseWorld(const std::string& _urdfFileName) +simulation::WorldPtr DartLoader::parseWorld( + const std::string& _uri, + const utils::ResourceRetrieverPtr& _resourceRetriever) { - std::string urdfString = readFileToString(_urdfFileName); + const utils::ResourceRetrieverPtr resourceRetriever + = getResourceRetriever(_resourceRetriever); - if(urdfString.empty()) - { - dtwarn << "[DartLoader::parseWorld] A blank or nonexistent file cannot " - << "be parsed into a World. Returning a nullptr\n"; + std::string content; + if (!readFileToString(resourceRetriever, _uri, content)) return nullptr; - } - // Change path to a Unix-style path if given a Windows one - // Windows can handle Unix-style paths (apparently) - mRootToWorldPath = _urdfFileName; - std::replace(mRootToWorldPath.begin(), mRootToWorldPath.end(), '\\' , '/'); - mRootToWorldPath = mRootToWorldPath.substr(0, mRootToWorldPath.rfind("/") + 1); + // TODO: Support for Windows-style paths? + mRootToSkelPath = getBasePath(_uri); - return parseWorldString(urdfString, mRootToWorldPath); + // TODO: Pass the ResourceRetriever down here. + return parseWorldString(content, mRootToWorldPath); } simulation::WorldPtr DartLoader::parseWorldString( - const std::string& _urdfString, const std::string& _urdfFileDirectory) + const std::string& _urdfString, const std::string& _baseUri, + const utils::ResourceRetrieverPtr& _resourceRetriever) { if(_urdfString.empty()) { @@ -135,15 +135,19 @@ simulation::WorldPtr DartLoader::parseWorldString( return nullptr; } - mRootToWorldPath = _urdfFileDirectory; + mRootToWorldPath = _baseUri; std::shared_ptr worldInterface = urdf::parseWorldURDF(_urdfString, mRootToWorldPath); if(!worldInterface) - return nullptr; + { + dtwarn << "[DartLoader::parseWorldString] Failed loading URDF.\n"; + return nullptr; + } // Store paths from world to entities + // TODO: Pass the ResourceRetriever down here. parseWorldToEntityPaths(_urdfString); simulation::WorldPtr world(new simulation::World()); @@ -343,33 +347,21 @@ bool DartLoader::createSkeletonRecursive( /** * @function readXml */ -std::string DartLoader::readFileToString(std::string _xmlFile) { - - std::string xml_string; - std::ifstream xml_file(_xmlFile.c_str()); +bool DartLoader::readFileToString( + const utils::ResourceRetrieverPtr& _resourceRetriever, + const std::string &_uri, + std::string &_output) +{ + const ResourcePtr resource = _resourceRetriever->retrieve(_uri); + if (!resource) + return false; - if(!xml_file.is_open()) - { - dtwarn << "[DartLoader::readFileToString] Failed to open file '" << _xmlFile << "'! " - << "Check whether the file exists and has appropriate permissions.\n"; - return xml_string; - } - - // Read xml - while(xml_file.good()) { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); + // Safe because std::string is guaranteed to be contiguous in C++11. + const size_t size = resource->getFileSize(); + _output.resize(size); + resource->read(&_output.front(), size, 1); - if(xml_string.empty()) - { - dtwarn << "[DartLoader::readFileToString] Opened file '" << _xmlFile << "', but found it to " - << "be empty. Please make sure you provided the correct filename\n"; - } - - return xml_string; + return true; } /** @@ -577,6 +569,15 @@ dynamics::ShapePtr DartLoader::createShape( return shape; } +utils::ResourceRetrieverPtr DartLoader::getResourceRetriever( + const utils::ResourceRetrieverPtr& _resourceRetriever) +{ + if (_resourceRetriever) + return _resourceRetriever; + else + return mRetriever; +} + template dynamics::ShapePtr DartLoader::createShape( const urdf::Visual* _vizOrCol, const utils::ResourceRetrieverPtr& _resourceRetriever); diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 96d15174776c0..ee03b33965358 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -49,9 +49,7 @@ namespace utils { * @class DartLoader */ class DartLoader { - -public: - /// Constructor with the default ResourceRetriever. + public: /// Constructor with the default ResourceRetriever. DartLoader(); /// Specify the directory of a ROS package. In your URDF files, you may see @@ -80,22 +78,24 @@ class DartLoader { void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); - dynamics::SkeletonPtr readSkeleton(const std::string& _uri, - const utils::ResourceRetrieverPtr& _resourceRetriever); - /// Parse a file to produce a Skeleton - dynamics::SkeletonPtr parseSkeleton(const std::string& _urdfFileName); + dynamics::SkeletonPtr parseSkeleton( + const std::string& _uri, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a text string to produce a Skeleton - dynamics::SkeletonPtr parseSkeletonString(const std::string& _urdfString, - const std::string& _urdfFileDirectory); + dynamics::SkeletonPtr parseSkeletonString( + const std::string& _urdfString, const std::string& _baseUri, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a file to produce a World - dart::simulation::WorldPtr parseWorld(const std::string& _urdfFileName); + dart::simulation::WorldPtr parseWorld(const std::string& _uri, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a text string to produce a World - dart::simulation::WorldPtr parseWorldString(const std::string& _urdfString, - const std::string& _urdfFileDirectory); + dart::simulation::WorldPtr parseWorldString( + const std::string& _urdfString, const std::string& _baseUri, + const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); private: @@ -129,9 +129,16 @@ class DartLoader { const urdf::Link* _lk, dynamics::BodyNode::Properties *properties, const utils::ResourceRetrieverPtr& _resourceRetriever); + utils::ResourceRetrieverPtr getResourceRetriever( + const utils::ResourceRetrieverPtr& _resourceRetriever); + Eigen::Isometry3d toEigen(const urdf::Pose& _pose); Eigen::Vector3d toEigen(const urdf::Vector3& _vector); - std::string readFileToString(std::string _xmlFile); + + static bool readFileToString( + const utils::ResourceRetrieverPtr& _resourceRetriever, + const std::string &_uri, + std::string &_output); ResourceRetrieverPtr mResourceRetriever; std::map mWorld_To_Entity_Paths; diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp index deb9a33c915bd..cf367ec8b45d4 100644 --- a/unittests/testLocalResourceRetriever.cpp +++ b/unittests/testLocalResourceRetriever.cpp @@ -135,19 +135,19 @@ TEST(LocalResourceRetriever, retrieve_ResourceOperations) ASSERT_EQ(0, resource->read(buffer.data(), content.size() + 1, 1)); // Reading should only return full blocks. - buffer.assign('\0', buffer.size()); + buffer.assign(buffer.size(), '\0'); ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); ASSERT_EQ(1, resource->read(buffer.data(), 8, 1)); EXPECT_STREQ(content.substr(0, 8).c_str(), buffer.data()); // Reading multiple blocks - buffer.assign('\0', buffer.size()); + buffer.assign(buffer.size(), '\0'); ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); ASSERT_EQ(2, resource->read(buffer.data(), 4, 2)); EXPECT_STREQ(content.substr(0, 8).c_str(), buffer.data()); // Reading the whole file at once. - buffer.assign('\0', buffer.size()); + buffer.assign(buffer.size(), '\0'); ASSERT_TRUE(resource->seek(0, Resource::SEEKTYPE_SET)); ASSERT_EQ(1, resource->read(buffer.data(), content.size(), 1)); EXPECT_STREQ(content.c_str(), buffer.data()); From 224ff23b7bedc8176f1c127a80ace04ffb55bd56 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 13:53:54 -0500 Subject: [PATCH 11/55] Tested RelativeResourceRetriever. --- dart/utils/LocalResource.cpp | 2 +- dart/utils/LocalResource.h | 2 +- dart/utils/Resource.h | 4 +- unittests/TestHelpers.h | 64 ++++++++++ unittests/testLocalResourceRetriever.cpp | 2 +- unittests/testPackageResourceRetriever.cpp | 63 ---------- unittests/testRelativeResourceRetriever.cpp | 124 ++++++++++++++++++++ 7 files changed, 192 insertions(+), 69 deletions(-) create mode 100644 unittests/testRelativeResourceRetriever.cpp diff --git a/dart/utils/LocalResource.cpp b/dart/utils/LocalResource.cpp index ff42748d9e402..e52502f883402 100644 --- a/dart/utils/LocalResource.cpp +++ b/dart/utils/LocalResource.cpp @@ -89,7 +89,7 @@ size_t LocalResource::tell() return offset; } -bool LocalResource::seek(size_t _offset, SeekType _mode) +bool LocalResource::seek(ptrdiff_t _offset, SeekType _mode) { int origin; switch(_mode) diff --git a/dart/utils/LocalResource.h b/dart/utils/LocalResource.h index b11ff59e56ba3..ab26edf47d3d8 100644 --- a/dart/utils/LocalResource.h +++ b/dart/utils/LocalResource.h @@ -59,7 +59,7 @@ class LocalResource : public virtual Resource size_t tell() override; // Documentation inherited. - bool seek(size_t _origin, SeekType _mode) override; + bool seek(ptrdiff_t _origin, SeekType _mode) override; // Documentation inherited. size_t read(void *_buffer, size_t _size, size_t _count) override; diff --git a/dart/utils/Resource.h b/dart/utils/Resource.h index 9adf7867f524e..549b049659fd5 100644 --- a/dart/utils/Resource.h +++ b/dart/utils/Resource.h @@ -50,15 +50,13 @@ class Resource SEEKTYPE_SET }; - static const size_t SIZE_UNKNOWN = -1; - virtual ~Resource() = default; virtual size_t getFileSize() = 0; virtual size_t tell() = 0; - virtual bool seek(size_t _offset, SeekType _origin) = 0; + virtual bool seek(ptrdiff_t _offset, SeekType _origin) = 0; virtual size_t read(void *_buffer, size_t _size, size_t _count) = 0; }; diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 171cf3fa64ad9..36ccdc506215f 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -45,6 +45,7 @@ #ifndef DART_UNITTESTS_TEST_HELPERS_H #define DART_UNITTESTS_TEST_HELPERS_H +#include #include #include #include "dart/math/Geometry.h" @@ -52,6 +53,7 @@ #include "dart/collision/CollisionDetector.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/simulation/World.h" +#include "dart/utils/ResourceRetriever.h" using namespace Eigen; using namespace dart::math; @@ -374,4 +376,66 @@ SkeletonPtr createBox( return box; } +//============================================================================== +struct TestResource : public dart::utils::Resource +{ + size_t getFileSize() override + { + return 0; + } + + size_t tell() override + { + return 0; + } + + bool seek(ptrdiff_t _offset, SeekType _origin) override + { + return false; + } + + size_t read(void *_buffer, size_t _size, size_t _count) override + { + return 0; + } +}; + +//============================================================================== +struct PresentResourceRetriever : public dart::utils::ResourceRetriever +{ + bool exists(const std::string& _uri) override + { + mExists.push_back(_uri); + return true; + } + + dart::utils::ResourcePtr retrieve(const std::string& _uri) override + { + mRetrieve.push_back(_uri); + return std::make_shared(); + } + + std::vector mExists; + std::vector mRetrieve; +}; + +//============================================================================== +struct AbsentResourceRetriever : public dart::utils::ResourceRetriever +{ + bool exists(const std::string& _uri) override + { + mExists.push_back(_uri); + return false; + } + + dart::utils::ResourcePtr retrieve(const std::string& _uri) override + { + mRetrieve.push_back(_uri); + return nullptr; + } + + std::vector mExists; + std::vector mRetrieve; +}; + #endif // #ifndef DART_UNITTESTS_TEST_HELPERS_H diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp index cf367ec8b45d4..dd1803d09384a 100644 --- a/unittests/testLocalResourceRetriever.cpp +++ b/unittests/testLocalResourceRetriever.cpp @@ -127,7 +127,7 @@ TEST(LocalResourceRetriever, retrieve_ResourceOperations) EXPECT_EQ(content.size(), resource->tell()); // TODO: SEEKTYPE_END should require negative input. - ASSERT_TRUE(resource->seek(3, Resource::SEEKTYPE_END)); + ASSERT_TRUE(resource->seek(-3, Resource::SEEKTYPE_END)); EXPECT_EQ(content.size() - 3, resource->tell()); // Reading a block that's too large should do nothing. diff --git a/unittests/testPackageResourceRetriever.cpp b/unittests/testPackageResourceRetriever.cpp index dc4146bae0750..18a4e7d06aaa5 100644 --- a/unittests/testPackageResourceRetriever.cpp +++ b/unittests/testPackageResourceRetriever.cpp @@ -43,69 +43,6 @@ using dart::utils::ResourcePtr; using dart::utils::ResourceRetriever; using dart::utils::PackageResourceRetriever; -namespace { - -struct TestResource : public Resource -{ - size_t getFileSize() override - { - return 0; - } - - size_t tell() override - { - return 0; - } - - bool seek(size_t _offset, SeekType _origin) override - { - return false; - } - - size_t read(void *_buffer, size_t _size, size_t _count) override - { - return 0; - } -}; - -struct PresentResourceRetriever : public ResourceRetriever -{ - bool exists(const std::string& _uri) override - { - mExists.push_back(_uri); - return true; - } - - ResourcePtr retrieve(const std::string& _uri) override - { - mRetrieve.push_back(_uri); - return std::make_shared(); - } - - std::vector mExists; - std::vector mRetrieve; -}; - -struct AbsentResourceRetriever : public ResourceRetriever -{ - bool exists(const std::string& _uri) override - { - mExists.push_back(_uri); - return false; - } - - ResourcePtr retrieve(const std::string& _uri) override - { - mRetrieve.push_back(_uri); - return nullptr; - } - - std::vector mExists; - std::vector mRetrieve; -}; - -} - TEST(PackageResourceRetriever, exists_UnableToResolve_ReturnsFalse) { auto mockRetriever = std::make_shared(); diff --git a/unittests/testRelativeResourceRetriever.cpp b/unittests/testRelativeResourceRetriever.cpp new file mode 100644 index 0000000000000..0a07bfb55c8e1 --- /dev/null +++ b/unittests/testRelativeResourceRetriever.cpp @@ -0,0 +1,124 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 +#include "dart/utils/RelativeResourceRetriever.h" +#include "TestHelpers.h" + +using dart::utils::Resource; +using dart::utils::RelativeResourceRetriever; + +TEST(RelativeResourceRetriever, exists_AbsolutePath_CallsDelegate) +{ + auto presentDelegate = std::make_shared(); + RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); + + EXPECT_TRUE(presentRetriever.exists("file:///absolute/path")); + EXPECT_TRUE(presentDelegate->mRetrieve.empty()); + ASSERT_EQ(1, presentDelegate->mExists.size()); + EXPECT_EQ("file:///absolute/path", presentDelegate->mExists.front()); + + auto absentDelegate = std::make_shared(); + RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); + + EXPECT_FALSE(absentRetriever.exists("file:///absolute/path")); + EXPECT_TRUE(absentDelegate->mRetrieve.empty()); + ASSERT_EQ(1, absentDelegate->mExists.size()); + EXPECT_EQ("file:///absolute/path", absentDelegate->mExists.front()); +} + +TEST(RelativeResourceRetriever, exists_RelativePath_Appends) +{ + auto presentDelegate = std::make_shared(); + RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); + + EXPECT_TRUE(presentRetriever.exists("relative/path")); + EXPECT_TRUE(presentDelegate->mRetrieve.empty()); + ASSERT_EQ(1, presentDelegate->mExists.size()); + EXPECT_EQ("file:///prefix/relative/path", presentDelegate->mExists.front()); + + auto absentDelegate = std::make_shared(); + RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); + + EXPECT_FALSE(absentRetriever.exists("relative/path")); + EXPECT_TRUE(absentDelegate->mRetrieve.empty()); + ASSERT_EQ(1, absentDelegate->mExists.size()); + EXPECT_EQ("file:///prefix/relative/path", absentDelegate->mExists.front()); +} + +TEST(RelativeResourceRetriever, retrieve_AbsolutePath_CallsDelegate) +{ + auto presentDelegate = std::make_shared(); + RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); + + EXPECT_TRUE(nullptr != presentRetriever.retrieve("file:///absolute/path")); + EXPECT_TRUE(presentDelegate->mExists.empty()); + ASSERT_EQ(1, presentDelegate->mRetrieve.size()); + EXPECT_EQ("file:///absolute/path", presentDelegate->mRetrieve.front()); + + auto absentDelegate = std::make_shared(); + RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); + + EXPECT_EQ(nullptr, absentRetriever.retrieve("file:///absolute/path")); + EXPECT_TRUE(absentDelegate->mExists.empty()); + ASSERT_EQ(1, absentDelegate->mRetrieve.size()); + EXPECT_EQ("file:///absolute/path", absentDelegate->mRetrieve.front()); +} + +TEST(RelativeResourceRetriever, retrieve_RelativePath_Appends) +{ + auto presentDelegate = std::make_shared(); + RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); + + EXPECT_TRUE(nullptr != presentRetriever.retrieve("relative/path")); + EXPECT_TRUE(presentDelegate->mExists.empty()); + ASSERT_EQ(1, presentDelegate->mRetrieve.size()); + EXPECT_EQ("file:///prefix/relative/path", presentDelegate->mRetrieve.front()); + + auto absentDelegate = std::make_shared(); + RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); + + EXPECT_EQ(nullptr, absentRetriever.retrieve("relative/path")); + EXPECT_TRUE(absentDelegate->mExists.empty()); + ASSERT_EQ(1, absentDelegate->mRetrieve.size()); + EXPECT_EQ("file:///prefix/relative/path", absentDelegate->mRetrieve.front()); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 23eaddcf92f2fa2f6e1bb6282466e9ba22c4322e Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 18:25:49 -0500 Subject: [PATCH 12/55] Added URI parsing utilities. --- CMakeLists.txt | 2 +- dart/utils/UriUtils.cpp | 234 ++++++++++++++++++++++++++++++++++ dart/utils/UriUtils.h | 100 +++++++++++++++ unittests/testUriHelpers.cpp | 239 +++++++++++++++++++++++++++++++++++ 4 files changed, 574 insertions(+), 1 deletion(-) create mode 100644 dart/utils/UriUtils.cpp create mode 100644 dart/utils/UriUtils.h create mode 100644 unittests/testUriHelpers.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9075148d6185a..624a394568906 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,7 +158,7 @@ endif() add_definitions(-DBOOST_TEST_DYN_LINK) set(Boost_USE_MULTITHREADED ON) set(Boost_USE_STATIC_RUNTIME OFF) -find_package(Boost ${DART_MIN_BOOST_VERSION} COMPONENTS system QUIET) +find_package(Boost ${DART_MIN_BOOST_VERSION} COMPONENTS regex system QUIET) if(Boost_FOUND) message(STATUS "Looking for Boost - ${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION} found") else() diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp new file mode 100644 index 0000000000000..66bae974111ca --- /dev/null +++ b/dart/utils/UriUtils.cpp @@ -0,0 +1,234 @@ +#include +#include +#include +#include "dart/common/Console.h" +#include "UriUtils.h" + +// std::regex is only implemented in GCC 4.9 and above; i.e. libstdc++ 6.0.20 +// or above. In fact, it contains major bugs in GCC 4.8 [1]. There is no +// reliable way to test the version of libstdc++ when building with Clang [2], +// so we'll fall back on Boost.Regex when using libstdc++. +// +// [1] http://stackoverflow.com/a/12665408/111426 +// [2] http://stackoverflow.com/q/31506594/111426 +// +#ifdef __GLIBCXX__ + +#include + +using boost::regex; +using boost::regex_match; +using boost::smatch; +using boost::ssub_match; + +#else + +#include + +using std::regex; +using std::regex_match; +using std::smatch; +using std::ssub_match; + +#endif + +namespace dart { +namespace utils { + +/* + * UriComponent + */ +UriComponent::UriComponent() +{ + reset(); +} + +UriComponent::UriComponent(reference_const_type _value) +{ + assign(_value); +} + +UriComponent::operator bool() const +{ + return mExists; +} + +bool UriComponent::operator !() const +{ + return !mExists; +} + +auto UriComponent::operator =(reference_const_type _value) -> UriComponent& +{ + assign(_value); + return *this; +} + +auto UriComponent::operator*() -> reference_type +{ + return get(); +} + +auto UriComponent::operator*() const -> reference_const_type +{ + return get(); +} + +auto UriComponent::operator->() -> pointer_type +{ + return &get(); +} + +auto UriComponent::operator->() const -> pointer_const_type +{ + return &get(); +} + +void UriComponent::assign(reference_const_type _value) +{ + mExists = true; + mValue = _value; +} + +void UriComponent::reset() +{ + mExists = false; +} + +auto UriComponent::get() -> reference_type +{ + assert(mExists); + return mValue; +} + +auto UriComponent::get() const -> reference_const_type +{ + assert(mExists); + return mValue; +} + +auto UriComponent::get_value_or(reference_type _default) -> reference_type +{ + if(mExists) + return mValue; + else + return _default; +} + +auto UriComponent::get_value_or(reference_const_type _default) const -> reference_const_type +{ + if(mExists) + return mValue; + else + return _default; +} + + +/* + * Uri + */ +void Uri::clear() +{ + mScheme.reset(); + mAuthority.reset(); + mPath.reset(); + mQuery.reset(); + mFragment.reset(); +} + +bool Uri::fromString(const std::string& _input) +{ + // This is regex is from Appendix B of RFC 3986. + static regex uriRegex( + R"END(^(([^:/?#]+):)?(//([^/?#]*))?([^?#]*)(\?([^#]*))?(#(.*))?)END", + regex::extended); + static const size_t schemeIndex = 2; + static const size_t authorityIndex = 4; + static const size_t pathIndex = 5; + static const size_t queryIndex = 7; + static const size_t fragmentIndex = 9; + + clear(); + + smatch matches; + if(!regex_match(_input, matches, uriRegex)) + return false; + + assert(matches.size() > schemeIndex); + const ssub_match& schemeMatch = matches[schemeIndex]; + if(schemeMatch.matched) + mScheme = schemeMatch; + + assert(matches.size() > authorityIndex); + const ssub_match& authorityMatch = matches[authorityIndex]; + if(authorityMatch.matched) + mAuthority = authorityMatch; + + assert(matches.size() > pathIndex); + const ssub_match& pathMatch = matches[pathIndex]; + if(pathMatch.matched) + mPath = pathMatch; + + assert(matches.size() > queryIndex); + const ssub_match& queryMatch = matches[queryIndex]; + if(queryMatch.matched) + mQuery = queryMatch; + + assert(matches.size() > fragmentIndex); + const ssub_match& fragmentMatch = matches[fragmentIndex]; + if (fragmentMatch.matched) + mFragment = fragmentMatch; + + return true; +} + +std::string Uri::toString() const +{ + // This function implements the pseudo-code from Section 5.3 of RFC 3986. + std::stringstream output; + + if(mScheme) + output << *mScheme << ":"; + + if(mAuthority) + output << "//" << *mAuthority; + + output << mPath.get_value_or(""); + + if(mQuery) + output << "?" << *mQuery; + + if(mFragment) + output << "#" << *mFragment; + + return output.str(); +} + +bool Uri::fromStringOrPath(const std::string& _input) +{ + if (!fromString(_input)) + return false; + + // Assume that any URI without a scheme is a path. + if (!mScheme && mPath) + { + mScheme = "file"; + + // Replace backslashes (from Windows paths) with forward slashes. + std::replace(std::begin(*mPath), std::end(*mPath), '\\', '/'); + } + + return true; +} + +std::string Uri::getUri(const std::string& _input) +{ + Uri uri; + if(uri.fromStringOrPath(_input)) + return uri.toString(); + else + return ""; +} + +} // namespace utils +} // namespace dart diff --git a/dart/utils/UriUtils.h b/dart/utils/UriUtils.h new file mode 100644 index 0000000000000..f492121b366ff --- /dev/null +++ b/dart/utils/UriUtils.h @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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. + */ +#ifndef DART_UTILS_URIUTILS_H_ +#define DART_UTILS_URIUTILS_H_ + +namespace dart { +namespace utils { + +class UriComponent { +public: + using value_type = std::string; + using reference_type = value_type&; + using reference_const_type = const value_type&; + using pointer_type = value_type*; + using pointer_const_type = const value_type*; + + UriComponent(); + UriComponent(reference_const_type _value); + + operator bool() const; + + bool operator !() const; + + UriComponent& operator =(reference_const_type _value); + + reference_type operator *(); + reference_const_type operator *() const; + + pointer_type operator ->(); + pointer_const_type operator ->() const; + + void assign(reference_const_type _value); + void reset(); + + reference_type get(); + reference_const_type get() const; + + reference_type get_value_or(reference_type _default); + reference_const_type get_value_or(reference_const_type _default) const; + +private: + bool mExists; + std::string mValue; +}; + + +class Uri { +public: + UriComponent mScheme; + UriComponent mAuthority; + UriComponent mPath; + UriComponent mQuery; + UriComponent mFragment; + + void clear(); + + bool fromString(const std::string& _input); + bool fromStringOrPath(const std::string& _input); + + std::string toString() const; + + static std::string getUri(const std::string& _input); +}; + +} // namespace utils +} // namespace dart + +#endif // ifndef DART_UTILS_URIUTILS_H_ diff --git a/unittests/testUriHelpers.cpp b/unittests/testUriHelpers.cpp new file mode 100644 index 0000000000000..fa316d9bda82b --- /dev/null +++ b/unittests/testUriHelpers.cpp @@ -0,0 +1,239 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 +#include "dart/utils/UriUtils.h" +#include "TestHelpers.h" + +using dart::utils::Uri; + +TEST(UriHelpers, fromString_ValidUri_ReturnsTrue) +{ + // These examples are from Section 1.1.2 of RFC 3986. + Uri uri; + + ASSERT_TRUE(uri.fromString("ftp://ftp.is.co.za/rfc/rfc1808.txt")); + ASSERT_TRUE(uri.mScheme); + ASSERT_TRUE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + EXPECT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("ftp", *uri.mScheme); + EXPECT_EQ("ftp.is.co.za", *uri.mAuthority); + EXPECT_EQ("/rfc/rfc1808.txt", *uri.mPath); + + ASSERT_TRUE(uri.fromString("http://www.ietf.org/rfc/rfc2396.txt")); + ASSERT_TRUE(uri.mScheme); + ASSERT_TRUE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + EXPECT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("http", *uri.mScheme); + EXPECT_EQ("www.ietf.org", *uri.mAuthority); + EXPECT_EQ("/rfc/rfc2396.txt", *uri.mPath); + + ASSERT_TRUE(uri.fromString("ldap://[2001:db8::7]/c=GB?objectClass?one")); + ASSERT_TRUE(uri.mScheme); + ASSERT_TRUE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_TRUE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("ldap", *uri.mScheme); + EXPECT_EQ("[2001:db8::7]", *uri.mAuthority); + EXPECT_EQ("/c=GB", *uri.mPath); + EXPECT_EQ("objectClass?one", *uri.mQuery); + + ASSERT_TRUE(uri.fromString("mailto:John.Doe@example.com")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("mailto", *uri.mScheme); + EXPECT_EQ("John.Doe@example.com", *uri.mPath); + + ASSERT_TRUE(uri.fromString("news:comp.infosystems.www.servers.unix")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("news", *uri.mScheme); + EXPECT_EQ("comp.infosystems.www.servers.unix", *uri.mPath); + + ASSERT_TRUE(uri.fromString("tel:+1-816-555-1212")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("tel", *uri.mScheme); + EXPECT_EQ("+1-816-555-1212", *uri.mPath); + + ASSERT_TRUE(uri.fromString("telnet://192.0.2.16:80/")); + ASSERT_TRUE(uri.mScheme); + ASSERT_TRUE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("telnet", *uri.mScheme); + EXPECT_EQ("192.0.2.16:80", *uri.mAuthority); + EXPECT_EQ("/", *uri.mPath); + + ASSERT_TRUE(uri.fromString("urn:oasis:names:specification:docbook:dtd:xml:4.1.2")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_FALSE(uri.mAuthority); + EXPECT_EQ("oasis:names:specification:docbook:dtd:xml:4.1.2", *uri.mPath); +} + +TEST(UriHelpers, fromStringOrPath_PathNotUri_ReturnsOnlyPath) +{ + Uri uri; + + ASSERT_TRUE(uri.fromStringOrPath("foo")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("foo", *uri.mPath); + + ASSERT_TRUE(uri.fromStringOrPath("foo/")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("foo/", *uri.mPath); + + ASSERT_TRUE(uri.fromStringOrPath("foo/bar")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("foo/bar", *uri.mPath); + + ASSERT_TRUE(uri.fromStringOrPath("/foo")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("/foo", *uri.mPath); + + ASSERT_TRUE(uri.fromStringOrPath("/foo/")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("/foo/", *uri.mPath); + + ASSERT_TRUE(uri.fromStringOrPath("/foo/bar")); + ASSERT_TRUE(uri.mScheme); + ASSERT_FALSE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + ASSERT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("file", *uri.mScheme); + EXPECT_EQ("/foo/bar", *uri.mPath); +} + +TEST(UriHelpers, fromStringOrPath_InputIsUri_DoesNotChange) +{ + Uri uri; + + ASSERT_TRUE(uri.fromStringOrPath("ftp://ftp.is.co.za/rfc/rfc1808.txt")); + ASSERT_TRUE(uri.mScheme); + ASSERT_TRUE(uri.mAuthority); + ASSERT_TRUE(uri.mPath); + EXPECT_FALSE(uri.mQuery); + EXPECT_FALSE(uri.mFragment); + EXPECT_EQ("ftp", *uri.mScheme); + EXPECT_EQ("ftp.is.co.za", *uri.mAuthority); + EXPECT_EQ("/rfc/rfc1808.txt", *uri.mPath); +} + +TEST(UriHelpers, getUri_InputIsUri_DoesNotChange) +{ + std::vector testUris = { + "ftp://ftp.is.co.za/rfc/rfc1808.txt", + "http://www.ietf.org/rfc/rfc2396.txt", + "ldap://[2001:db8::7]/c=GB?objectClass?one", + "mailto:John.Doe@example.com", + "news:comp.infosystems.www.servers.unix", + "tel:+1-816-555-1212", + "telnet://192.0.2.16:80/", + "urn:oasis:names:specification:docbook:dtd:xml:4.1.2" + }; + + for (const std::string& testUri : testUris) + EXPECT_EQ(testUri, Uri::getUri(testUri)); +} + +TEST(UriHelpers, getUri_InputIsPath_AppendsFileSchema) +{ + std::vector testPaths = { + "foo", + "foo/", + "foo/bar", + "/foo", + "/foo/", + "/foo/bar" + }; + + for(const std::string& testPath : testPaths) + { + const std::string testUri = "file://" + testUri; + EXPECT_EQ(testUri, Uri::getUri(testUri)); + } +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1853280c21bac9f94cc37e71299bafa010bb444a Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 19:48:10 -0500 Subject: [PATCH 13/55] Better support for relative URIs. --- dart/utils/PackageResourceRetriever.cpp | 1 + dart/utils/UriUtils.cpp | 142 ++++++++++++++++++++ dart/utils/UriUtils.h | 17 +++ dart/utils/urdf/DartLoader.cpp | 110 ++++++++++----- dart/utils/urdf/DartLoader.h | 23 ++-- unittests/testRelativeResourceRetriever.cpp | 124 ----------------- 6 files changed, 254 insertions(+), 163 deletions(-) delete mode 100644 unittests/testRelativeResourceRetriever.cpp diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 697bc409d82fb..6f1f8b84d9403 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -5,6 +5,7 @@ #include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/ResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" + namespace dart { namespace utils { diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index 66bae974111ca..aebc7e4d4866c 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -136,6 +136,84 @@ void Uri::clear() mFragment.reset(); } +void Uri::append(const std::string& _relativePath) +{ + if (mPath) + mPath = *mPath + "/" + _relativePath; + else + mPath = _relativePath; +} + +bool Uri::isPath() const +{ + return !mScheme && mPath; +} + +bool Uri::isRelativePath() const +{ + return isPath() && !mPath->empty() && mPath->front() != '/'; +} + +bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative) +{ + Uri relativeUri; + relativeUri.mPath = _relative; + + return fromRelativeUri(_base, relativeUri, true); +} + +bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) +{ + assert(_base.mPath && "The path component is always defined."); + assert(_relative.mPath && "The path component is always defined."); + + // This directly implements the psueocode in Section 5.2.2. of RFC 3986. + if(_relative.mScheme || (!_strict && _relative.mScheme == _base.mScheme)) + { + mScheme = _relative.mScheme; + mAuthority = _relative.mAuthority; + mPath = removeDotSegments(*_relative.mPath); + mQuery = _relative.mQuery; + } + else + { + if(_relative.mAuthority) + { + mAuthority = _relative.mAuthority; + mPath = removeDotSegments(*_relative.mPath); + mQuery = _relative.mQuery; + } + else + { + if(_relative.mPath->empty()) + { + mPath = _base.mPath; + + if(_relative.mQuery) + mQuery = _relative.mQuery; + else + mQuery = _base.mQuery; + } + else + { + if(_relative.mPath->front() == '/') + mPath = removeDotSegments(*_relative.mPath); + else + mPath = removeDotSegments(mergePaths(_base, _relative)); + + mQuery = _relative.mQuery; + } + + mAuthority = _base.mAuthority; + } + + mScheme = _base.mScheme; + } + + mFragment = _relative.mFragment; + return true; +} + bool Uri::fromString(const std::string& _input) { // This is regex is from Appendix B of RFC 3986. @@ -230,5 +308,69 @@ std::string Uri::getUri(const std::string& _input) return ""; } +std::string Uri::mergePaths(const Uri& _base, const Uri& _relative) +{ + assert(_base.mPath && "The path component is always defined."); + assert(_relative.mPath && "The path component is always defined."); + + // This directly implements the logic from Section 5.2.3. of RFC 3986. + if(_base.mAuthority && _base.mPath->empty()) + return "/" + *_relative.mPath; + else + { + const size_t index = _base.mPath->find_last_of('/'); + if(index != std::string::npos) + return _base.mPath->substr(0, index + 1) + *_relative.mPath; + else + return *_relative.mPath; + } +} + +std::string Uri::removeDotSegments(const std::string& _path) +{ + /* + The pseudocode also refers to a "remove_dot_segments" routine for + interpreting and removing the special "." and ".." complete path + segments from a referenced path. This is done after the path is + extracted from a reference, whether or not the path was relative, in + order to remove any invalid or extraneous dot-segments prior to + forming the target URI. Although there are many ways to accomplish + this removal process, we describe a simple method using two string + buffers. + + 1. The input buffer is initialized with the now-appended path + components and the output buffer is initialized to the empty + string. + + 2. While the input buffer is not empty, loop as follows: + + A. If the input buffer begins with a prefix of "../" or "./", + then remove that prefix from the input buffer; otherwise, + + B. if the input buffer begins with a prefix of "/./" or "/.", + where "." is a complete path segment, then replace that + prefix with "/" in the input buffer; otherwise, + + C. if the input buffer begins with a prefix of "/../" or "/..", + where ".." is a complete path segment, then replace that + prefix with "/" in the input buffer and remove the last + segment and its preceding "/" (if any) from the output + buffer; otherwise, + + D. if the input buffer consists only of "." or "..", then remove + that from the input buffer; otherwise, + + E. move the first path segment in the input buffer to the end of + the output buffer, including the initial "/" character (if + any) and any subsequent characters up to, but not including, + the next "/" character or the end of the input buffer. + + 3. Finally, the output buffer is returned as the result of + remove_dot_segments. + */ + assert(false && "Not implemented."); + return ""; +} + } // namespace utils } // namespace dart diff --git a/dart/utils/UriUtils.h b/dart/utils/UriUtils.h index f492121b366ff..8c419140fb4d0 100644 --- a/dart/utils/UriUtils.h +++ b/dart/utils/UriUtils.h @@ -86,12 +86,29 @@ class Uri { void clear(); + bool isPath() const; + bool isRelativePath() const; + + void append(const std::string& _relativePath); + void transform(); + + bool fromString(const std::string& _input); bool fromStringOrPath(const std::string& _input); + bool fromRelativeUri(const Uri& _base, const std::string& _relative); + bool fromRelativeUri(const Uri& _base, const Uri& _relative, + bool _strict = false); + std::string toString() const; static std::string getUri(const std::string& _input); + +private: + // These are helper functions for implementing transform(); + + static std::string mergePaths(const Uri& _base, const Uri& _relative); + static std::string removeDotSegments(const std::string& _path); }; } // namespace utils diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index b9634a2b06313..6b3116ef0f5fa 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -61,8 +61,13 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( const utils::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); - // TODO: Support for Windows-style paths? - mRootToSkelPath = getBasePath(_uri); + Uri uri; + if(!uri.fromString(_uri)) + { + dtwarn << "[DartLoader::parseSkeleton] Failed parsing URI: " + << _uri << "\n"; + return nullptr; + } std::string content; if (!readFileToString(resourceRetriever, _uri, content)) @@ -77,12 +82,15 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( return nullptr; } - // TODO: Pass the ResourceRetriever down here. - return modelInterfaceToSkeleton(urdfInterface.get(), resourceRetriever); + // TODO: Extract the base path from _uri. + Uri baseUri; + + return modelInterfaceToSkeleton( + urdfInterface.get(), baseUri, resourceRetriever); } dynamics::SkeletonPtr DartLoader::parseSkeletonString( - const std::string& _urdfString, const std::string& _baseUri, + const std::string& _urdfString, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { if(_urdfString.empty()) @@ -92,8 +100,6 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( return nullptr; } - mRootToSkelPath = _baseUri; - ModelInterfacePtr urdfInterface = urdf::parseURDF(_urdfString); if(!urdfInterface) { @@ -103,7 +109,7 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( // TODO: Pass the ResourceRetriever down here. return modelInterfaceToSkeleton( - urdfInterface.get(), getResourceRetriever(_resourceRetriever)); + urdfInterface.get(), _baseUri, getResourceRetriever(_resourceRetriever)); } simulation::WorldPtr DartLoader::parseWorld( @@ -113,19 +119,27 @@ simulation::WorldPtr DartLoader::parseWorld( const utils::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); + Uri uri; + if(!uri.fromString(_uri)) + { + dtwarn << "[DartLoader::parseSkeleton] Failed parsing URI: " + << _uri << "\n"; + return nullptr; + } + std::string content; if (!readFileToString(resourceRetriever, _uri, content)) return nullptr; - // TODO: Support for Windows-style paths? - mRootToSkelPath = getBasePath(_uri); + // TODO: Extract the base path from _uri. + Uri baseUri; // TODO: Pass the ResourceRetriever down here. - return parseWorldString(content, mRootToWorldPath); + return parseWorldString(content, baseUri); } simulation::WorldPtr DartLoader::parseWorldString( - const std::string& _urdfString, const std::string& _baseUri, + const std::string& _urdfString, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { if(_urdfString.empty()) @@ -135,8 +149,6 @@ simulation::WorldPtr DartLoader::parseWorldString( return nullptr; } - mRootToWorldPath = _baseUri; - std::shared_ptr worldInterface = urdf::parseWorldURDF(_urdfString, mRootToWorldPath); @@ -150,7 +162,7 @@ simulation::WorldPtr DartLoader::parseWorldString( // TODO: Pass the ResourceRetriever down here. parseWorldToEntityPaths(_urdfString); - simulation::WorldPtr world(new simulation::World()); + simulation::WorldPtr world(new simulation::World); for(size_t i = 0; i < worldInterface->models.size(); ++i) { @@ -167,7 +179,7 @@ simulation::WorldPtr DartLoader::parseWorldString( mRootToSkelPath = mRootToWorldPath + it->second; dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( - worldInterface->models[i].model.get(), mRetriever); + worldInterface->models[i].model.get(), _baseUri, mRetriever); if(!skeleton) { @@ -262,6 +274,7 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) */ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( const urdf::ModelInterface* _model, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(_model->getName()); @@ -278,12 +291,12 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( { root = root->child_links[0].get(); dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties, _resourceRetriever)) + if (!createDartNodeProperties(root, &rootProperties, _baseUri, _resourceRetriever)) return nullptr; rootNode = createDartJointAndNode( root->parent_joint.get(), rootProperties, nullptr, skeleton, - _resourceRetriever); + _baseUri, _resourceRetriever); if(nullptr == rootNode) { dterr << "[DartLoader::modelInterfaceToSkeleton] Failed to create root node!\n"; @@ -294,7 +307,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( else { dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties, _resourceRetriever)) + if (!createDartNodeProperties(root, &rootProperties, _baseUri, _resourceRetriever)) return nullptr; std::pair pair = @@ -308,8 +321,9 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( for(size_t i = 0; i < root->child_links.size(); i++) { - if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), - *rootNode, _resourceRetriever)) + if (!createSkeletonRecursive( + skeleton, root->child_links[i].get(), *rootNode, + _baseUri, _resourceRetriever)) return nullptr; } @@ -321,15 +335,16 @@ bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parentNode, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::BodyNode::Properties properties; - if (!createDartNodeProperties(_lk, &properties, _resourceRetriever)) + if (!createDartNodeProperties(_lk, &properties, _baseUri, _resourceRetriever)) return false; dynamics::BodyNode* node = createDartJointAndNode( _lk->parent_joint.get(), properties, &_parentNode, _skel, - _resourceRetriever); + _baseUri, _resourceRetriever); if(!node) return false; @@ -337,7 +352,7 @@ bool DartLoader::createSkeletonRecursive( for(size_t i = 0; i < _lk->child_links.size(); ++i) { if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), *node, - _resourceRetriever)) + _baseUri, _resourceRetriever)) return false; } return true; @@ -372,6 +387,7 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::Joint::Properties basicProperties; @@ -461,6 +477,7 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( bool DartLoader::createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties *node, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { node->mName = _lk->name; @@ -486,8 +503,10 @@ bool DartLoader::createDartNodeProperties( // Set visual information for(size_t i = 0; i < _lk->visual_array.size(); i++) { - if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get(), - _resourceRetriever)) + dynamics::ShapePtr shape = createShape( + _lk->visual_array[i].get(), _baseUri, _resourceRetriever); + + if(shape) node->mVizShapes.push_back(shape); else return false; @@ -495,8 +514,10 @@ bool DartLoader::createDartNodeProperties( // Set collision information for(size_t i = 0; i < _lk->collision_array.size(); i++) { - if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get(), - _resourceRetriever)) + dynamics::ShapePtr shape = createShape( + _lk->collision_array[i].get(), _baseUri, _resourceRetriever); + + if (shape) node->mColShapes.push_back(shape); else return false; @@ -521,6 +542,7 @@ void setMaterial(dynamics::ShapePtr _shape, const urdf::Collision* _col) { template dynamics::ShapePtr DartLoader::createShape( const VisualOrCollision* _vizOrCol, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { dynamics::ShapePtr shape; @@ -546,14 +568,38 @@ dynamics::ShapePtr DartLoader::createShape( // Mesh else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { - const std::string& uri = mesh->filename; - const aiScene* scene = dynamics::MeshShape::loadMesh(uri, _resourceRetriever); + // Parse the URI. This is necessary to detect whether it is a full URI, an + // absolute path, or a relative path. + Uri meshUri; + if(!meshUri.fromStringOrPath(mesh->filename)) + { + dtwarn << "[DartLoader::createShape] Failed parsing mesh URI: " + << mesh->filename << "\n"; + return nullptr; + } + + // Resolve the relative path using _baseUri as the root. + std::string resolvedUri; + if(meshUri.isRelativePath() && meshUri.mPath) + { + meshUri = _baseUri; + meshUri.append(*meshUri.mPath); + resolvedUri = meshUri.toString(); + } + else + { + resolvedUri = mesh->filename; + } + + // Load the mesh. + const aiScene* scene = dynamics::MeshShape::loadMesh( + resolvedUri, _resourceRetriever); if (!scene) return nullptr; const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shape = std::make_shared( - scale, scene, uri, true, mRetriever); + scale, scene, resolvedUri, true, mRetriever); } // Unknown geometry type else @@ -580,9 +626,11 @@ utils::ResourceRetrieverPtr DartLoader::getResourceRetriever( template dynamics::ShapePtr DartLoader::createShape( const urdf::Visual* _vizOrCol, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); template dynamics::ShapePtr DartLoader::createShape( const urdf::Collision* _vizOrCol, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); /** diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index ee03b33965358..1858e5237008f 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -19,6 +19,7 @@ #include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" #include "dart/utils/SchemaResourceRetriever.h" +#include "dart/utils/UriUtils.h" namespace urdf { @@ -85,7 +86,7 @@ class DartLoader { /// Parse a text string to produce a Skeleton dynamics::SkeletonPtr parseSkeletonString( - const std::string& _urdfString, const std::string& _baseUri, + const std::string& _urdfString, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a file to produce a World @@ -94,7 +95,7 @@ class DartLoader { /// Parse a text string to produce a World dart::simulation::WorldPtr parseWorldString( - const std::string& _urdfString, const std::string& _baseUri, + const std::string& _urdfString, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); private: @@ -106,27 +107,33 @@ class DartLoader { dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( const urdf::ModelInterface* _model, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); bool createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); template dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); dynamics::BodyNode* createDartJointAndNode( - const urdf::Joint* _jt, - const dynamics::BodyNode::Properties& _body, - dynamics::BodyNode* _parent, - dynamics::SkeletonPtr _skeleton, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const urdf::Joint* _jt, + const dynamics::BodyNode::Properties& _body, + dynamics::BodyNode* _parent, + dynamics::SkeletonPtr _skeleton, + const Uri& _baseUri, + const utils::ResourceRetrieverPtr& _resourceRetriever); bool createDartNodeProperties( - const urdf::Link* _lk, dynamics::BodyNode::Properties *properties, + const urdf::Link* _lk, + dynamics::BodyNode::Properties *properties, + const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); utils::ResourceRetrieverPtr getResourceRetriever( diff --git a/unittests/testRelativeResourceRetriever.cpp b/unittests/testRelativeResourceRetriever.cpp deleted file mode 100644 index 0a07bfb55c8e1..0000000000000 --- a/unittests/testRelativeResourceRetriever.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* - * Copyright (c) 2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Michael Koval - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * 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 -#include "dart/utils/RelativeResourceRetriever.h" -#include "TestHelpers.h" - -using dart::utils::Resource; -using dart::utils::RelativeResourceRetriever; - -TEST(RelativeResourceRetriever, exists_AbsolutePath_CallsDelegate) -{ - auto presentDelegate = std::make_shared(); - RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); - - EXPECT_TRUE(presentRetriever.exists("file:///absolute/path")); - EXPECT_TRUE(presentDelegate->mRetrieve.empty()); - ASSERT_EQ(1, presentDelegate->mExists.size()); - EXPECT_EQ("file:///absolute/path", presentDelegate->mExists.front()); - - auto absentDelegate = std::make_shared(); - RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); - - EXPECT_FALSE(absentRetriever.exists("file:///absolute/path")); - EXPECT_TRUE(absentDelegate->mRetrieve.empty()); - ASSERT_EQ(1, absentDelegate->mExists.size()); - EXPECT_EQ("file:///absolute/path", absentDelegate->mExists.front()); -} - -TEST(RelativeResourceRetriever, exists_RelativePath_Appends) -{ - auto presentDelegate = std::make_shared(); - RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); - - EXPECT_TRUE(presentRetriever.exists("relative/path")); - EXPECT_TRUE(presentDelegate->mRetrieve.empty()); - ASSERT_EQ(1, presentDelegate->mExists.size()); - EXPECT_EQ("file:///prefix/relative/path", presentDelegate->mExists.front()); - - auto absentDelegate = std::make_shared(); - RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); - - EXPECT_FALSE(absentRetriever.exists("relative/path")); - EXPECT_TRUE(absentDelegate->mRetrieve.empty()); - ASSERT_EQ(1, absentDelegate->mExists.size()); - EXPECT_EQ("file:///prefix/relative/path", absentDelegate->mExists.front()); -} - -TEST(RelativeResourceRetriever, retrieve_AbsolutePath_CallsDelegate) -{ - auto presentDelegate = std::make_shared(); - RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); - - EXPECT_TRUE(nullptr != presentRetriever.retrieve("file:///absolute/path")); - EXPECT_TRUE(presentDelegate->mExists.empty()); - ASSERT_EQ(1, presentDelegate->mRetrieve.size()); - EXPECT_EQ("file:///absolute/path", presentDelegate->mRetrieve.front()); - - auto absentDelegate = std::make_shared(); - RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); - - EXPECT_EQ(nullptr, absentRetriever.retrieve("file:///absolute/path")); - EXPECT_TRUE(absentDelegate->mExists.empty()); - ASSERT_EQ(1, absentDelegate->mRetrieve.size()); - EXPECT_EQ("file:///absolute/path", absentDelegate->mRetrieve.front()); -} - -TEST(RelativeResourceRetriever, retrieve_RelativePath_Appends) -{ - auto presentDelegate = std::make_shared(); - RelativeResourceRetriever presentRetriever("file:///prefix", presentDelegate); - - EXPECT_TRUE(nullptr != presentRetriever.retrieve("relative/path")); - EXPECT_TRUE(presentDelegate->mExists.empty()); - ASSERT_EQ(1, presentDelegate->mRetrieve.size()); - EXPECT_EQ("file:///prefix/relative/path", presentDelegate->mRetrieve.front()); - - auto absentDelegate = std::make_shared(); - RelativeResourceRetriever absentRetriever("file:///prefix", absentDelegate); - - EXPECT_EQ(nullptr, absentRetriever.retrieve("relative/path")); - EXPECT_TRUE(absentDelegate->mExists.empty()); - ASSERT_EQ(1, absentDelegate->mRetrieve.size()); - EXPECT_EQ("file:///prefix/relative/path", absentDelegate->mRetrieve.front()); -} - -int main(int argc, char* argv[]) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 8a7958c9aaa3df50dd93579882a2cb7c6d35d5ce Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 20:03:27 -0500 Subject: [PATCH 14/55] Cleaned up URDF world loading. --- dart/utils/urdf/DartLoader.cpp | 47 +++++++++++++++++----------------- dart/utils/urdf/DartLoader.h | 13 +++------- 2 files changed, 26 insertions(+), 34 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 6b3116ef0f5fa..39be8b2df0cf0 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -82,11 +82,8 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( return nullptr; } - // TODO: Extract the base path from _uri. - Uri baseUri; - return modelInterfaceToSkeleton( - urdfInterface.get(), baseUri, resourceRetriever); + urdfInterface.get(), uri, resourceRetriever); } dynamics::SkeletonPtr DartLoader::parseSkeletonString( @@ -107,7 +104,6 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( return nullptr; } - // TODO: Pass the ResourceRetriever down here. return modelInterfaceToSkeleton( urdfInterface.get(), _baseUri, getResourceRetriever(_resourceRetriever)); } @@ -131,11 +127,7 @@ simulation::WorldPtr DartLoader::parseWorld( if (!readFileToString(resourceRetriever, _uri, content)) return nullptr; - // TODO: Extract the base path from _uri. - Uri baseUri; - - // TODO: Pass the ResourceRetriever down here. - return parseWorldString(content, baseUri); + return parseWorldString(content, uri, _resourceRetriever); } simulation::WorldPtr DartLoader::parseWorldString( @@ -149,6 +141,9 @@ simulation::WorldPtr DartLoader::parseWorldString( return nullptr; } + // TODO: Where does this come from? + std::string mRootToWorldPath; + std::shared_ptr worldInterface = urdf::parseWorldURDF(_urdfString, mRootToWorldPath); @@ -159,8 +154,9 @@ simulation::WorldPtr DartLoader::parseWorldString( } // Store paths from world to entities - // TODO: Pass the ResourceRetriever down here. - parseWorldToEntityPaths(_urdfString); + std::map worldToEntityPaths; + if(!parseWorldToEntityPaths(_urdfString, worldToEntityPaths)) + return nullptr; simulation::WorldPtr world(new simulation::World); @@ -168,16 +164,18 @@ simulation::WorldPtr DartLoader::parseWorldString( { std::string model_name = worldInterface->models[i].model->getName(); std::map::const_iterator it = - mWorld_To_Entity_Paths.find(model_name); + worldToEntityPaths.find(model_name); - if(it == mWorld_To_Entity_Paths.end()) + if(it == worldToEntityPaths.end()) { dtwarn << "[DartLoader::parseWorldString] Could not find file path for [" << model_name << "]. We will not parse it!\n"; continue; } - mRootToSkelPath = mRootToWorldPath + it->second; + // TODO: Where does this come from? What is it used for? + //mRootToSkelPath = mRootToWorldPath + it->second; + dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( worldInterface->models[i].model.get(), _baseUri, mRetriever); @@ -206,7 +204,11 @@ simulation::WorldPtr DartLoader::parseWorldString( /** * @function parseWorldToEntityPaths */ -void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) + +bool DartLoader::parseWorldToEntityPaths( + const std::string& _xml_string, + std::map& _worldToEntityPaths +) { TiXmlDocument xml_doc; xml_doc.Parse(_xml_string.c_str()); @@ -214,7 +216,7 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) TiXmlElement *world_xml = xml_doc.FirstChildElement("world"); if( !world_xml ) { - return; + return false; } // Get all include filenames @@ -225,11 +227,7 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) const char* filename = include_xml->Attribute("filename"); const char* model_name = include_xml->Attribute("model_name"); - std::string string_filename( filename ); - std::string string_filepath = string_filename.substr( 0, string_filename.rfind("/") + 1 ); - std::string string_model_name( model_name ); - - includedFiles[string_model_name] = string_filepath; + includedFiles[model_name] = filename; } // Get all entities @@ -249,12 +247,12 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) { dtwarn <<"[DartLoader::parseWorldToEntityPaths] Did not find entity model [" << string_entity_model << "] included. We might fail to load some Skeletons!\n"; - return; + return false; } // Add it else { - mWorld_To_Entity_Paths[string_entity_name] = + _worldToEntityPaths[string_entity_name] = includedFiles.find( string_entity_model )->second; } } @@ -266,6 +264,7 @@ void DartLoader::parseWorldToEntityPaths(const std::string& _xml_string) } // for all entities + return true; } /** diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 1858e5237008f..7bee9c10a200d 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -72,10 +72,6 @@ class DartLoader { /// "/path/to/my_robot/meshes/mesh_for_my_robot.stl" exists. Whatever you /// specify as the package directory will end up replacing the 'package /// keyword' and 'package name' components of the URI string. - /// - /// DEPRECATED: This functionality has been moved into the - /// PackageResourceRetrievew class. - DEPRECATED(5.0) void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); @@ -99,11 +95,12 @@ class DartLoader { const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); private: - typedef std::shared_ptr BodyPropPtr; typedef std::shared_ptr JointPropPtr; - void parseWorldToEntityPaths(const std::string& _xml_string); + bool parseWorldToEntityPaths( + const std::string& _xml_string, + std::map& _worldToEntityPaths); dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( const urdf::ModelInterface* _model, @@ -148,12 +145,8 @@ class DartLoader { std::string &_output); ResourceRetrieverPtr mResourceRetriever; - std::map mWorld_To_Entity_Paths; std::map mPackageDirectories; - std::string mRootToSkelPath; - std::string mRootToWorldPath; - utils::LocalResourceRetrieverPtr mLocalRetriever; utils::PackageResourceRetrieverPtr mPackageRetriever; utils::SchemaResourceRetrieverPtr mRetriever; From 5f057891d957de876c3210e7f807b23536281f46 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 20:04:26 -0500 Subject: [PATCH 15/55] Removed more member variables from DartLoader. --- dart/utils/urdf/DartLoader.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 7bee9c10a200d..03fccd2871b14 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -144,9 +144,6 @@ class DartLoader { const std::string &_uri, std::string &_output); - ResourceRetrieverPtr mResourceRetriever; - - std::map mPackageDirectories; utils::LocalResourceRetrieverPtr mLocalRetriever; utils::PackageResourceRetrieverPtr mPackageRetriever; utils::SchemaResourceRetrieverPtr mRetriever; From 0e3d7181fa4b0bd9daa63a812cd29cece09ac085 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 20:11:10 -0500 Subject: [PATCH 16/55] Mesh loading cleanup. --- dart/utils/UriUtils.cpp | 5 +++-- dart/utils/UriUtils.h | 3 ++- dart/utils/urdf/DartLoader.cpp | 26 +++++++------------------- dart/utils/urdf/DartLoader.h | 3 ++- 4 files changed, 14 insertions(+), 23 deletions(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index aebc7e4d4866c..98dabf183c671 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -154,10 +154,11 @@ bool Uri::isRelativePath() const return isPath() && !mPath->empty() && mPath->front() != '/'; } -bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative) +bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, bool _strict) { Uri relativeUri; - relativeUri.mPath = _relative; + if(!relativeUri.fromString(_relative)) + return false; return fromRelativeUri(_base, relativeUri, true); } diff --git a/dart/utils/UriUtils.h b/dart/utils/UriUtils.h index 8c419140fb4d0..bdc15f1c08598 100644 --- a/dart/utils/UriUtils.h +++ b/dart/utils/UriUtils.h @@ -96,7 +96,8 @@ class Uri { bool fromString(const std::string& _input); bool fromStringOrPath(const std::string& _input); - bool fromRelativeUri(const Uri& _base, const std::string& _relative); + bool fromRelativeUri(const Uri& _base, const std::string& _relative, + bool _strict = false); bool fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict = false); diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 39be8b2df0cf0..3bff9cf17aa1f 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -567,30 +567,18 @@ dynamics::ShapePtr DartLoader::createShape( // Mesh else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { - // Parse the URI. This is necessary to detect whether it is a full URI, an - // absolute path, or a relative path. - Uri meshUri; - if(!meshUri.fromStringOrPath(mesh->filename)) + // Resolve relative URIs. + Uri relativeUri, absoluteUri; + if(!absoluteUri.fromRelativeUri(_baseUri, mesh->filename)) { - dtwarn << "[DartLoader::createShape] Failed parsing mesh URI: " - << mesh->filename << "\n"; + dtwarn << "[DartLoader::createShape] Failed resolving mesh URI '" + << mesh->filename << "' relative to '" << _baseUri.toString() + << "'\n"; return nullptr; } - // Resolve the relative path using _baseUri as the root. - std::string resolvedUri; - if(meshUri.isRelativePath() && meshUri.mPath) - { - meshUri = _baseUri; - meshUri.append(*meshUri.mPath); - resolvedUri = meshUri.toString(); - } - else - { - resolvedUri = mesh->filename; - } - // Load the mesh. + const std::string resolvedUri = absoluteUri.toString(); const aiScene* scene = dynamics::MeshShape::loadMesh( resolvedUri, _resourceRetriever); if (!scene) diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 03fccd2871b14..a3c0dda2d8c89 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -50,7 +50,8 @@ namespace utils { * @class DartLoader */ class DartLoader { - public: /// Constructor with the default ResourceRetriever. + public: + /// Constructor with the default ResourceRetriever. DartLoader(); /// Specify the directory of a ROS package. In your URDF files, you may see From 533fa53693ff75a88b06e4a3a125beb60459916d Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 20:42:19 -0500 Subject: [PATCH 17/55] Implement removeDotSegments. --- dart/utils/UriUtils.cpp | 128 +++++++++++++++++++++++++++------------- 1 file changed, 86 insertions(+), 42 deletions(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index 98dabf183c671..024b2bb34f9f6 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -32,6 +32,11 @@ using std::ssub_match; #endif +static bool startsWith(const std::string& _target, const std::string& _prefix) +{ + return _target.substr(0, _prefix.size()) == _prefix; +} + namespace dart { namespace utils { @@ -329,48 +334,87 @@ std::string Uri::mergePaths(const Uri& _base, const Uri& _relative) std::string Uri::removeDotSegments(const std::string& _path) { - /* - The pseudocode also refers to a "remove_dot_segments" routine for - interpreting and removing the special "." and ".." complete path - segments from a referenced path. This is done after the path is - extracted from a reference, whether or not the path was relative, in - order to remove any invalid or extraneous dot-segments prior to - forming the target URI. Although there are many ways to accomplish - this removal process, we describe a simple method using two string - buffers. - - 1. The input buffer is initialized with the now-appended path - components and the output buffer is initialized to the empty - string. - - 2. While the input buffer is not empty, loop as follows: - - A. If the input buffer begins with a prefix of "../" or "./", - then remove that prefix from the input buffer; otherwise, - - B. if the input buffer begins with a prefix of "/./" or "/.", - where "." is a complete path segment, then replace that - prefix with "/" in the input buffer; otherwise, - - C. if the input buffer begins with a prefix of "/../" or "/..", - where ".." is a complete path segment, then replace that - prefix with "/" in the input buffer and remove the last - segment and its preceding "/" (if any) from the output - buffer; otherwise, - - D. if the input buffer consists only of "." or "..", then remove - that from the input buffer; otherwise, - - E. move the first path segment in the input buffer to the end of - the output buffer, including the initial "/" character (if - any) and any subsequent characters up to, but not including, - the next "/" character or the end of the input buffer. - - 3. Finally, the output buffer is returned as the result of - remove_dot_segments. - */ - assert(false && "Not implemented."); - return ""; + // 1. The input buffer is initialized with the now-appended path + // components and the output buffer is initialized to the empty + // string. + std::string input = _path; + std::string output; + + // 2. While the input buffer is not empty, loop as follows: + while(!input.empty()) + { + // A. If the input buffer begins with a prefix of "../" or "./", + // then remove that prefix from the input buffer; otherwise, + if(startsWith(input, "../")) + { + input = input.substr(3); + } + else if(startsWith(input, "./")) + { + input = input.substr(2); + } + // B. if the input buffer begins with a prefix of "/./" or "/.", + // where "." is a complete path segment, then replace that + // prefix with "/" in the input buffer; otherwise, + else if(input == "/.") + { + input = "/"; + } + else if(startsWith(input, "/./")) + { + input = "/" + input.substr(3); + } + // C. if the input buffer begins with a prefix of "/../" or "/..", + // where ".." is a complete path segment, then replace that + // prefix with "/" in the input buffer and remove the last + // segment and its preceding "/" (if any) from the output + // buffer; otherwise, + else if(input == "/..") + { + input = "/"; + + size_t index = output.find_last_of('/'); + if(index != std::string::npos) + output = output.substr(0, index); + else + output = ""; + } + else if(startsWith(input, "/../")) + { + input = "/" + input.substr(4); + + size_t index = output.find_last_of('/'); + if(index != std::string::npos) + output = output.substr(0, index); + else + output = ""; + } + // D. if the input buffer consists only of "." or "..", then remove + // that from the input buffer; otherwise, + else if(input == "." || input == "..") + { + input = ""; + } + // E. move the first path segment in the input buffer to the end of + // the output buffer, including the initial "/" character (if + // any) and any subsequent characters up to, but not including, + // the next "/" character or the end of the input buffer. + else + { + // Start at index one so we keep the leading '/'. + size_t index = input.find_first_of('/', 1); + output += input.substr(0, index); + + if(index != std::string::npos) + input = input.substr(index); + else + input = ""; + } + } + + // 3. Finally, the output buffer is returned as the result of + // remove_dot_segments. + return output; } } // namespace utils From 3dd3b1191bd39a8e6471473c25549db80919fd8d Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 21:06:34 -0500 Subject: [PATCH 18/55] Implemented URI joining. --- dart/utils/UriUtils.cpp | 7 +++- unittests/testUriHelpers.cpp | 77 ++++++++++++++++++++++++++++++++++++ 2 files changed, 82 insertions(+), 2 deletions(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index 024b2bb34f9f6..571cf8d6c311d 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -165,7 +165,7 @@ bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, bool _ if(!relativeUri.fromString(_relative)) return false; - return fromRelativeUri(_base, relativeUri, true); + return fromRelativeUri(_base, relativeUri, _strict); } bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) @@ -173,8 +173,11 @@ bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) assert(_base.mPath && "The path component is always defined."); assert(_relative.mPath && "The path component is always defined."); + // TODO If !(!_strict && _relative.mScheme == _base.mScheme), then we need to + // enable backwards compatability. + // This directly implements the psueocode in Section 5.2.2. of RFC 3986. - if(_relative.mScheme || (!_strict && _relative.mScheme == _base.mScheme)) + if(_relative.mScheme) { mScheme = _relative.mScheme; mAuthority = _relative.mAuthority; diff --git a/unittests/testUriHelpers.cpp b/unittests/testUriHelpers.cpp index fa316d9bda82b..08eca63d4d057 100644 --- a/unittests/testUriHelpers.cpp +++ b/unittests/testUriHelpers.cpp @@ -232,6 +232,83 @@ TEST(UriHelpers, getUri_InputIsPath_AppendsFileSchema) } } +TEST(UriHelpers, getRelativeUri) +{ + std::vector > testPairs = { + // RFC 3986, Section 5.4.1.: Normal Examples + { "g:h" , "g:h" }, + { "g" , "http://a/b/c/g" }, + { "./g" , "http://a/b/c/g" }, + { "g/" , "http://a/b/c/g/" }, + { "/g" , "http://a/g" }, + { "//g" , "http://g" }, + { "?y" , "http://a/b/c/d;p?y" }, + { "g?y" , "http://a/b/c/g?y" }, + { "#s" , "http://a/b/c/d;p?q#s" }, + { "g#s" , "http://a/b/c/g#s" }, + { "g?y#s" , "http://a/b/c/g?y#s" }, + { ";x" , "http://a/b/c/;x" }, + { "g;x" , "http://a/b/c/g;x" }, + { "g;x?y#s" , "http://a/b/c/g;x?y#s" }, + { "" , "http://a/b/c/d;p?q" }, + { "." , "http://a/b/c/" }, + { "./" , "http://a/b/c/" }, + { ".." , "http://a/b/" }, + { "../" , "http://a/b/" }, + { "../g" , "http://a/b/g" }, + { "../.." , "http://a/" }, + { "../../" , "http://a/" }, + { "../../g" , "http://a/g" }, + // RFC 3986, Section 5.4.2.: Abnormal Examples + { "../../../g" , "http://a/g" }, + { "../../../../g" , "http://a/g" }, + { "/./g" , "http://a/g" }, + { "/../g" , "http://a/g" }, + { "g." , "http://a/b/c/g." }, + { ".g" , "http://a/b/c/.g" }, + { "g.." , "http://a/b/c/g.." }, + { "..g" , "http://a/b/c/..g" }, + { "./../g" , "http://a/b/g" }, + { "./g/." , "http://a/b/c/g/" }, + { "g/./h" , "http://a/b/c/g/h" }, + { "g/../h" , "http://a/b/c/h" }, + { "g;x=1/./y" , "http://a/b/c/g;x=1/y" }, + { "g;x=1/../y" , "http://a/b/c/y" }, + { "g?y/./x" , "http://a/b/c/g?y/./x" }, + { "g?y/../x" , "http://a/b/c/g?y/../x" }, + { "g#s/./x" , "http://a/b/c/g#s/./x" }, + { "g#s/../x" , "http://a/b/c/g#s/../x" }, + }; + + Uri baseUri, relativeUri, mergedUri; + ASSERT_TRUE(baseUri.fromString("http://a/b/c/d;p?q")); + + for (const auto& it : testPairs) + { + const std::string& expectedUri = it.second; + + ASSERT_TRUE(relativeUri.fromString(it.first)); + + // Strict mode + ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, relativeUri, true)); + EXPECT_EQ(expectedUri, mergedUri.toString()); + + // Backwards compatability mode + ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, relativeUri, false)); + EXPECT_EQ(expectedUri, mergedUri.toString()); + } + + // Strict mode behavior. + ASSERT_TRUE(relativeUri.fromString("http:g")); + ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, "http:g", true)); + EXPECT_EQ("http:g", mergedUri.toString()); + + // Backwards compatability mode behavior. + ASSERT_TRUE(relativeUri.fromString("http:g")); + ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, "http:g", false)); + EXPECT_EQ("http://a/b/c/g", mergedUri.toString()); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From ac28d8f3d21f87a1f8138f2ae7195c307185c23e Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 22:27:06 -0500 Subject: [PATCH 19/55] Made most of DartLoader static. --- dart/utils/UriUtils.cpp | 2 +- dart/utils/urdf/DartLoader.cpp | 9 ++++++--- dart/utils/urdf/DartLoader.h | 16 ++++++++-------- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index 571cf8d6c311d..d698a68d31ffd 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -173,7 +173,7 @@ bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) assert(_base.mPath && "The path component is always defined."); assert(_relative.mPath && "The path component is always defined."); - // TODO If !(!_strict && _relative.mScheme == _base.mScheme), then we need to + // TODO If (!_strict && _relative.mScheme == _base.mScheme), then we need to // enable backwards compatability. // This directly implements the psueocode in Section 5.2.2. of RFC 3986. diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 3bff9cf17aa1f..cd68d6a6007be 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -134,6 +134,9 @@ simulation::WorldPtr DartLoader::parseWorldString( const std::string& _urdfString, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever) { + const utils::ResourceRetrieverPtr resourceRetriever + = getResourceRetriever(_resourceRetriever); + if(_urdfString.empty()) { dtwarn << "[DartLoader::parseWorldString] A blank string cannot be " @@ -177,7 +180,7 @@ simulation::WorldPtr DartLoader::parseWorldString( //mRootToSkelPath = mRootToWorldPath + it->second; dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton( - worldInterface->models[i].model.get(), _baseUri, mRetriever); + worldInterface->models[i].model.get(), _baseUri, resourceRetriever); if(!skeleton) { @@ -573,7 +576,7 @@ dynamics::ShapePtr DartLoader::createShape( { dtwarn << "[DartLoader::createShape] Failed resolving mesh URI '" << mesh->filename << "' relative to '" << _baseUri.toString() - << "'\n"; + << "'.\n"; return nullptr; } @@ -586,7 +589,7 @@ dynamics::ShapePtr DartLoader::createShape( const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shape = std::make_shared( - scale, scene, resolvedUri, true, mRetriever); + scale, scene, resolvedUri, true, _resourceRetriever); } // Unknown geometry type else diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index a3c0dda2d8c89..1e5d0a2a72771 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -99,16 +99,16 @@ class DartLoader { typedef std::shared_ptr BodyPropPtr; typedef std::shared_ptr JointPropPtr; - bool parseWorldToEntityPaths( + static bool parseWorldToEntityPaths( const std::string& _xml_string, std::map& _worldToEntityPaths); - dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( + static dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( const urdf::ModelInterface* _model, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); - bool createSkeletonRecursive( + static bool createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent, @@ -116,11 +116,11 @@ class DartLoader { const utils::ResourceRetrieverPtr& _resourceRetriever); template - dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol, + static dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol, const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); - dynamics::BodyNode* createDartJointAndNode( + static dynamics::BodyNode* createDartJointAndNode( const urdf::Joint* _jt, const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, @@ -128,7 +128,7 @@ class DartLoader { const Uri& _baseUri, const utils::ResourceRetrieverPtr& _resourceRetriever); - bool createDartNodeProperties( + static bool createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties *properties, const Uri& _baseUri, @@ -137,8 +137,8 @@ class DartLoader { utils::ResourceRetrieverPtr getResourceRetriever( const utils::ResourceRetrieverPtr& _resourceRetriever); - Eigen::Isometry3d toEigen(const urdf::Pose& _pose); - Eigen::Vector3d toEigen(const urdf::Vector3& _vector); + static Eigen::Isometry3d toEigen(const urdf::Pose& _pose); + static Eigen::Vector3d toEigen(const urdf::Vector3& _vector); static bool readFileToString( const utils::ResourceRetrieverPtr& _resourceRetriever, From ccb131660e1492d6c6ec8fdb4cfc93b547bd310e Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 19 Jul 2015 23:29:27 -0500 Subject: [PATCH 20/55] Loading Robonaut works again. --- dart/utils/LocalResourceRetriever.cpp | 42 +++++++++++++------------- dart/utils/SchemaResourceRetriever.cpp | 13 ++++---- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index 9db060b86aa13..316af4472f446 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -1,43 +1,43 @@ #include #include #include "dart/common/Console.h" +#include "dart/utils/UriUtils.h" #include "dart/utils/LocalResourceRetriever.h" #include "LocalResource.h" -static std::string getPath(const std::string& _uriOrPath) -{ - static const std::string file_schema = "file://"; - - // There is no schema, so we'll interpret the input as a path. - if(_uriOrPath.find_first_of(':') == std::string::npos) - return _uriOrPath; - // This is a file:// URI, so we'll strip off the schema. - else if(_uriOrPath.size() > file_schema.size() - && _uriOrPath.substr(0, file_schema.size()) == file_schema) - return _uriOrPath.substr(file_schema.size()); - else - return ""; -} - - namespace dart { namespace utils { bool LocalResourceRetriever::exists(const std::string& _uri) { + Uri uri; + if(!uri.fromString(_uri)) + { + dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; + return false; + } + // Open and close the file to check if it exists. It would be more efficient // to stat() it, but that is not portable. - const std::string path = getPath(_uri); - return !path.empty() && std::ifstream(path, std::ios::binary).good(); + if (uri.mScheme.get_value_or("file") != "file") + return false; + + return std::ifstream(*uri.mScheme, std::ios::binary).good(); } ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) { - const std::string path = getPath(_uri); - if(path.empty()) + Uri uri; + if(!uri.fromString(_uri)) + { + dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; + return nullptr; + } + + if (uri.mScheme.get_value_or("file") != "file") return nullptr; - const auto resource = std::make_shared(path); + const auto resource = std::make_shared(*uri.mPath); if(resource->isGood()) return resource; else diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index c8c5c61579e00..65b336595f485 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -1,6 +1,7 @@ #include #include "dart/common/Console.h" #include "dart/utils/SchemaResourceRetriever.h" +#include "dart/utils/UriUtils.h" namespace dart { namespace utils { @@ -74,15 +75,15 @@ const std::vector& SchemaResourceRetriever::getRetrievers( std::string SchemaResourceRetriever::getSchema(const std::string& _uri) { - const auto schemaIndex = _uri.find("://"); - if(schemaIndex == std::string::npos) + Uri uri; + if(!uri.fromString(_uri)) { - dterr << "[SchemaResourceRetriever::retrieve] Failed to extract schema from" - " URI '" << _uri << "'. Is this a valid URI?\n"; - return nullptr; + dtwarn << "[SchemaResourceRetriever::retrieve] Failed parsing URI:" + << _uri << "\n"; + return ""; } - return _uri.substr(0, schemaIndex); + return uri.mScheme.get_value_or("file"); } } // namespace utils From f347112ce77c8bae1c9004a56ce88a157b61592b Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 00:11:59 -0500 Subject: [PATCH 21/55] Optimized the regex. --- dart/utils/UriUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index d698a68d31ffd..1c004c2e0bbe1 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -228,7 +228,7 @@ bool Uri::fromString(const std::string& _input) // This is regex is from Appendix B of RFC 3986. static regex uriRegex( R"END(^(([^:/?#]+):)?(//([^/?#]*))?([^?#]*)(\?([^#]*))?(#(.*))?)END", - regex::extended); + regex::extended | regex::optimize); static const size_t schemeIndex = 2; static const size_t authorityIndex = 4; static const size_t pathIndex = 5; From 483613e845bce78ab907d9766c8d5a0e69ea8ba9 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 21:11:53 -0500 Subject: [PATCH 22/55] Added unit tests for SchemaResourceRetriever. --- dart/utils/SchemaResourceRetriever.cpp | 28 +++- dart/utils/SchemaResourceRetriever.h | 11 +- unittests/testSchemaResourceRetriever.cpp | 196 ++++++++++++++++++++++ 3 files changed, 225 insertions(+), 10 deletions(-) create mode 100644 unittests/testSchemaResourceRetriever.cpp diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index 65b336595f485..e19c4b6b6c469 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -6,6 +6,12 @@ namespace dart { namespace utils { +void SchemaResourceRetriever::addDefaultRetriever( + const ResourceRetrieverPtr& _resourceRetriever) +{ + mDefaultResourceRetrievers.push_back(_resourceRetriever); +} + bool SchemaResourceRetriever::addSchemaRetriever( const std::string& _schema, const ResourceRetrieverPtr& _resourceRetriever) { @@ -54,23 +60,33 @@ ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) return nullptr; } -const std::vector& SchemaResourceRetriever::getRetrievers( +std::vector SchemaResourceRetriever::getRetrievers( const std::string& _uri) { - static const std::vector empty_placeholder; - const std::string schema = getSchema(_uri); + std::vector retrievers; + const auto it = mResourceRetrievers.find(schema); if(it != std::end(mResourceRetrievers)) - return it->second; - else + retrievers.insert( + std::end(retrievers), + std::begin(it->second), + std::end(it->second)); + + retrievers.insert( + std::end(retrievers), + std::begin(mDefaultResourceRetrievers), + std::end(mDefaultResourceRetrievers)); + + if(retrievers.empty()) { dtwarn << "[SchemaResourceRetriever::retrieve] There are no resource" " retrievers registered for the schema '" << schema << "'" " that is necessary to retrieve URI '" << _uri << "'.\n"; - return empty_placeholder; } + + return retrievers; } std::string SchemaResourceRetriever::getSchema(const std::string& _uri) diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index 8eac46d64b8bd..e701ab66e842a 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -47,6 +47,8 @@ class SchemaResourceRetriever : public virtual ResourceRetriever public: virtual ~SchemaResourceRetriever() = default; + void addDefaultRetriever(const ResourceRetrieverPtr& _resourceRetriever); + bool addSchemaRetriever( const std::string& _schema, const ResourceRetrieverPtr& _resourceRetriever); @@ -55,12 +57,13 @@ class SchemaResourceRetriever : public virtual ResourceRetriever ResourcePtr retrieve(const std::string& _uri) override; private: - std::unordered_map > mResourceRetrievers; + std::vector getRetrievers(const std::string& _uri); - const std::vector& getRetrievers( - const std::string& _uri); std::string getSchema(const std::string& _uri); + + std::unordered_map > mResourceRetrievers; + std::vector mDefaultResourceRetrievers; }; typedef std::shared_ptr SchemaResourceRetrieverPtr; diff --git a/unittests/testSchemaResourceRetriever.cpp b/unittests/testSchemaResourceRetriever.cpp new file mode 100644 index 0000000000000..3f2f428b18df0 --- /dev/null +++ b/unittests/testSchemaResourceRetriever.cpp @@ -0,0 +1,196 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 +#include "dart/utils/SchemaResourceRetriever.h" +#include "TestHelpers.h" + +using dart::utils::Resource; +using dart::utils::ResourcePtr; +using dart::utils::ResourceRetriever; +using dart::utils::SchemaResourceRetriever; + +TEST(SchemaResourceRetriever, exists_NothingRegistered_ReturnsFalse) +{ + SchemaResourceRetriever retriever; + EXPECT_FALSE(retriever.exists("package://test/foo")); +} + +TEST(SchemaResourceRetriever, exists_AllRetrieversFail_ReturnsFalse) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + retriever.addDefaultRetriever(retriever2); + + EXPECT_FALSE(retriever.exists("package://test/foo")); + + EXPECT_TRUE(retriever1->mRetrieve.empty()); + ASSERT_EQ(1, retriever1->mExists.size()); + EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + + EXPECT_TRUE(retriever2->mRetrieve.empty()); + ASSERT_EQ(1, retriever2->mExists.size()); + EXPECT_EQ("package://test/foo", retriever2->mExists.front()); +} + +TEST(SchemaResourceRetriever, exists_SchemaResourceRetrieverSucceeds_ReturnsTrue) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + auto retriever3 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever2)); + retriever.addDefaultRetriever(retriever3); + + EXPECT_TRUE(retriever.exists("package://test/foo")); + + EXPECT_TRUE(retriever1->mRetrieve.empty()); + ASSERT_EQ(1, retriever1->mExists.size()); + EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + + EXPECT_TRUE(retriever2->mExists.empty()); + EXPECT_TRUE(retriever2->mRetrieve.empty()); + EXPECT_TRUE(retriever3->mExists.empty()); + EXPECT_TRUE(retriever3->mRetrieve.empty()); +} + +TEST(SchemaResourceRetriever, exists_DefaultResourceRetrieverSucceeds_ReturnsTrue) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + auto retriever3 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + retriever.addDefaultRetriever(retriever2); + retriever.addDefaultRetriever(retriever3); + + EXPECT_TRUE(retriever.exists("package://test/foo")); + EXPECT_TRUE(retriever1->mRetrieve.empty()); + ASSERT_EQ(1, retriever1->mExists.size()); + EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + + EXPECT_TRUE(retriever2->mRetrieve.empty()); + ASSERT_EQ(1, retriever1->mExists.size()); + EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + + EXPECT_TRUE(retriever3->mExists.empty()); + EXPECT_TRUE(retriever3->mRetrieve.empty()); +} + +TEST(SchemaResourceRetriever, retrieve_NothingRegistered_ReturnsNull) +{ + SchemaResourceRetriever retriever; + EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); +} + +TEST(SchemaResourceRetriever, retrieve_AllRetrieversFail_ReturnsNull) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + retriever.addDefaultRetriever(retriever2); + + EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); + + EXPECT_TRUE(retriever1->mExists.empty()); + ASSERT_EQ(1, retriever1->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); + + EXPECT_TRUE(retriever2->mExists.empty()); + ASSERT_EQ(1, retriever2->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever2->mRetrieve.front()); +} + +TEST(SchemaResourceRetriever, retrieve_SchemaResourceRetrieverSucceeds_ReturnsNonNull) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + auto retriever3 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever2)); + retriever.addDefaultRetriever(retriever3); + + EXPECT_TRUE(nullptr != retriever.retrieve("package://test/foo")); + + EXPECT_TRUE(retriever1->mExists.empty()); + ASSERT_EQ(1, retriever1->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + + EXPECT_TRUE(retriever2->mExists.empty()); + EXPECT_TRUE(retriever2->mRetrieve.empty()); + EXPECT_TRUE(retriever3->mExists.empty()); + EXPECT_TRUE(retriever3->mRetrieve.empty()); +} + +TEST(SchemaResourceRetriever, retrieve_DefaultResourceRetrieverSucceeds_ReturnsNonNull) +{ + auto retriever1 = std::make_shared(); + auto retriever2 = std::make_shared(); + auto retriever3 = std::make_shared(); + SchemaResourceRetriever retriever; + + EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); + retriever.addDefaultRetriever(retriever2); + retriever.addDefaultRetriever(retriever3); + + EXPECT_TRUE(nullptr != retriever.retrieve("package://test/foo")); + EXPECT_TRUE(retriever1->mExists.empty()); + ASSERT_EQ(1, retriever1->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); + + EXPECT_TRUE(retriever2->mExists.empty()); + ASSERT_EQ(1, retriever1->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); + + EXPECT_TRUE(retriever3->mExists.empty()); + EXPECT_TRUE(retriever3->mRetrieve.empty()); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From f5e1ceef3925ddbf99ff4c48b45221ae72d8073c Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 22:36:51 -0500 Subject: [PATCH 23/55] Non-backwards compatable SDF changes. --- dart/utils/sdf/SdfParser.cpp | 92 ++++++++++++++++++++------------ dart/utils/sdf/SdfParser.h | 49 ++++++++++++----- dart/utils/sdf/SoftSdfParser.cpp | 40 +++++++++----- dart/utils/sdf/SoftSdfParser.h | 16 ++++-- 4 files changed, 130 insertions(+), 67 deletions(-) diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 2439a565aa7f4..326253dca51c1 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -25,15 +25,21 @@ namespace dart { namespace utils { -simulation::WorldPtr SdfParser::readSdfFile(const std::string& _filename) +simulation::WorldPtr SdfParser::readSdfFile( + const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return readSdfFile(_filename, - static_cast(&SdfParser::readWorld)); + return readSdfFile(_filename, _retriever, + static_cast(&SdfParser::readWorld)); } -simulation::WorldPtr SdfParser::readSdfFile(const std::string& _filename, - std::function xmlReader) +simulation::WorldPtr SdfParser::readSdfFile( + const std::string& _filename, + const ResourceRetrieverPtr& _retriever, + std::function xmlReader) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -82,19 +88,24 @@ simulation::WorldPtr SdfParser::readSdfFile(const std::string& _filename, std::replace(unixFileName.begin(), unixFileName.end(), '\\' , '/' ); std::string skelPath = unixFileName.substr(0, unixFileName.rfind("/") + 1); - return xmlReader(worldElement, skelPath); + return xmlReader(worldElement, skelPath, _retriever); } -dynamics::SkeletonPtr SdfParser::readSkeleton(const std::string& _fileName) +dynamics::SkeletonPtr SdfParser::readSkeleton( + const std::string& _fileName, const ResourceRetrieverPtr& _retriever) { - return readSkeleton(_fileName, - static_cast(&SdfParser::readSkeleton)); + return readSkeleton(_fileName, _retriever, + static_cast(&SdfParser::readSkeleton)); } dynamics::SkeletonPtr SdfParser::readSkeleton( const std::string& _filename, - std::function xmlReader) + const ResourceRetrieverPtr& _retriever, + std::function xmlReader) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -142,24 +153,29 @@ dynamics::SkeletonPtr SdfParser::readSkeleton( std::replace(unixFileName.begin(), unixFileName.end(), '\\' , '/' ); std::string skelPath = unixFileName.substr(0, unixFileName.rfind("/") + 1); - dynamics::SkeletonPtr newSkeleton = xmlReader(skelElement, skelPath); + dynamics::SkeletonPtr newSkeleton = xmlReader(skelElement, skelPath, _retriever); return newSkeleton; } simulation::WorldPtr SdfParser::readWorld( tinyxml2::XMLElement* _worldElement, - const std::string& _skelPath) + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { - return readWorld(_worldElement, _skelPath, - static_cast(&SdfParser::readSkeleton)); + return readWorld(_worldElement, _skelPath, _retriever, + static_cast(&SdfParser::readSkeleton)); } simulation::WorldPtr SdfParser::readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - std::function skeletonReader) + const ResourceRetrieverPtr& _retriever, + std::function skeletonReader) { assert(_worldElement != nullptr); @@ -185,7 +201,7 @@ simulation::WorldPtr SdfParser::readWorld( while (skeletonElements.next()) { dynamics::SkeletonPtr newSkeleton - = skeletonReader(skeletonElements.get(), _skelPath); + = skeletonReader(skeletonElements.get(), _skelPath, _retriever); newWorld->addSkeleton(newSkeleton); } @@ -223,18 +239,20 @@ void SdfParser::readPhysics(tinyxml2::XMLElement* _physicsElement, dynamics::SkeletonPtr SdfParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, - const std::string& _skelPath) + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { - return readSkeleton(_skeletonElement, _skelPath, &readBodyNode, &createPair); + return readSkeleton(_skeletonElement, _skelPath, _retriever, &readBodyNode, &createPair); } dynamics::SkeletonPtr SdfParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - + const ResourceRetrieverPtr& _retriever, std::function bodyReader, + const std::string&, + const ResourceRetrieverPtr&)> bodyReader, std::function bodyReader) + const std::string&, + const ResourceRetrieverPtr&)> bodyReader) { ElementEnumerator bodies(_skeletonElement, "link"); BodyMap sdfBodyNodes; while (bodies.next()) { SDFBodyNode body = bodyReader( - bodies.get(), skeletonFrame, _skelPath); + bodies.get(), skeletonFrame, _skelPath, _retriever); BodyMap::iterator it = sdfBodyNodes.find(body.properties->mName); if(it != sdfBodyNodes.end()) @@ -434,7 +455,8 @@ SdfParser::BodyMap SdfParser::readAllBodyNodes( SdfParser::SDFBodyNode SdfParser::readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, - const std::string& _skelPath) + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { assert(_bodyNodeElement != nullptr); @@ -477,7 +499,7 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( ElementEnumerator vizShapes(_bodyNodeElement, "visual"); while (vizShapes.next()) { - dynamics::ShapePtr newShape(readShape(vizShapes.get(), _skelPath)); + dynamics::ShapePtr newShape(readShape(vizShapes.get(), _skelPath, _retriever)); if (newShape) properties.mVizShapes.push_back(newShape); } @@ -487,7 +509,7 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( ElementEnumerator collShapes(_bodyNodeElement, "collision"); while (collShapes.next()) { - dynamics::ShapePtr newShape(readShape(collShapes.get(), _skelPath)); + dynamics::ShapePtr newShape(readShape(collShapes.get(), _skelPath, _retriever)); if (newShape) properties.mColShapes.push_back(newShape); @@ -549,8 +571,10 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( return sdfBodyNode; } -dynamics::ShapePtr SdfParser::readShape(tinyxml2::XMLElement* _shapelement, - const std::string& _skelPath) +dynamics::ShapePtr SdfParser::readShape( + tinyxml2::XMLElement* _shapelement, + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { dynamics::ShapePtr newShape; diff --git a/dart/utils/sdf/SdfParser.h b/dart/utils/sdf/SdfParser.h index 17ccb4599390f..186cb6465346b 100644 --- a/dart/utils/sdf/SdfParser.h +++ b/dart/utils/sdf/SdfParser.h @@ -11,6 +11,7 @@ #include #include "dart/utils/Parser.h" +#include "dart/utils/ResourceRetriever.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/WeldJoint.h" #include "dart/dynamics/RevoluteJoint.h" @@ -50,19 +51,28 @@ class SdfParser { public: /// \brief - static dart::simulation::WorldPtr readSdfFile(const std::string& _filename); + // TODO: Make ResourceRetriever optional. + static dart::simulation::WorldPtr readSdfFile( + const std::string& _filename, const ResourceRetrieverPtr& _retriever); static simulation::WorldPtr readSdfFile( const std::string& _filename, - std::function xmlReader); + const ResourceRetrieverPtr& _retriever, + std::function xmlReader); /// \brief - static dynamics::SkeletonPtr readSkeleton(const std::string& _fileName); + // TODO: Make ResourceRetriever optional. + static dynamics::SkeletonPtr readSkeleton( + const std::string& _fileName, const ResourceRetrieverPtr& _retriever); - static dynamics::SkeletonPtr readSkeleton(const std::string& _fileName, + static dynamics::SkeletonPtr readSkeleton( + const std::string& _fileName, + const ResourceRetrieverPtr& _retriever, std::function xmlReader); + tinyxml2::XMLElement*, const std::string&, + const ResourceRetrieverPtr&)> xmlReader); typedef std::shared_ptr BodyPropPtr; @@ -92,14 +102,17 @@ class SdfParser static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); /// \brief static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - std::function skeletonReader); + const ResourceRetrieverPtr& _retriever, + std::function skeletonReader); /// \brief static void readPhysics(tinyxml2::XMLElement* _physicsElement, @@ -108,15 +121,18 @@ class SdfParser /// \brief static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever, std::function bodyReader, + const std::string&, + const ResourceRetrieverPtr&)> bodyReader, std::function bodyReader); + const std::string&, + const ResourceRetrieverPtr&)> bodyReader); /// \brief static SDFBodyNode readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); /// \brief static dynamics::ShapePtr readShape( tinyxml2::XMLElement* _shapelement, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); /// \brief static JointMap readAllJoints( diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index 62c701000d9b2..67bc50717d1e4 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -68,17 +68,22 @@ namespace dart { namespace utils { -simulation::WorldPtr SoftSdfParser::readSoftSdfFile(const std::string& _filename) +simulation::WorldPtr SoftSdfParser::readSoftSdfFile( + const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return SdfParser::readSdfFile(_filename, - static_cast(&SoftSdfParser::readWorld)); + return SdfParser::readSdfFile(_filename, _retriever, + static_cast(&SoftSdfParser::readWorld)); } dynamics::SkeletonPtr SoftSdfParser::readSkeleton( - const std::string& _filename) + const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return SdfParser::readSkeleton(_filename, - static_cast(&SoftSdfParser::readSkeleton)); + return SdfParser::readSkeleton(_filename, _retriever, + static_cast(&SoftSdfParser::readSkeleton)); } bool SoftSdfParser::createSoftPair( @@ -108,23 +113,30 @@ bool SoftSdfParser::createSoftPair( } simulation::WorldPtr SoftSdfParser::readWorld( - tinyxml2::XMLElement* _worldElement, const std::string& _skelPath) + tinyxml2::XMLElement* _worldElement, + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { - return SdfParser::readWorld(_worldElement, _skelPath, - static_cast(&SoftSdfParser::readSkeleton)); + return SdfParser::readWorld(_worldElement, _skelPath, _retriever, + static_cast(&SoftSdfParser::readSkeleton)); } dynamics::SkeletonPtr SoftSdfParser::readSkeleton( - tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath) + tinyxml2::XMLElement* _skeletonElement, + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { - return SdfParser::readSkeleton(_skeletonElement, _skelPath, + return SdfParser::readSkeleton(_skeletonElement, _skelPath, _retriever, &readSoftBodyNode, &createSoftPair); } SdfParser::SDFBodyNode SoftSdfParser::readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, - const std::string& _skelPath) + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever) { //---------------------------------- Note ------------------------------------ // SoftBodyNode is created if _softBodyNodeElement has . @@ -137,11 +149,11 @@ SdfParser::SDFBodyNode SoftSdfParser::readSoftBodyNode( if (!hasElement(_softBodyNodeElement, "soft_shape")) { return SdfParser::readBodyNode( - _softBodyNodeElement, _skeletonFrame, _skelPath); + _softBodyNodeElement, _skeletonFrame, _skelPath, _retriever); } SDFBodyNode standardSDF = - SdfParser::readBodyNode(_softBodyNodeElement, _skeletonFrame, _skelPath); + SdfParser::readBodyNode(_softBodyNodeElement, _skeletonFrame, _skelPath, _retriever); BodyPropPtr standardProperties = standardSDF.properties; diff --git a/dart/utils/sdf/SoftSdfParser.h b/dart/utils/sdf/SoftSdfParser.h index f03df4c69731f..bebde5873816a 100644 --- a/dart/utils/sdf/SoftSdfParser.h +++ b/dart/utils/sdf/SoftSdfParser.h @@ -65,11 +65,14 @@ class SoftSdfParser : public SdfParser { public: /// \brief - static simulation::WorldPtr readSoftSdfFile(const std::string& _filename); + // TODO: Make ResourceRetrieverPtr optional. + static simulation::WorldPtr readSoftSdfFile( + const std::string& _filename, const ResourceRetrieverPtr& _retriever); /// \brief + // TODO: Make ResourceRetrieverPtr optional. static dynamics::SkeletonPtr readSkeleton( - const std::string& _fileName); + const std::string& _fileName, const ResourceRetrieverPtr& _retriever); static bool createSoftPair(dynamics::SkeletonPtr skeleton, dynamics::BodyNode* parent, @@ -79,18 +82,21 @@ class SoftSdfParser : public SdfParser /// \brief static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); /// \brief static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); /// \brief static SDFBodyNode readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, - const std::string& _skelPath); + const std::string& _skelPath, + const ResourceRetrieverPtr& _retriever); }; } // namespace utils From fce0a78a5f1bd5f39dee808fc2d6844822cec2a8 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 22:41:47 -0500 Subject: [PATCH 24/55] Added SDFParser backwards compatability. --- dart/utils/sdf/SdfParser.cpp | 14 ++++++++++++-- dart/utils/sdf/SdfParser.h | 9 ++++++--- dart/utils/sdf/SoftSdfParser.cpp | 7 +++++-- dart/utils/sdf/SoftSdfParser.h | 8 ++++---- 4 files changed, 27 insertions(+), 11 deletions(-) diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 326253dca51c1..40d22cecc008a 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -20,6 +20,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" +#include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/sdf/SdfParser.h" namespace dart { @@ -28,7 +29,7 @@ namespace utils { simulation::WorldPtr SdfParser::readSdfFile( const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return readSdfFile(_filename, _retriever, + return readSdfFile(_filename, getResourceRetriever(_retriever), static_cast(&SdfParser::readWorld)); @@ -94,7 +95,7 @@ simulation::WorldPtr SdfParser::readSdfFile( dynamics::SkeletonPtr SdfParser::readSkeleton( const std::string& _fileName, const ResourceRetrieverPtr& _retriever) { - return readSkeleton(_fileName, _retriever, + return readSkeleton(_fileName, getResourceRetriever(_retriever), static_cast(&SdfParser::readSkeleton)); @@ -1056,5 +1057,14 @@ dynamics::FreeJoint::Properties SdfParser::readFreeJoint( return dynamics::FreeJoint::Properties(); } +ResourceRetrieverPtr SdfParser::getResourceRetriever( + const ResourceRetrieverPtr& _retriever) +{ + if(_retriever) + return _retriever; + else + return std::make_shared(); +} + } // namespace utils } // namespace dart diff --git a/dart/utils/sdf/SdfParser.h b/dart/utils/sdf/SdfParser.h index 186cb6465346b..781cda6a2dd14 100644 --- a/dart/utils/sdf/SdfParser.h +++ b/dart/utils/sdf/SdfParser.h @@ -53,7 +53,8 @@ class SdfParser /// \brief // TODO: Make ResourceRetriever optional. static dart::simulation::WorldPtr readSdfFile( - const std::string& _filename, const ResourceRetrieverPtr& _retriever); + const std::string& _filename, + const ResourceRetrieverPtr& _retriever = nullptr); static simulation::WorldPtr readSdfFile( const std::string& _filename, @@ -65,7 +66,8 @@ class SdfParser /// \brief // TODO: Make ResourceRetriever optional. static dynamics::SkeletonPtr readSkeleton( - const std::string& _fileName, const ResourceRetrieverPtr& _retriever); + const std::string& _fileName, + const ResourceRetrieverPtr& _retriever = nullptr); static dynamics::SkeletonPtr readSkeleton( const std::string& _fileName, @@ -291,7 +293,8 @@ class SdfParser const Eigen::Isometry3d& _parentModelFrame, const std::string& _name); - + static ResourceRetrieverPtr getResourceRetriever( + const ResourceRetrieverPtr& _retriever); }; } // namespace utils diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index 67bc50717d1e4..e7176c37cc8bf 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -71,7 +71,7 @@ namespace utils { simulation::WorldPtr SoftSdfParser::readSoftSdfFile( const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return SdfParser::readSdfFile(_filename, _retriever, + return SdfParser::readSdfFile(_filename, getResourceRetriever(_retriever), static_cast(&SoftSdfParser::readWorld)); @@ -80,7 +80,7 @@ simulation::WorldPtr SoftSdfParser::readSoftSdfFile( dynamics::SkeletonPtr SoftSdfParser::readSkeleton( const std::string& _filename, const ResourceRetrieverPtr& _retriever) { - return SdfParser::readSkeleton(_filename, _retriever, + return SdfParser::readSkeleton(_filename, getResourceRetriever(_retriever), static_cast(&SoftSdfParser::readSkeleton)); @@ -238,5 +238,8 @@ SdfParser::SDFBodyNode SoftSdfParser::readSoftBodyNode( return sdfBodyNode; } +ResourceRetrieverPtr getResourceRetriever( + const ResourceRetrieverPtr& _retriever); + } // namespace utils } // namespace dart diff --git a/dart/utils/sdf/SoftSdfParser.h b/dart/utils/sdf/SoftSdfParser.h index bebde5873816a..5905fe831d4f6 100644 --- a/dart/utils/sdf/SoftSdfParser.h +++ b/dart/utils/sdf/SoftSdfParser.h @@ -65,14 +65,14 @@ class SoftSdfParser : public SdfParser { public: /// \brief - // TODO: Make ResourceRetrieverPtr optional. static simulation::WorldPtr readSoftSdfFile( - const std::string& _filename, const ResourceRetrieverPtr& _retriever); + const std::string& _filename, + const ResourceRetrieverPtr& _retriever = nullptr); /// \brief - // TODO: Make ResourceRetrieverPtr optional. static dynamics::SkeletonPtr readSkeleton( - const std::string& _fileName, const ResourceRetrieverPtr& _retriever); + const std::string& _fileName, + const ResourceRetrieverPtr& _retriever = nullptr); static bool createSoftPair(dynamics::SkeletonPtr skeleton, dynamics::BodyNode* parent, From 9fe0c18d22bf4769bc64989685bbf59eb9577860 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 22:52:05 -0500 Subject: [PATCH 25/55] Use ResourceRetriever for SDF mesh loading. --- dart/utils/UriUtils.cpp | 30 ++++++++++++++++++++++++++++++ dart/utils/UriUtils.h | 3 +++ dart/utils/sdf/SdfParser.cpp | 12 ++++++++---- 3 files changed, 41 insertions(+), 4 deletions(-) diff --git a/dart/utils/UriUtils.cpp b/dart/utils/UriUtils.cpp index 1c004c2e0bbe1..6a2f807045f38 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/UriUtils.cpp @@ -317,6 +317,36 @@ std::string Uri::getUri(const std::string& _input) return ""; } +std::string Uri::getRelativeUri( + const std::string& _base, const std::string& _relative, bool _strict) +{ + Uri baseUri; + if(!baseUri.fromString(_base)) + { + dtwarn << "[getRelativeUri] Failed parsing base URI '" + << _base << "'.\n"; + return ""; + } + + Uri relativeUri; + if(!relativeUri.fromString(_relative)) + { + dtwarn << "[getRelativeUri] Failed parsing relative URI '" + << _relative << "'.\n"; + return ""; + } + + Uri mergedUri; + if(!mergedUri.fromRelativeUri(baseUri, relativeUri, _strict)) + { + dtwarn << "[getRelativeUri] Failed merging URI '" << _relative + << "' with base URI '" << _base << "'.\n"; + return ""; + } + + return mergedUri.toString(); +} + std::string Uri::mergePaths(const Uri& _base, const Uri& _relative) { assert(_base.mPath && "The path component is always defined."); diff --git a/dart/utils/UriUtils.h b/dart/utils/UriUtils.h index bdc15f1c08598..3a4f994ac070c 100644 --- a/dart/utils/UriUtils.h +++ b/dart/utils/UriUtils.h @@ -104,6 +104,9 @@ class Uri { std::string toString() const; static std::string getUri(const std::string& _input); + static std::string getRelativeUri(const std::string& _base, + const std::string& _relative, + bool _strict = false); private: // These are helper functions for implementing transform(); diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 40d22cecc008a..47bb1947ce3fd 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -21,6 +21,7 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" #include "dart/utils/LocalResourceRetriever.h" +#include "dart/utils/UriUtils.h" #include "dart/utils/sdf/SdfParser.h" namespace dart { @@ -628,12 +629,15 @@ dynamics::ShapePtr SdfParser::readShape( // TODO(JS): We assume that uri is just file name for the mesh std::string uri = getValueString(meshEle, "uri"); Eigen::Vector3d scale = getValueVector3d(meshEle, "scale"); - const aiScene* model = dynamics::MeshShape::loadMesh(_skelPath + uri); + + const std::string meshUri = Uri::getRelativeUri(_skelPath, uri); + const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, _retriever); + if (model) - newShape = dynamics::ShapePtr(new dynamics::MeshShape(scale, model, - _skelPath)); + newShape = std::make_shared( + scale, model, meshUri, true, _retriever); else - dterr << "Fail to load model[" << uri << "]." << std::endl; + dterr << "Fail to load model[" << meshUri << "]." << std::endl; } else { From f96e3e12dda0f1d3315c200300b33e6ee32d2189 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 23:32:12 -0500 Subject: [PATCH 26/55] Modified openXMLFile to use ResourceRetriever. --- dart/utils/Parser.cpp | 52 ++++++++++++++++++++++++------------ dart/utils/Parser.h | 4 ++- dart/utils/sdf/SdfParser.cpp | 4 +-- 3 files changed, 40 insertions(+), 20 deletions(-) diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index 7f6b0cea06bfd..a9cb630fc83ce 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -43,6 +43,7 @@ #include "dart/common/Console.h" #include "dart/math/Geometry.h" +#include "dart/utils/LocalResourceRetriever.h" namespace dart { namespace utils { @@ -585,25 +586,42 @@ tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, return _parentElement->FirstChildElement(_name.c_str()); } -void openXMLFile(tinyxml2::XMLDocument& doc, - const char* const filename) +void openXMLFile( + tinyxml2::XMLDocument& doc, const char* const filename, + const ResourceRetrieverPtr& _retriever) { - int const result = doc.LoadFile(filename); - switch(result) + ResourceRetrieverPtr retriever; + if(_retriever) + retriever = _retriever; + else + retriever = std::make_shared(); + + const ResourcePtr resource = retriever->retrieve(filename); + if(!resource) { - case tinyxml2::XML_SUCCESS: - break; - case tinyxml2::XML_ERROR_FILE_NOT_FOUND: - throw std::runtime_error("File not found"); - case tinyxml2::XML_ERROR_FILE_COULD_NOT_BE_OPENED: - throw std::runtime_error("File not found"); - default: - { - std::ostringstream oss; - oss << "Parse error = " << result; - throw std::runtime_error(oss.str()); - } - }; + dtwarn << "[openXMLFile] Failed opening URI '" + << filename << "'.\n"; + throw std::runtime_error("Failed opening URI."); + } + + // C++11 guarantees that std::string has contiguous storage. + const size_t size = resource->getFileSize(); + std::string content; + content.resize(size); + if(resource->read(&content.front(), size, 1) != 1) + { + dtwarn << "[openXMLFile] Failed reading from URI '" + << filename << "'.\n"; + throw std::runtime_error("Failed reading from URI."); + } + + int const result = doc.Parse(&content.front()); + if(result != tinyxml2::XML_SUCCESS) + { + dtwarn << "[openXMLFile] Failed parsing XML: TinyXML2 returned error" + " code " << result << ".\n"; + throw std::runtime_error("Failed parsing XML."); + } } bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) diff --git a/dart/utils/Parser.h b/dart/utils/Parser.h index 27d0074dc80c0..9298e8702d377 100644 --- a/dart/utils/Parser.h +++ b/dart/utils/Parser.h @@ -43,6 +43,7 @@ // http://www.grinninglizard.com/tinyxml2/index.html #include +#include "dart/utils/ResourceRetriever.h" #include "dart/math/MathTypes.h" namespace dart { @@ -91,7 +92,8 @@ Eigen::VectorXd getValueVectorXd (tinyxml2::XMLElement* _parentElement, const Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name); Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name); -void openXMLFile(tinyxml2::XMLDocument& doc, const char* const filename); +void openXMLFile(tinyxml2::XMLDocument& doc, const char* const filename, + const ResourceRetrieverPtr& _retriever = nullptr); bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); bool hasAttribute(tinyxml2::XMLElement* element, const char* const name); diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 47bb1947ce3fd..7cc0f138e751a 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -48,7 +48,7 @@ simulation::WorldPtr SdfParser::readSdfFile( tinyxml2::XMLDocument _dartFile; try { - openXMLFile(_dartFile, _filename.c_str()); + openXMLFile(_dartFile, _filename.c_str(), _retriever); } catch(std::exception const& e) { @@ -114,7 +114,7 @@ dynamics::SkeletonPtr SdfParser::readSkeleton( tinyxml2::XMLDocument _dartFile; try { - openXMLFile(_dartFile, _filename.c_str()); + openXMLFile(_dartFile, _filename.c_str(), _retriever); } catch(std::exception const& e) { From 956e0c5c7c8058f24193f31a69a9bb00b4dd6ad2 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 23:42:21 -0500 Subject: [PATCH 27/55] Updated SkelParser w/o backwards compat. --- dart/utils/SkelParser.cpp | 54 +++++++++++++++++++++++++-------------- dart/utils/SkelParser.h | 37 +++++++++++++++++++-------- 2 files changed, 62 insertions(+), 29 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 6027139d9b470..6df2cb82ba091 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -104,7 +104,8 @@ static tinyxml2::XMLElement* checkFormatAndGetWorldElement( } //============================================================================== -simulation::WorldPtr SkelParser::readWorld(const std::string& _filename) +simulation::WorldPtr SkelParser::readWorld( + const std::string& _filename, const ResourceRetrieverPtr& _retriever) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -128,11 +129,12 @@ simulation::WorldPtr SkelParser::readWorld(const std::string& _filename) return nullptr; } - return readWorld(worldElement); + return readWorld(worldElement, _filename, _retriever); } //============================================================================== -simulation::WorldPtr SkelParser::readWorldXML(const std::string& _xmlString) +simulation::WorldPtr SkelParser::readWorldXML(const std::string& _xmlString, + const std::string& _baseUri, const ResourceRetrieverPtr& _retriever) { tinyxml2::XMLDocument _dartXML; if(_dartXML.Parse(_xmlString.c_str()) != tinyxml2::XML_SUCCESS) @@ -148,11 +150,12 @@ simulation::WorldPtr SkelParser::readWorldXML(const std::string& _xmlString) return nullptr; } - return readWorld(worldElement); + return readWorld(worldElement, _baseUri, _retriever); } //============================================================================== -dynamics::SkeletonPtr SkelParser::readSkeleton(const std::string& _filename) +dynamics::SkeletonPtr SkelParser::readSkeleton( + const std::string& _filename, const ResourceRetrieverPtr& _retriever) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -191,13 +194,17 @@ dynamics::SkeletonPtr SkelParser::readSkeleton(const std::string& _filename) return nullptr; } - dynamics::SkeletonPtr newSkeleton = readSkeleton(skeletonElement); + dynamics::SkeletonPtr newSkeleton = readSkeleton( + skeletonElement, _filename, _retriever); return newSkeleton; } //============================================================================== -simulation::WorldPtr SkelParser::readWorld(tinyxml2::XMLElement* _worldElement) +simulation::WorldPtr SkelParser::readWorld( + tinyxml2::XMLElement* _worldElement, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { assert(_worldElement != nullptr); @@ -276,7 +283,7 @@ simulation::WorldPtr SkelParser::readWorld(tinyxml2::XMLElement* _worldElement) while (SkeletonElements.next()) { dynamics::SkeletonPtr newSkeleton - = readSkeleton(SkeletonElements.get()); + = readSkeleton(SkeletonElements.get(), _baseUri, _retriever); newWorld->addSkeleton(newSkeleton); } @@ -436,7 +443,9 @@ bool createJointAndNodePair(dynamics::SkeletonPtr skeleton, //============================================================================== dynamics::SkeletonPtr SkelParser::readSkeleton( - tinyxml2::XMLElement* _skeletonElement) + tinyxml2::XMLElement* _skeletonElement, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { assert(_skeletonElement != nullptr); @@ -472,7 +481,8 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( ElementEnumerator xmlBodies(_skeletonElement, "body"); while (xmlBodies.next()) { - SkelBodyNode newBodyNode = readSoftBodyNode(xmlBodies.get(), skeletonFrame); + SkelBodyNode newBodyNode = readSoftBodyNode( + xmlBodies.get(), skeletonFrame, _baseUri, _retriever); BodyMap::const_iterator it = bodyNodes.find(newBodyNode.properties->mName); if(it != bodyNodes.end()) @@ -549,7 +559,9 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( //============================================================================== SkelParser::SkelBodyNode SkelParser::readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, - const Eigen::Isometry3d& _skeletonFrame) + const Eigen::Isometry3d& _skeletonFrame, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { assert(_bodyNodeElement != nullptr); @@ -591,8 +603,8 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( ElementEnumerator vizShapes(_bodyNodeElement, "visualization_shape"); while (vizShapes.next()) { - dynamics::ShapePtr newShape = readShape(vizShapes.get(), - newBodyNode->mName); + dynamics::ShapePtr newShape = readShape( + vizShapes.get(), newBodyNode->mName, _baseUri, _retriever); if(newShape) newBodyNode->mVizShapes.push_back(newShape); @@ -603,8 +615,8 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( ElementEnumerator collShapes(_bodyNodeElement, "collision_shape"); while (collShapes.next()) { - dynamics::ShapePtr newShape = readShape(collShapes.get(), - newBodyNode->mName); + dynamics::ShapePtr newShape = readShape( + collShapes.get(), newBodyNode->mName, _baseUri, _retriever); if(newShape) newBodyNode->mColShapes.push_back(newShape); @@ -671,7 +683,9 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( //============================================================================== SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, - const Eigen::Isometry3d& _skeletonFrame) + const Eigen::Isometry3d& _skeletonFrame, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { //---------------------------------- Note ------------------------------------ // SoftBodyNode is created if _softBodyNodeElement has . @@ -680,8 +694,8 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( //---------------------------------------------------------------------------- assert(_softBodyNodeElement != nullptr); - SkelBodyNode standardBodyNode = readBodyNode(_softBodyNodeElement, - _skeletonFrame); + SkelBodyNode standardBodyNode = readBodyNode( + _softBodyNodeElement, _skeletonFrame, _baseUri, _retriever); // If _softBodyNodeElement has no , return rigid body node if (!hasElement(_softBodyNodeElement, "soft_shape")) @@ -781,7 +795,9 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( //============================================================================== dynamics::ShapePtr SkelParser::readShape(tinyxml2::XMLElement* vizEle, - const std::string& bodyName) + const std::string& bodyName, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { dynamics::ShapePtr newShape; diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index dae326d49280b..06ce63c3fa801 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -88,13 +88,17 @@ class SkelParser { public: /// Read World from skel file - static simulation::WorldPtr readWorld(const std::string& _filename); + static simulation::WorldPtr readWorld(const std::string& _filename, + const ResourceRetrieverPtr& _retriever); /// Read World from an xml-formatted string - static simulation::WorldPtr readWorldXML(const std::string& _xmlString); + static simulation::WorldPtr readWorldXML( + const std::string& _xmlString, + const std::string& _baseUri, const ResourceRetrieverPtr& _retriever); /// Read Skeleton from skel file - static dynamics::SkeletonPtr readSkeleton(const std::string& _filename); + static dynamics::SkeletonPtr readSkeleton(const std::string& _filename, + const ResourceRetrieverPtr& _retriever); typedef std::shared_ptr BodyPropPtr; @@ -134,23 +138,36 @@ class SkelParser protected: /// - static simulation::WorldPtr readWorld(tinyxml2::XMLElement* _worldElement); + static simulation::WorldPtr readWorld( + tinyxml2::XMLElement* _worldElement, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever); /// static dart::dynamics::SkeletonPtr readSkeleton( - tinyxml2::XMLElement* _skeletonElement); + tinyxml2::XMLElement* _skeletonElement, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever); /// - static SkelBodyNode readBodyNode(tinyxml2::XMLElement* _bodyElement, - const Eigen::Isometry3d& _skeletonFrame); + static SkelBodyNode readBodyNode( + tinyxml2::XMLElement* _bodyElement, + const Eigen::Isometry3d& _skeletonFrame, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever); /// - static SkelBodyNode readSoftBodyNode(tinyxml2::XMLElement* _softBodyNodeElement, - const Eigen::Isometry3d& _skeletonFrame); + static SkelBodyNode readSoftBodyNode( + tinyxml2::XMLElement* _softBodyNodeElement, + const Eigen::Isometry3d& _skeletonFrame, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever); /// static dynamics::ShapePtr readShape(tinyxml2::XMLElement* _shapeElement, - const std::string& bodyName); + const std::string& bodyName, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever); /// Read marker static dynamics::Marker::Properties readMarker( From 4467332fc510dfe4567d0aa76f9d04c6449ae5db Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 23:47:27 -0500 Subject: [PATCH 28/55] Fixed backwards compatability. --- dart/utils/SkelParser.cpp | 39 ++++++++++++++++++++++++++++++--------- dart/utils/SkelParser.h | 15 ++++++++++----- 2 files changed, 40 insertions(+), 14 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 6df2cb82ba091..905011f33e7bd 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -71,6 +71,8 @@ #include "dart/dynamics/Marker.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" +#include "dart/utils/LocalResourceRetriever.h" +#include "dart/utils/UriUtils.h" namespace dart { namespace utils { @@ -105,14 +107,17 @@ static tinyxml2::XMLElement* checkFormatAndGetWorldElement( //============================================================================== simulation::WorldPtr SkelParser::readWorld( - const std::string& _filename, const ResourceRetrieverPtr& _retriever) + const std::string& _filename, + const ResourceRetrieverPtr& _retriever) { + const ResourceRetrieverPtr retriever = getRetriever(_retriever); + //-------------------------------------------------------------------------- // Load xml and create Document tinyxml2::XMLDocument _dartFile; try { - openXMLFile(_dartFile, _filename.c_str()); + openXMLFile(_dartFile, _filename.c_str(), retriever); } catch(std::exception const& e) { @@ -129,13 +134,17 @@ simulation::WorldPtr SkelParser::readWorld( return nullptr; } - return readWorld(worldElement, _filename, _retriever); + return readWorld(worldElement, _filename, retriever); } //============================================================================== -simulation::WorldPtr SkelParser::readWorldXML(const std::string& _xmlString, - const std::string& _baseUri, const ResourceRetrieverPtr& _retriever) +simulation::WorldPtr SkelParser::readWorldXML( + const std::string& _xmlString, + const std::string& _baseUri, + const ResourceRetrieverPtr& _retriever) { + const ResourceRetrieverPtr retriever = getRetriever(_retriever); + tinyxml2::XMLDocument _dartXML; if(_dartXML.Parse(_xmlString.c_str()) != tinyxml2::XML_SUCCESS) { @@ -150,19 +159,22 @@ simulation::WorldPtr SkelParser::readWorldXML(const std::string& _xmlString, return nullptr; } - return readWorld(worldElement, _baseUri, _retriever); + return readWorld(worldElement, _baseUri, retriever); } //============================================================================== dynamics::SkeletonPtr SkelParser::readSkeleton( - const std::string& _filename, const ResourceRetrieverPtr& _retriever) + const std::string& _filename, + const ResourceRetrieverPtr& _retriever) { + const ResourceRetrieverPtr retriever = getRetriever(_retriever); + //-------------------------------------------------------------------------- // Load xml and create Document tinyxml2::XMLDocument _dartFile; try { - openXMLFile(_dartFile, _filename.c_str()); + openXMLFile(_dartFile, _filename.c_str(), retriever); } catch(std::exception const& e) { @@ -195,7 +207,7 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( } dynamics::SkeletonPtr newSkeleton = readSkeleton( - skeletonElement, _filename, _retriever); + skeletonElement, _filename, retriever); return newSkeleton; } @@ -1952,5 +1964,14 @@ SkelParser::JointPropPtr SkelParser::readFreeJoint( properties); } +ResourceRetrieverPtr SkelParser::getRetriever( + const ResourceRetrieverPtr& _retriever) +{ + if(_retriever) + return _retriever; + else + return std::make_shared(); +} + } // namespace utils } // namespace dart diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index 06ce63c3fa801..15a429caa5118 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -88,17 +88,20 @@ class SkelParser { public: /// Read World from skel file - static simulation::WorldPtr readWorld(const std::string& _filename, - const ResourceRetrieverPtr& _retriever); + static simulation::WorldPtr readWorld( + const std::string& _filename, + const ResourceRetrieverPtr& _retriever = nullptr); /// Read World from an xml-formatted string static simulation::WorldPtr readWorldXML( const std::string& _xmlString, - const std::string& _baseUri, const ResourceRetrieverPtr& _retriever); + const std::string& _baseUri = "", + const ResourceRetrieverPtr& _retriever = nullptr); /// Read Skeleton from skel file - static dynamics::SkeletonPtr readSkeleton(const std::string& _filename, - const ResourceRetrieverPtr& _retriever); + static dynamics::SkeletonPtr readSkeleton( + const std::string& _filename, + const ResourceRetrieverPtr& _retriever = nullptr); typedef std::shared_ptr BodyPropPtr; @@ -240,6 +243,8 @@ class SkelParser tinyxml2::XMLElement* _jointElement, SkelJoint& _joint, const std::string& _name); + + static ResourceRetrieverPtr getRetriever(const ResourceRetrieverPtr& _retriever); }; } // namespace utils From a08abd5997640017322323bfe04b339acf65dc55 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 20 Jul 2015 23:57:37 -0500 Subject: [PATCH 29/55] Load meshes using relative URIs in SkelParser. --- dart/utils/SkelParser.cpp | 9 +++++---- data/skel/bullet_collision.skel | 4 ++-- data/skel/shapes.skel | 6 +++--- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 905011f33e7bd..6f5744aeaa634 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -860,12 +860,13 @@ dynamics::ShapePtr SkelParser::readShape(tinyxml2::XMLElement* vizEle, tinyxml2::XMLElement* meshEle = getElement(geometryEle, "mesh"); std::string filename = getValueString(meshEle, "file_name"); Eigen::Vector3d scale = getValueVector3d(meshEle, "scale"); - // TODO(JS): Do we assume that all mesh files place at DART_DATA_PATH? - const aiScene* model = dynamics::MeshShape::loadMesh(DART_DATA_PATH + - filename); + + const std::string meshUri = Uri::getRelativeUri(_baseUri, filename); + const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, _retriever); if (model) { - newShape = dynamics::ShapePtr(new dynamics::MeshShape(scale, model)); + newShape = std::make_shared( + scale, model, meshUri, true, _retriever); } else { diff --git a/data/skel/bullet_collision.skel b/data/skel/bullet_collision.skel index 604a114f911fe..d818c768cd611 100644 --- a/data/skel/bullet_collision.skel +++ b/data/skel/bullet_collision.skel @@ -147,7 +147,7 @@ 0 0 0 0 0 0 - obj/foot.obj + ../obj/foot.obj 1.0 1.0 1.0 @@ -157,7 +157,7 @@ 0 0 0 0 0 0 - obj/foot.obj + ../obj/foot.obj 1.0 1.0 1.0 diff --git a/data/skel/shapes.skel b/data/skel/shapes.skel index c0f1f8545366b..f4b981f14cedb 100644 --- a/data/skel/shapes.skel +++ b/data/skel/shapes.skel @@ -180,7 +180,7 @@ 0 0 0 0 0 0 - obj/foot.obj + ../obj/foot.obj 1.0 1.0 1.0 @@ -190,7 +190,7 @@ 0 0 0 0 0 0 - obj/foot.obj + ../obj/foot.obj 1.0 1.0 1.0 @@ -204,4 +204,4 @@ - \ No newline at end of file + From 238b112389f8a7c60ee358f2badf01e6fdbf1f96 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 00:05:03 -0500 Subject: [PATCH 30/55] Removed unnecessary isUri flag. --- dart/dynamics/MeshShape.cpp | 19 ++++++++++++------- dart/dynamics/MeshShape.h | 8 ++++---- dart/utils/SkelParser.cpp | 2 +- dart/utils/sdf/SdfParser.cpp | 2 +- dart/utils/urdf/DartLoader.cpp | 2 +- 5 files changed, 19 insertions(+), 14 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 1e75887d4ad02..b80358d9e11a7 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -50,6 +50,7 @@ #include "dart/common/Console.h" #include "dart/utils/AssimpInputResourceAdaptor.h" #include "dart/utils/LocalResourceRetriever.h" +#include "dart/utils/UriUtils.h" // We define our own constructor for aiScene, because it seems to be missing // from the standard assimp library @@ -158,7 +159,7 @@ static std::string extractPathFromUri(const std::string &_uri) } MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, - const std::string &_path, bool _isUri, + const std::string &_path, const utils::ResourceRetrieverPtr& _resourceRetriever) : Shape(MESH), mResourceRetriever(_resourceRetriever), @@ -168,7 +169,7 @@ MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, assert(_scale[1] > 0.0); assert(_scale[2] > 0.0); - setMesh(_mesh, _path, _isUri, _resourceRetriever); + setMesh(_mesh, _path, _resourceRetriever); setScale(_scale); } @@ -191,7 +192,7 @@ const std::string &MeshShape::getMeshPath() const } void MeshShape::setMesh( - const aiScene* _mesh, const std::string &_path, bool _isUri, + const aiScene* _mesh, const std::string &_path, const utils::ResourceRetrieverPtr& _resourceRetriever) { mMesh = _mesh; @@ -203,15 +204,19 @@ void MeshShape::setMesh( return; } - if(_isUri) + utils::Uri uri; + if(uri.fromString(_path)) { mMeshUri = _path; - mMeshPath = extractPathFromUri(_path); + + if(uri.mScheme.get_value_or("file") == "file") + mMeshPath = uri.mPath.get_value_or(""); } else { - mMeshUri = "file://" + _path; - mMeshPath = _path; + dtwarn << "[MeshShape::setMesh] Failed parsing URI '" << _path << "'.\n"; + mMeshUri = ""; + mMeshPath = ""; } mResourceRetriever = _resourceRetriever; diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 48ed37d18ff9b..6551575f51e2a 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -59,8 +59,9 @@ class MeshShape : public Shape { public: /// \brief Constructor. MeshShape( - const Eigen::Vector3d& _scale, const aiScene* _mesh, - const std::string &_path = std::string(), bool _isUri = false, + const Eigen::Vector3d& _scale, + const aiScene* _mesh, + const std::string &_path = "", const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief Destructor. @@ -71,8 +72,7 @@ class MeshShape : public Shape { /// \brief void setMesh( - const aiScene* _mesh, - const std::string &path = std::string(), bool _isUri = false, + const aiScene* _mesh, const std::string &path = "", const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief URI to the mesh; an empty string if unavailable. diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 6f5744aeaa634..36dbbfe6e2b38 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -866,7 +866,7 @@ dynamics::ShapePtr SkelParser::readShape(tinyxml2::XMLElement* vizEle, if (model) { newShape = std::make_shared( - scale, model, meshUri, true, _retriever); + scale, model, meshUri, _retriever); } else { diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 7cc0f138e751a..4468ba9b8c394 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -635,7 +635,7 @@ dynamics::ShapePtr SdfParser::readShape( if (model) newShape = std::make_shared( - scale, model, meshUri, true, _retriever); + scale, model, meshUri, _retriever); else dterr << "Fail to load model[" << meshUri << "]." << std::endl; } diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index cd68d6a6007be..d0b2827d4f4e0 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -589,7 +589,7 @@ dynamics::ShapePtr DartLoader::createShape( const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shape = std::make_shared( - scale, scene, resolvedUri, true, _resourceRetriever); + scale, scene, resolvedUri, _resourceRetriever); } // Unknown geometry type else From 6618f95660ce2527aed248d3a7bfc57f233e4b5b Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 00:11:04 -0500 Subject: [PATCH 31/55] Renamed UriUtils to Uri. --- dart/dynamics/MeshShape.cpp | 2 +- dart/utils/LocalResourceRetriever.cpp | 2 +- dart/utils/SchemaResourceRetriever.cpp | 2 +- dart/utils/SkelParser.cpp | 2 +- dart/utils/{UriUtils.cpp => Uri.cpp} | 2 +- dart/utils/{UriUtils.h => Uri.h} | 6 +++--- dart/utils/sdf/SdfParser.cpp | 2 +- dart/utils/urdf/DartLoader.h | 2 +- unittests/{testUriHelpers.cpp => testUri.cpp} | 0 9 files changed, 10 insertions(+), 10 deletions(-) rename dart/utils/{UriUtils.cpp => Uri.cpp} (99%) rename dart/utils/{UriUtils.h => Uri.h} (97%) rename unittests/{testUriHelpers.cpp => testUri.cpp} (100%) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index b80358d9e11a7..fec15a447703d 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -50,7 +50,7 @@ #include "dart/common/Console.h" #include "dart/utils/AssimpInputResourceAdaptor.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" // We define our own constructor for aiScene, because it seems to be missing // from the standard assimp library diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index 316af4472f446..d4c8ed54b1dd3 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -1,7 +1,7 @@ #include #include #include "dart/common/Console.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" #include "dart/utils/LocalResourceRetriever.h" #include "LocalResource.h" diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index e19c4b6b6c469..3aa3c801ba63a 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -1,7 +1,7 @@ #include #include "dart/common/Console.h" #include "dart/utils/SchemaResourceRetriever.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" namespace dart { namespace utils { diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 36dbbfe6e2b38..0d216da7252f3 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -72,7 +72,7 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" namespace dart { namespace utils { diff --git a/dart/utils/UriUtils.cpp b/dart/utils/Uri.cpp similarity index 99% rename from dart/utils/UriUtils.cpp rename to dart/utils/Uri.cpp index 6a2f807045f38..26c9886fb13e5 100644 --- a/dart/utils/UriUtils.cpp +++ b/dart/utils/Uri.cpp @@ -2,7 +2,7 @@ #include #include #include "dart/common/Console.h" -#include "UriUtils.h" +#include "Uri.h" // std::regex is only implemented in GCC 4.9 and above; i.e. libstdc++ 6.0.20 // or above. In fact, it contains major bugs in GCC 4.8 [1]. There is no diff --git a/dart/utils/UriUtils.h b/dart/utils/Uri.h similarity index 97% rename from dart/utils/UriUtils.h rename to dart/utils/Uri.h index 3a4f994ac070c..c481c4870282e 100644 --- a/dart/utils/UriUtils.h +++ b/dart/utils/Uri.h @@ -32,8 +32,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_URIUTILS_H_ -#define DART_UTILS_URIUTILS_H_ +#ifndef DART_UTILS_URI_H_ +#define DART_UTILS_URI_H_ namespace dart { namespace utils { @@ -118,4 +118,4 @@ class Uri { } // namespace utils } // namespace dart -#endif // ifndef DART_UTILS_URIUTILS_H_ +#endif // ifndef DART_UTILS_URI_H_ diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 4468ba9b8c394..1535e4f96f088 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -21,7 +21,7 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" #include "dart/utils/sdf/SdfParser.h" namespace dart { diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 1e5d0a2a72771..fb706d53ae3e0 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -19,7 +19,7 @@ #include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" #include "dart/utils/SchemaResourceRetriever.h" -#include "dart/utils/UriUtils.h" +#include "dart/utils/Uri.h" namespace urdf { diff --git a/unittests/testUriHelpers.cpp b/unittests/testUri.cpp similarity index 100% rename from unittests/testUriHelpers.cpp rename to unittests/testUri.cpp From c214bab33905de13eaffcdc930f795867b446927 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 00:45:11 -0500 Subject: [PATCH 32/55] Moved ResourceRetriever and Uri into common. --- dart/{utils => common}/Resource.h | 8 +-- dart/{utils => common}/ResourceRetriever.h | 8 +-- dart/{utils => common}/Uri.cpp | 4 +- dart/{utils => common}/Uri.h | 9 ++-- dart/dynamics/MeshShape.cpp | 12 ++--- dart/dynamics/MeshShape.h | 10 ++-- dart/utils/AssimpInputResourceAdaptor.cpp | 8 +-- dart/utils/AssimpInputResourceAdaptor.h | 12 ++--- dart/utils/LocalResource.h | 4 +- dart/utils/LocalResourceRetriever.cpp | 8 +-- dart/utils/LocalResourceRetriever.h | 6 +-- dart/utils/PackageResourceRetriever.cpp | 5 +- dart/utils/PackageResourceRetriever.h | 10 ++-- dart/utils/Parser.cpp | 6 +-- dart/utils/Parser.h | 4 +- dart/utils/SchemaResourceRetriever.cpp | 22 ++++---- dart/utils/SchemaResourceRetriever.h | 16 +++--- dart/utils/SkelParser.cpp | 30 +++++------ dart/utils/SkelParser.h | 18 +++---- dart/utils/sdf/SdfParser.cpp | 48 ++++++++--------- dart/utils/sdf/SdfParser.h | 44 ++++++++-------- dart/utils/sdf/SoftSdfParser.cpp | 20 ++++---- dart/utils/sdf/SoftSdfParser.h | 10 ++-- dart/utils/urdf/DartLoader.cpp | 60 +++++++++++----------- dart/utils/urdf/DartLoader.h | 42 +++++++-------- unittests/TestHelpers.h | 12 ++--- unittests/testLocalResourceRetriever.cpp | 2 +- unittests/testPackageResourceRetriever.cpp | 6 +-- unittests/testSchemaResourceRetriever.cpp | 6 +-- unittests/testUri.cpp | 4 +- 30 files changed, 226 insertions(+), 228 deletions(-) rename dart/{utils => common}/Resource.h (95%) rename dart/{utils => common}/ResourceRetriever.h (94%) rename dart/{utils => common}/Uri.cpp (99%) rename dart/{utils => common}/Uri.h (97%) diff --git a/dart/utils/Resource.h b/dart/common/Resource.h similarity index 95% rename from dart/utils/Resource.h rename to dart/common/Resource.h index 549b049659fd5..41c66b9c1c830 100644 --- a/dart/utils/Resource.h +++ b/dart/common/Resource.h @@ -33,13 +33,13 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_RESOURCE_H_ -#define DART_UTILS_RESOURCE_H_ +#ifndef DART_COMMON_RESOURCE_H_ +#define DART_COMMON_RESOURCE_H_ #include #include namespace dart { -namespace utils { +namespace common { class Resource { @@ -63,7 +63,7 @@ class Resource typedef std::shared_ptr ResourcePtr; -} // namespace utils +} // namespace common } // namespace dart #endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ diff --git a/dart/utils/ResourceRetriever.h b/dart/common/ResourceRetriever.h similarity index 94% rename from dart/utils/ResourceRetriever.h rename to dart/common/ResourceRetriever.h index 571bf69cb489a..ced7bee578fd6 100644 --- a/dart/utils/ResourceRetriever.h +++ b/dart/common/ResourceRetriever.h @@ -32,15 +32,15 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_RESOURCERETRIEVER_H_ -#define DART_UTILS_RESOURCERETRIEVER_H_ +#ifndef DART_COMMON_RESOURCERETRIEVER_H_ +#define DART_COMMON_RESOURCERETRIEVER_H_ #include #include #include "Resource.h" namespace dart { -namespace utils { +namespace common { struct ResourceRetriever { virtual ~ResourceRetriever() = default; @@ -51,7 +51,7 @@ struct ResourceRetriever { typedef std::shared_ptr ResourceRetrieverPtr; -} // namespace utils +} // namespace common } // namespace dart #endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ diff --git a/dart/utils/Uri.cpp b/dart/common/Uri.cpp similarity index 99% rename from dart/utils/Uri.cpp rename to dart/common/Uri.cpp index 26c9886fb13e5..c1fe887e79dc0 100644 --- a/dart/utils/Uri.cpp +++ b/dart/common/Uri.cpp @@ -38,7 +38,7 @@ static bool startsWith(const std::string& _target, const std::string& _prefix) } namespace dart { -namespace utils { +namespace common { /* * UriComponent @@ -450,5 +450,5 @@ std::string Uri::removeDotSegments(const std::string& _path) return output; } -} // namespace utils +} // namespace common } // namespace dart diff --git a/dart/utils/Uri.h b/dart/common/Uri.h similarity index 97% rename from dart/utils/Uri.h rename to dart/common/Uri.h index c481c4870282e..f65229115d82c 100644 --- a/dart/utils/Uri.h +++ b/dart/common/Uri.h @@ -32,11 +32,11 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_URI_H_ -#define DART_UTILS_URI_H_ +#ifndef DART_COMMON_URI_H_ +#define DART_COMMON_URI_H_ namespace dart { -namespace utils { +namespace common { class UriComponent { public: @@ -110,12 +110,11 @@ class Uri { private: // These are helper functions for implementing transform(); - static std::string mergePaths(const Uri& _base, const Uri& _relative); static std::string removeDotSegments(const std::string& _path); }; -} // namespace utils +} // namespace common } // namespace dart #endif // ifndef DART_UTILS_URI_H_ diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index fec15a447703d..160b5ba94c47f 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -36,8 +36,6 @@ */ #include "dart/dynamics/MeshShape.h" -#include "dart/utils/LocalResourceRetriever.h" - #include #include #include @@ -50,7 +48,7 @@ #include "dart/common/Console.h" #include "dart/utils/AssimpInputResourceAdaptor.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/Uri.h" +#include "dart/common/Uri.h" // We define our own constructor for aiScene, because it seems to be missing // from the standard assimp library @@ -160,7 +158,7 @@ static std::string extractPathFromUri(const std::string &_uri) MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, const std::string &_path, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) : Shape(MESH), mResourceRetriever(_resourceRetriever), mDisplayList(0) @@ -193,7 +191,7 @@ const std::string &MeshShape::getMeshPath() const void MeshShape::setMesh( const aiScene* _mesh, const std::string &_path, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) { mMesh = _mesh; @@ -204,7 +202,7 @@ void MeshShape::setMesh( return; } - utils::Uri uri; + common::Uri uri; if(uri.fromString(_path)) { mMeshUri = _path; @@ -316,7 +314,7 @@ void MeshShape::_updateBoundingBoxDim() { } const aiScene* MeshShape::loadMesh( - const std::string& _uri, const utils::ResourceRetrieverPtr& _retriever) + const std::string& _uri, const common::ResourceRetrieverPtr& _retriever) { // Remove points and lines from the import. aiPropertyStore* propertyStore = aiCreatePropertyStore(); diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 6551575f51e2a..e1abf7682a51c 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -43,7 +43,7 @@ #include #include "dart/dynamics/Shape.h" -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" namespace Assimp { @@ -62,7 +62,7 @@ class MeshShape : public Shape { const Eigen::Vector3d& _scale, const aiScene* _mesh, const std::string &_path = "", - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief Destructor. virtual ~MeshShape(); @@ -73,7 +73,7 @@ class MeshShape : public Shape { /// \brief void setMesh( const aiScene* _mesh, const std::string &path = "", - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief URI to the mesh; an empty string if unavailable. const std::string &getMeshUri() const; @@ -103,7 +103,7 @@ class MeshShape : public Shape { /// \brief static const aiScene* loadMesh( - const std::string& _uri, const utils::ResourceRetrieverPtr& _retriever); + const std::string& _uri, const common::ResourceRetrieverPtr& _retriever); // Documentation inherited. virtual Eigen::Matrix3d computeInertia(double _mass) const; @@ -127,7 +127,7 @@ class MeshShape : public Shape { std::string mMeshPath; /// \brief Optional method of loading resources by URI. - utils::ResourceRetrieverPtr mResourceRetriever; + common::ResourceRetrieverPtr mResourceRetriever; /// \brief OpenGL DisplayList id for rendering int mDisplayList; diff --git a/dart/utils/AssimpInputResourceAdaptor.cpp b/dart/utils/AssimpInputResourceAdaptor.cpp index eae1cc17dc163..49a01970e7b5c 100644 --- a/dart/utils/AssimpInputResourceAdaptor.cpp +++ b/dart/utils/AssimpInputResourceAdaptor.cpp @@ -11,7 +11,7 @@ namespace utils { * AssimpInputResourceRetrieverWrapper */ AssimpInputResourceRetrieverAdaptor::AssimpInputResourceRetrieverAdaptor( - const ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) : mResourceRetriever(_resourceRetriever) { } @@ -39,7 +39,7 @@ Assimp::IOStream* AssimpInputResourceRetrieverAdaptor::Open( return nullptr; } - if(const ResourcePtr resource = mResourceRetriever->retrieve(pFile)) + if(const common::ResourcePtr resource = mResourceRetriever->retrieve(pFile)) return new AssimpInputResourceAdaptor(resource); else return nullptr; @@ -56,7 +56,7 @@ void AssimpInputResourceRetrieverAdaptor::Close(Assimp::IOStream* pFile) * AssimpInputResourceAdaptor */ AssimpInputResourceAdaptor::AssimpInputResourceAdaptor( - const ResourcePtr& _resource) + const common::ResourcePtr& _resource) : mResource(_resource) { assert(_resource); @@ -78,6 +78,8 @@ size_t AssimpInputResourceAdaptor::Write( aiReturn AssimpInputResourceAdaptor::Seek(size_t pOffset, aiOrigin pOrigin) { + using common::Resource; + Resource::SeekType origin; switch(pOrigin) { diff --git a/dart/utils/AssimpInputResourceAdaptor.h b/dart/utils/AssimpInputResourceAdaptor.h index 9b83546907615..5b255e3dd22a6 100644 --- a/dart/utils/AssimpInputResourceAdaptor.h +++ b/dart/utils/AssimpInputResourceAdaptor.h @@ -38,8 +38,8 @@ #include #include #include -#include "dart/utils/Resource.h" -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/Resource.h" +#include "dart/common/ResourceRetriever.h" namespace dart { namespace utils { @@ -48,7 +48,7 @@ class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem { public: AssimpInputResourceRetrieverAdaptor( - const ResourceRetrieverPtr& _resourceRetriever); + const common::ResourceRetrieverPtr& _resourceRetriever); virtual ~AssimpInputResourceRetrieverAdaptor() = default; /// \brief Tests for the existence of a file at the given path. @@ -80,13 +80,13 @@ class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem void Close(Assimp::IOStream* pFile) override; private: - ResourceRetrieverPtr mResourceRetriever; + common::ResourceRetrieverPtr mResourceRetriever; }; class AssimpInputResourceAdaptor : public Assimp::IOStream { public: - AssimpInputResourceAdaptor(const ResourcePtr& _resource); + AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); virtual ~AssimpInputResourceAdaptor() = default; /// \brief Read from the file @@ -117,7 +117,7 @@ class AssimpInputResourceAdaptor : public Assimp::IOStream void Flush() override; private: - ResourcePtr mResource; + common::ResourcePtr mResource; }; diff --git a/dart/utils/LocalResource.h b/dart/utils/LocalResource.h index ab26edf47d3d8..0a2868c058281 100644 --- a/dart/utils/LocalResource.h +++ b/dart/utils/LocalResource.h @@ -35,12 +35,12 @@ */ #ifndef DART_UTILS_LOCALRESOURCE_H_ #define DART_UTILS_LOCALRESOURCE_H_ -#include "Resource.h" +#include "dart/common/Resource.h" namespace dart { namespace utils { -class LocalResource : public virtual Resource +class LocalResource : public virtual common::Resource { public: LocalResource(const std::string& _path); diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/utils/LocalResourceRetriever.cpp index d4c8ed54b1dd3..0fdb02edef7e6 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/utils/LocalResourceRetriever.cpp @@ -1,7 +1,7 @@ #include #include #include "dart/common/Console.h" -#include "dart/utils/Uri.h" +#include "dart/common/Uri.h" #include "dart/utils/LocalResourceRetriever.h" #include "LocalResource.h" @@ -10,7 +10,7 @@ namespace utils { bool LocalResourceRetriever::exists(const std::string& _uri) { - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; @@ -25,9 +25,9 @@ bool LocalResourceRetriever::exists(const std::string& _uri) return std::ifstream(*uri.mScheme, std::ios::binary).good(); } -ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) +common::ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) { - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; diff --git a/dart/utils/LocalResourceRetriever.h b/dart/utils/LocalResourceRetriever.h index 518a06edfa339..e4d363f4ee6f9 100644 --- a/dart/utils/LocalResourceRetriever.h +++ b/dart/utils/LocalResourceRetriever.h @@ -35,19 +35,19 @@ */ #ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ #define DART_UTILS_LOCALRESOURCERETRIEVER_H_ -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" namespace dart { namespace utils { /// \brief Retrieve local resources specified by file:// URIs. -class LocalResourceRetriever : public virtual ResourceRetriever +class LocalResourceRetriever : public virtual common::ResourceRetriever { public: virtual ~LocalResourceRetriever() = default; bool exists(const std::string& _uri) override; - ResourcePtr retrieve(const std::string& _uri) override; + common::ResourcePtr retrieve(const std::string& _uri) override; }; typedef std::shared_ptr LocalResourceRetrieverPtr; diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 6f1f8b84d9403..a7d41edb4a045 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -3,14 +3,13 @@ #include #include "dart/common/Console.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/ResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" namespace dart { namespace utils { PackageResourceRetriever::PackageResourceRetriever( - const ResourceRetrieverPtr& localRetriever) + const common::ResourceRetrieverPtr& localRetriever) { if (localRetriever) mLocalRetriever = localRetriever; @@ -47,7 +46,7 @@ bool PackageResourceRetriever::exists(const std::string& _uri) return false; } -ResourcePtr PackageResourceRetriever::retrieve(const std::string& _uri) +common::ResourcePtr PackageResourceRetriever::retrieve(const std::string& _uri) { std::string packageName, relativePath; if (!resolvePackageUri(_uri, packageName, relativePath)) diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index be28f68bc874d..6360014b7a3f6 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -37,17 +37,17 @@ #define DART_UTILS_PACKAGERESOURCERETRIEVER_H_ #include #include -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" namespace dart { namespace utils { /// \brief Retrieve local resources specified by package:// URIs. -class PackageResourceRetriever : public virtual ResourceRetriever +class PackageResourceRetriever : public virtual common::ResourceRetriever { public: PackageResourceRetriever( - const ResourceRetrieverPtr& localRetriever = nullptr); + const common::ResourceRetrieverPtr& localRetriever = nullptr); virtual ~PackageResourceRetriever() = default; @@ -76,10 +76,10 @@ class PackageResourceRetriever : public virtual ResourceRetriever const std::string& _packageDirectory); bool exists(const std::string& _uri) override; - ResourcePtr retrieve(const std::string& _uri) override; + common::ResourcePtr retrieve(const std::string& _uri) override; private: - ResourceRetrieverPtr mLocalRetriever; + common::ResourceRetrieverPtr mLocalRetriever; std::unordered_map > mPackageMap; const std::vector& getPackagePaths( diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index a9cb630fc83ce..b066a0b760f97 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -588,15 +588,15 @@ tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, void openXMLFile( tinyxml2::XMLDocument& doc, const char* const filename, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { - ResourceRetrieverPtr retriever; + common::ResourceRetrieverPtr retriever; if(_retriever) retriever = _retriever; else retriever = std::make_shared(); - const ResourcePtr resource = retriever->retrieve(filename); + const common::ResourcePtr resource = retriever->retrieve(filename); if(!resource) { dtwarn << "[openXMLFile] Failed opening URI '" diff --git a/dart/utils/Parser.h b/dart/utils/Parser.h index 9298e8702d377..d80bb9071b881 100644 --- a/dart/utils/Parser.h +++ b/dart/utils/Parser.h @@ -43,7 +43,7 @@ // http://www.grinninglizard.com/tinyxml2/index.html #include -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" #include "dart/math/MathTypes.h" namespace dart { @@ -93,7 +93,7 @@ Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name); void openXMLFile(tinyxml2::XMLDocument& doc, const char* const filename, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); bool hasAttribute(tinyxml2::XMLElement* element, const char* const name); diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index 3aa3c801ba63a..1009c5fb3523a 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -1,19 +1,19 @@ #include #include "dart/common/Console.h" +#include "dart/common/Uri.h" #include "dart/utils/SchemaResourceRetriever.h" -#include "dart/utils/Uri.h" namespace dart { namespace utils { void SchemaResourceRetriever::addDefaultRetriever( - const ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) { mDefaultResourceRetrievers.push_back(_resourceRetriever); } bool SchemaResourceRetriever::addSchemaRetriever( - const std::string& _schema, const ResourceRetrieverPtr& _resourceRetriever) + const std::string& _schema, const common::ResourceRetrieverPtr& _resourceRetriever) { if(!_resourceRetriever) { @@ -36,7 +36,7 @@ bool SchemaResourceRetriever::addSchemaRetriever( bool SchemaResourceRetriever::exists(const std::string& _uri) { - for(const ResourceRetrieverPtr& resourceRetriever : getRetrievers(_uri)) + for(const common::ResourceRetrieverPtr& resourceRetriever : getRetrievers(_uri)) { if(resourceRetriever->exists(_uri)) return true; @@ -44,12 +44,12 @@ bool SchemaResourceRetriever::exists(const std::string& _uri) return false; } -ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) +common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) { - const std::vector &retrievers = getRetrievers(_uri); - for(const ResourceRetrieverPtr& resourceRetriever : retrievers) + const std::vector &retrievers = getRetrievers(_uri); + for(const common::ResourceRetrieverPtr& resourceRetriever : retrievers) { - if(ResourcePtr resource = resourceRetriever->retrieve(_uri)) + if(common::ResourcePtr resource = resourceRetriever->retrieve(_uri)) return resource; } @@ -60,12 +60,12 @@ ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) return nullptr; } -std::vector SchemaResourceRetriever::getRetrievers( +std::vector SchemaResourceRetriever::getRetrievers( const std::string& _uri) { const std::string schema = getSchema(_uri); - std::vector retrievers; + std::vector retrievers; const auto it = mResourceRetrievers.find(schema); if(it != std::end(mResourceRetrievers)) @@ -91,7 +91,7 @@ std::vector SchemaResourceRetriever::getRetrievers( std::string SchemaResourceRetriever::getSchema(const std::string& _uri) { - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { dtwarn << "[SchemaResourceRetriever::retrieve] Failed parsing URI:" diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index e701ab66e842a..e7b6c166e0ef8 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -37,33 +37,33 @@ #define DART_UTILS_SCHEMARESOURCERETRIEVER_H_ #include #include -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" namespace dart { namespace utils { -class SchemaResourceRetriever : public virtual ResourceRetriever +class SchemaResourceRetriever : public virtual common::ResourceRetriever { public: virtual ~SchemaResourceRetriever() = default; - void addDefaultRetriever(const ResourceRetrieverPtr& _resourceRetriever); + void addDefaultRetriever(const common::ResourceRetrieverPtr& _resourceRetriever); bool addSchemaRetriever( const std::string& _schema, - const ResourceRetrieverPtr& _resourceRetriever); + const common::ResourceRetrieverPtr& _resourceRetriever); bool exists(const std::string& _uri) override; - ResourcePtr retrieve(const std::string& _uri) override; + common::ResourcePtr retrieve(const std::string& _uri) override; private: - std::vector getRetrievers(const std::string& _uri); + std::vector getRetrievers(const std::string& _uri); std::string getSchema(const std::string& _uri); std::unordered_map > mResourceRetrievers; - std::vector mDefaultResourceRetrievers; + std::vector > mResourceRetrievers; + std::vector mDefaultResourceRetrievers; }; typedef std::shared_ptr SchemaResourceRetrieverPtr; diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 0d216da7252f3..ea8445bdec23e 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -72,7 +72,7 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/Uri.h" +#include "dart/common/Uri.h" namespace dart { namespace utils { @@ -108,9 +108,9 @@ static tinyxml2::XMLElement* checkFormatAndGetWorldElement( //============================================================================== simulation::WorldPtr SkelParser::readWorld( const std::string& _filename, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { - const ResourceRetrieverPtr retriever = getRetriever(_retriever); + const common::ResourceRetrieverPtr retriever = getRetriever(_retriever); //-------------------------------------------------------------------------- // Load xml and create Document @@ -141,9 +141,9 @@ simulation::WorldPtr SkelParser::readWorld( simulation::WorldPtr SkelParser::readWorldXML( const std::string& _xmlString, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { - const ResourceRetrieverPtr retriever = getRetriever(_retriever); + const common::ResourceRetrieverPtr retriever = getRetriever(_retriever); tinyxml2::XMLDocument _dartXML; if(_dartXML.Parse(_xmlString.c_str()) != tinyxml2::XML_SUCCESS) @@ -165,9 +165,9 @@ simulation::WorldPtr SkelParser::readWorldXML( //============================================================================== dynamics::SkeletonPtr SkelParser::readSkeleton( const std::string& _filename, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { - const ResourceRetrieverPtr retriever = getRetriever(_retriever); + const common::ResourceRetrieverPtr retriever = getRetriever(_retriever); //-------------------------------------------------------------------------- // Load xml and create Document @@ -216,7 +216,7 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( simulation::WorldPtr SkelParser::readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { assert(_worldElement != nullptr); @@ -457,7 +457,7 @@ bool createJointAndNodePair(dynamics::SkeletonPtr skeleton, dynamics::SkeletonPtr SkelParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { assert(_skeletonElement != nullptr); @@ -573,7 +573,7 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { assert(_bodyNodeElement != nullptr); @@ -697,7 +697,7 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { //---------------------------------- Note ------------------------------------ // SoftBodyNode is created if _softBodyNodeElement has . @@ -809,7 +809,7 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( dynamics::ShapePtr SkelParser::readShape(tinyxml2::XMLElement* vizEle, const std::string& bodyName, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { dynamics::ShapePtr newShape; @@ -861,7 +861,7 @@ dynamics::ShapePtr SkelParser::readShape(tinyxml2::XMLElement* vizEle, std::string filename = getValueString(meshEle, "file_name"); Eigen::Vector3d scale = getValueVector3d(meshEle, "scale"); - const std::string meshUri = Uri::getRelativeUri(_baseUri, filename); + const std::string meshUri = common::Uri::getRelativeUri(_baseUri, filename); const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, _retriever); if (model) { @@ -1965,8 +1965,8 @@ SkelParser::JointPropPtr SkelParser::readFreeJoint( properties); } -ResourceRetrieverPtr SkelParser::getRetriever( - const ResourceRetrieverPtr& _retriever) +common::ResourceRetrieverPtr SkelParser::getRetriever( + const common::ResourceRetrieverPtr& _retriever) { if(_retriever) return _retriever; diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index 15a429caa5118..875f0f7f918d3 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -90,18 +90,18 @@ class SkelParser /// Read World from skel file static simulation::WorldPtr readWorld( const std::string& _filename, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); /// Read World from an xml-formatted string static simulation::WorldPtr readWorldXML( const std::string& _xmlString, const std::string& _baseUri = "", - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); /// Read Skeleton from skel file static dynamics::SkeletonPtr readSkeleton( const std::string& _filename, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); typedef std::shared_ptr BodyPropPtr; @@ -144,33 +144,33 @@ class SkelParser static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// static dart::dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// static SkelBodyNode readBodyNode( tinyxml2::XMLElement* _bodyElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// static SkelBodyNode readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// static dynamics::ShapePtr readShape(tinyxml2::XMLElement* _shapeElement, const std::string& bodyName, const std::string& _baseUri, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// Read marker static dynamics::Marker::Properties readMarker( @@ -244,7 +244,7 @@ class SkelParser SkelJoint& _joint, const std::string& _name); - static ResourceRetrieverPtr getRetriever(const ResourceRetrieverPtr& _retriever); + static common::ResourceRetrieverPtr getRetriever(const common::ResourceRetrieverPtr& _retriever); }; } // namespace utils diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 1535e4f96f088..21e6510fff697 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -21,27 +21,27 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" #include "dart/utils/LocalResourceRetriever.h" -#include "dart/utils/Uri.h" +#include "dart/common/Uri.h" #include "dart/utils/sdf/SdfParser.h" namespace dart { namespace utils { simulation::WorldPtr SdfParser::readSdfFile( - const std::string& _filename, const ResourceRetrieverPtr& _retriever) + const std::string& _filename, const common::ResourceRetrieverPtr& _retriever) { return readSdfFile(_filename, getResourceRetriever(_retriever), static_cast(&SdfParser::readWorld)); + const common::ResourceRetrieverPtr&)>(&SdfParser::readWorld)); } simulation::WorldPtr SdfParser::readSdfFile( const std::string& _filename, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function xmlReader) + const common::ResourceRetrieverPtr&)> xmlReader) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -94,20 +94,20 @@ simulation::WorldPtr SdfParser::readSdfFile( } dynamics::SkeletonPtr SdfParser::readSkeleton( - const std::string& _fileName, const ResourceRetrieverPtr& _retriever) + const std::string& _fileName, const common::ResourceRetrieverPtr& _retriever) { return readSkeleton(_fileName, getResourceRetriever(_retriever), static_cast(&SdfParser::readSkeleton)); + const common::ResourceRetrieverPtr&)>(&SdfParser::readSkeleton)); } dynamics::SkeletonPtr SdfParser::readSkeleton( const std::string& _filename, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function xmlReader) + const common::ResourceRetrieverPtr&)> xmlReader) { //-------------------------------------------------------------------------- // Load xml and create Document @@ -163,21 +163,21 @@ dynamics::SkeletonPtr SdfParser::readSkeleton( simulation::WorldPtr SdfParser::readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { return readWorld(_worldElement, _skelPath, _retriever, static_cast(&SdfParser::readSkeleton)); + const common::ResourceRetrieverPtr&)>(&SdfParser::readSkeleton)); } simulation::WorldPtr SdfParser::readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function skeletonReader) + const common::ResourceRetrieverPtr&)> skeletonReader) { assert(_worldElement != nullptr); @@ -242,7 +242,7 @@ void SdfParser::readPhysics(tinyxml2::XMLElement* _physicsElement, dynamics::SkeletonPtr SdfParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { return readSkeleton(_skeletonElement, _skelPath, _retriever, &readBodyNode, &createPair); } @@ -250,11 +250,11 @@ dynamics::SkeletonPtr SdfParser::readSkeleton( dynamics::SkeletonPtr SdfParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function bodyReader, + const common::ResourceRetrieverPtr&)> bodyReader, std::function bodyReader) + const common::ResourceRetrieverPtr&)> bodyReader) { ElementEnumerator bodies(_skeletonElement, "link"); BodyMap sdfBodyNodes; @@ -458,7 +458,7 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { assert(_bodyNodeElement != nullptr); @@ -576,7 +576,7 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( dynamics::ShapePtr SdfParser::readShape( tinyxml2::XMLElement* _shapelement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { dynamics::ShapePtr newShape; @@ -630,7 +630,7 @@ dynamics::ShapePtr SdfParser::readShape( std::string uri = getValueString(meshEle, "uri"); Eigen::Vector3d scale = getValueVector3d(meshEle, "scale"); - const std::string meshUri = Uri::getRelativeUri(_skelPath, uri); + const std::string meshUri = common::Uri::getRelativeUri(_skelPath, uri); const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, _retriever); if (model) @@ -1061,8 +1061,8 @@ dynamics::FreeJoint::Properties SdfParser::readFreeJoint( return dynamics::FreeJoint::Properties(); } -ResourceRetrieverPtr SdfParser::getResourceRetriever( - const ResourceRetrieverPtr& _retriever) +common::ResourceRetrieverPtr SdfParser::getResourceRetriever( + const common::ResourceRetrieverPtr& _retriever) { if(_retriever) return _retriever; diff --git a/dart/utils/sdf/SdfParser.h b/dart/utils/sdf/SdfParser.h index 781cda6a2dd14..15febd885cb4b 100644 --- a/dart/utils/sdf/SdfParser.h +++ b/dart/utils/sdf/SdfParser.h @@ -11,7 +11,7 @@ #include #include "dart/utils/Parser.h" -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/WeldJoint.h" #include "dart/dynamics/RevoluteJoint.h" @@ -51,30 +51,30 @@ class SdfParser { public: /// \brief - // TODO: Make ResourceRetriever optional. + // TODO: Make common::ResourceRetriever optional. static dart::simulation::WorldPtr readSdfFile( const std::string& _filename, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); static simulation::WorldPtr readSdfFile( const std::string& _filename, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function xmlReader); + const common::ResourceRetrieverPtr&)> xmlReader); /// \brief - // TODO: Make ResourceRetriever optional. + // TODO: Make common::ResourceRetriever optional. static dynamics::SkeletonPtr readSkeleton( const std::string& _fileName, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); static dynamics::SkeletonPtr readSkeleton( const std::string& _fileName, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function xmlReader); + const common::ResourceRetrieverPtr&)> xmlReader); typedef std::shared_ptr BodyPropPtr; @@ -105,16 +105,16 @@ class SdfParser static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// \brief static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function skeletonReader); + const common::ResourceRetrieverPtr&)> skeletonReader); /// \brief static void readPhysics(tinyxml2::XMLElement* _physicsElement, @@ -124,17 +124,17 @@ class SdfParser static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever, + const common::ResourceRetrieverPtr& _retriever, std::function bodyReader, + const common::ResourceRetrieverPtr&)> bodyReader, std::function bodyReader); + const common::ResourceRetrieverPtr&)> bodyReader); /// \brief static SDFBodyNode readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// \brief static dynamics::ShapePtr readShape( tinyxml2::XMLElement* _shapelement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// \brief static JointMap readAllJoints( @@ -293,8 +293,8 @@ class SdfParser const Eigen::Isometry3d& _parentModelFrame, const std::string& _name); - static ResourceRetrieverPtr getResourceRetriever( - const ResourceRetrieverPtr& _retriever); + static common::ResourceRetrieverPtr getResourceRetriever( + const common::ResourceRetrieverPtr& _retriever); }; } // namespace utils diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index e7176c37cc8bf..2d2ff2eb68dd1 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -69,21 +69,21 @@ namespace dart { namespace utils { simulation::WorldPtr SoftSdfParser::readSoftSdfFile( - const std::string& _filename, const ResourceRetrieverPtr& _retriever) + const std::string& _filename, const common::ResourceRetrieverPtr& _retriever) { return SdfParser::readSdfFile(_filename, getResourceRetriever(_retriever), static_cast(&SoftSdfParser::readWorld)); + const common::ResourceRetrieverPtr&)>(&SoftSdfParser::readWorld)); } dynamics::SkeletonPtr SoftSdfParser::readSkeleton( - const std::string& _filename, const ResourceRetrieverPtr& _retriever) + const std::string& _filename, const common::ResourceRetrieverPtr& _retriever) { return SdfParser::readSkeleton(_filename, getResourceRetriever(_retriever), static_cast(&SoftSdfParser::readSkeleton)); + const common::ResourceRetrieverPtr&)>(&SoftSdfParser::readSkeleton)); } bool SoftSdfParser::createSoftPair( @@ -115,18 +115,18 @@ bool SoftSdfParser::createSoftPair( simulation::WorldPtr SoftSdfParser::readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { return SdfParser::readWorld(_worldElement, _skelPath, _retriever, static_cast(&SoftSdfParser::readSkeleton)); + const common::ResourceRetrieverPtr&)>(&SoftSdfParser::readSkeleton)); } dynamics::SkeletonPtr SoftSdfParser::readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { return SdfParser::readSkeleton(_skeletonElement, _skelPath, _retriever, &readSoftBodyNode, &createSoftPair); @@ -136,7 +136,7 @@ SdfParser::SDFBodyNode SoftSdfParser::readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever) + const common::ResourceRetrieverPtr& _retriever) { //---------------------------------- Note ------------------------------------ // SoftBodyNode is created if _softBodyNodeElement has . @@ -238,8 +238,8 @@ SdfParser::SDFBodyNode SoftSdfParser::readSoftBodyNode( return sdfBodyNode; } -ResourceRetrieverPtr getResourceRetriever( - const ResourceRetrieverPtr& _retriever); +common::ResourceRetrieverPtr getResourceRetriever( + const common::ResourceRetrieverPtr& _retriever); } // namespace utils } // namespace dart diff --git a/dart/utils/sdf/SoftSdfParser.h b/dart/utils/sdf/SoftSdfParser.h index 5905fe831d4f6..e94ae75910916 100644 --- a/dart/utils/sdf/SoftSdfParser.h +++ b/dart/utils/sdf/SoftSdfParser.h @@ -67,12 +67,12 @@ class SoftSdfParser : public SdfParser /// \brief static simulation::WorldPtr readSoftSdfFile( const std::string& _filename, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); /// \brief static dynamics::SkeletonPtr readSkeleton( const std::string& _fileName, - const ResourceRetrieverPtr& _retriever = nullptr); + const common::ResourceRetrieverPtr& _retriever = nullptr); static bool createSoftPair(dynamics::SkeletonPtr skeleton, dynamics::BodyNode* parent, @@ -83,20 +83,20 @@ class SoftSdfParser : public SdfParser static simulation::WorldPtr readWorld( tinyxml2::XMLElement* _worldElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// \brief static dynamics::SkeletonPtr readSkeleton( tinyxml2::XMLElement* _skeletonElement, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); /// \brief static SDFBodyNode readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, const Eigen::Isometry3d& _skeletonFrame, const std::string& _skelPath, - const ResourceRetrieverPtr& _retriever); + const common::ResourceRetrieverPtr& _retriever); }; } // namespace utils diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index d0b2827d4f4e0..9483851a6595b 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -56,12 +56,12 @@ void DartLoader::addPackageDirectory(const std::string& _packageName, dynamics::SkeletonPtr DartLoader::parseSkeleton( const std::string& _uri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) { - const utils::ResourceRetrieverPtr resourceRetriever + const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { dtwarn << "[DartLoader::parseSkeleton] Failed parsing URI: " @@ -87,8 +87,8 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton( } dynamics::SkeletonPtr DartLoader::parseSkeletonString( - const std::string& _urdfString, const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const std::string& _urdfString, const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { if(_urdfString.empty()) { @@ -110,12 +110,12 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString( simulation::WorldPtr DartLoader::parseWorld( const std::string& _uri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::ResourceRetrieverPtr& _resourceRetriever) { - const utils::ResourceRetrieverPtr resourceRetriever + const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { dtwarn << "[DartLoader::parseSkeleton] Failed parsing URI: " @@ -131,10 +131,10 @@ simulation::WorldPtr DartLoader::parseWorld( } simulation::WorldPtr DartLoader::parseWorldString( - const std::string& _urdfString, const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const std::string& _urdfString, const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { - const utils::ResourceRetrieverPtr resourceRetriever + const common::ResourceRetrieverPtr resourceRetriever = getResourceRetriever(_resourceRetriever); if(_urdfString.empty()) @@ -276,8 +276,8 @@ bool DartLoader::parseWorldToEntityPaths( */ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( const urdf::ModelInterface* _model, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(_model->getName()); @@ -337,8 +337,8 @@ bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parentNode, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::BodyNode::Properties properties; if (!createDartNodeProperties(_lk, &properties, _baseUri, _resourceRetriever)) @@ -365,11 +365,11 @@ bool DartLoader::createSkeletonRecursive( * @function readXml */ bool DartLoader::readFileToString( - const utils::ResourceRetrieverPtr& _resourceRetriever, + const common::ResourceRetrieverPtr& _resourceRetriever, const std::string &_uri, std::string &_output) { - const ResourcePtr resource = _resourceRetriever->retrieve(_uri); + const common::ResourcePtr resource = _resourceRetriever->retrieve(_uri); if (!resource) return false; @@ -389,8 +389,8 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::Joint::Properties basicProperties; @@ -479,8 +479,8 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( bool DartLoader::createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties *node, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { node->mName = _lk->name; @@ -544,8 +544,8 @@ void setMaterial(dynamics::ShapePtr _shape, const urdf::Collision* _col) { template dynamics::ShapePtr DartLoader::createShape( const VisualOrCollision* _vizOrCol, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever) + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::ShapePtr shape; @@ -571,7 +571,7 @@ dynamics::ShapePtr DartLoader::createShape( else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { // Resolve relative URIs. - Uri relativeUri, absoluteUri; + common::Uri relativeUri, absoluteUri; if(!absoluteUri.fromRelativeUri(_baseUri, mesh->filename)) { dtwarn << "[DartLoader::createShape] Failed resolving mesh URI '" @@ -605,8 +605,8 @@ dynamics::ShapePtr DartLoader::createShape( return shape; } -utils::ResourceRetrieverPtr DartLoader::getResourceRetriever( - const utils::ResourceRetrieverPtr& _resourceRetriever) +common::ResourceRetrieverPtr DartLoader::getResourceRetriever( + const common::ResourceRetrieverPtr& _resourceRetriever) { if (_resourceRetriever) return _resourceRetriever; @@ -616,12 +616,12 @@ utils::ResourceRetrieverPtr DartLoader::getResourceRetriever( template dynamics::ShapePtr DartLoader::createShape( const urdf::Visual* _vizOrCol, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); template dynamics::ShapePtr DartLoader::createShape( const urdf::Collision* _vizOrCol, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); /** * @function pose2Affine3d diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index fb706d53ae3e0..b54f530eb46fe 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -15,11 +15,11 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/simulation/World.h" -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/Uri.h" +#include "dart/common/ResourceRetriever.h" #include "dart/utils/LocalResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" #include "dart/utils/SchemaResourceRetriever.h" -#include "dart/utils/Uri.h" namespace urdf { @@ -79,21 +79,21 @@ class DartLoader { /// Parse a file to produce a Skeleton dynamics::SkeletonPtr parseSkeleton( const std::string& _uri, - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a text string to produce a Skeleton dynamics::SkeletonPtr parseSkeletonString( - const std::string& _urdfString, const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const std::string& _urdfString, const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a file to produce a World dart::simulation::WorldPtr parseWorld(const std::string& _uri, - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// Parse a text string to produce a World dart::simulation::WorldPtr parseWorldString( - const std::string& _urdfString, const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever = nullptr); + const std::string& _urdfString, const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); private: typedef std::shared_ptr BodyPropPtr; @@ -105,43 +105,43 @@ class DartLoader { static dart::dynamics::SkeletonPtr modelInterfaceToSkeleton( const urdf::ModelInterface* _model, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); static bool createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); template static dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); static dynamics::BodyNode* createDartJointAndNode( const urdf::Joint* _jt, const dynamics::BodyNode::Properties& _body, dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); static bool createDartNodeProperties( const urdf::Link* _lk, dynamics::BodyNode::Properties *properties, - const Uri& _baseUri, - const utils::ResourceRetrieverPtr& _resourceRetriever); + const common::Uri& _baseUri, + const common::ResourceRetrieverPtr& _resourceRetriever); - utils::ResourceRetrieverPtr getResourceRetriever( - const utils::ResourceRetrieverPtr& _resourceRetriever); + common::ResourceRetrieverPtr getResourceRetriever( + const common::ResourceRetrieverPtr& _resourceRetriever); static Eigen::Isometry3d toEigen(const urdf::Pose& _pose); static Eigen::Vector3d toEigen(const urdf::Vector3& _vector); static bool readFileToString( - const utils::ResourceRetrieverPtr& _resourceRetriever, + const common::ResourceRetrieverPtr& _resourceRetriever, const std::string &_uri, std::string &_output); diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 36ccdc506215f..65130ac5273a5 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -53,7 +53,7 @@ #include "dart/collision/CollisionDetector.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/simulation/World.h" -#include "dart/utils/ResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" using namespace Eigen; using namespace dart::math; @@ -377,7 +377,7 @@ SkeletonPtr createBox( } //============================================================================== -struct TestResource : public dart::utils::Resource +struct TestResource : public dart::common::Resource { size_t getFileSize() override { @@ -401,7 +401,7 @@ struct TestResource : public dart::utils::Resource }; //============================================================================== -struct PresentResourceRetriever : public dart::utils::ResourceRetriever +struct PresentResourceRetriever : public dart::common::ResourceRetriever { bool exists(const std::string& _uri) override { @@ -409,7 +409,7 @@ struct PresentResourceRetriever : public dart::utils::ResourceRetriever return true; } - dart::utils::ResourcePtr retrieve(const std::string& _uri) override + dart::common::ResourcePtr retrieve(const std::string& _uri) override { mRetrieve.push_back(_uri); return std::make_shared(); @@ -420,7 +420,7 @@ struct PresentResourceRetriever : public dart::utils::ResourceRetriever }; //============================================================================== -struct AbsentResourceRetriever : public dart::utils::ResourceRetriever +struct AbsentResourceRetriever : public dart::common::ResourceRetriever { bool exists(const std::string& _uri) override { @@ -428,7 +428,7 @@ struct AbsentResourceRetriever : public dart::utils::ResourceRetriever return false; } - dart::utils::ResourcePtr retrieve(const std::string& _uri) override + dart::common::ResourcePtr retrieve(const std::string& _uri) override { mRetrieve.push_back(_uri); return nullptr; diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp index dd1803d09384a..55845352f6c04 100644 --- a/unittests/testLocalResourceRetriever.cpp +++ b/unittests/testLocalResourceRetriever.cpp @@ -38,7 +38,7 @@ #include "dart/utils/LocalResourceRetriever.h" #include "TestHelpers.h" -using dart::utils::Resource; +using dart::common::Resource; using dart::utils::LocalResourceRetriever; TEST(LocalResourceRetriever, exists_UnsupportedUri_ReturnsFalse) diff --git a/unittests/testPackageResourceRetriever.cpp b/unittests/testPackageResourceRetriever.cpp index 18a4e7d06aaa5..84a7f4a26d923 100644 --- a/unittests/testPackageResourceRetriever.cpp +++ b/unittests/testPackageResourceRetriever.cpp @@ -38,9 +38,9 @@ #include "dart/utils/PackageResourceRetriever.h" #include "TestHelpers.h" -using dart::utils::Resource; -using dart::utils::ResourcePtr; -using dart::utils::ResourceRetriever; +using dart::common::Resource; +using dart::common::ResourcePtr; +using dart::common::ResourceRetriever; using dart::utils::PackageResourceRetriever; TEST(PackageResourceRetriever, exists_UnableToResolve_ReturnsFalse) diff --git a/unittests/testSchemaResourceRetriever.cpp b/unittests/testSchemaResourceRetriever.cpp index 3f2f428b18df0..9f073492be680 100644 --- a/unittests/testSchemaResourceRetriever.cpp +++ b/unittests/testSchemaResourceRetriever.cpp @@ -38,9 +38,9 @@ #include "dart/utils/SchemaResourceRetriever.h" #include "TestHelpers.h" -using dart::utils::Resource; -using dart::utils::ResourcePtr; -using dart::utils::ResourceRetriever; +using dart::common::Resource; +using dart::common::ResourcePtr; +using dart::common::ResourceRetriever; using dart::utils::SchemaResourceRetriever; TEST(SchemaResourceRetriever, exists_NothingRegistered_ReturnsFalse) diff --git a/unittests/testUri.cpp b/unittests/testUri.cpp index 08eca63d4d057..470c93da3ce59 100644 --- a/unittests/testUri.cpp +++ b/unittests/testUri.cpp @@ -35,10 +35,10 @@ */ #include -#include "dart/utils/UriUtils.h" +#include "dart/common/Uri.h" #include "TestHelpers.h" -using dart::utils::Uri; +using dart::common::Uri; TEST(UriHelpers, fromString_ValidUri_ReturnsTrue) { From d4693073c66d3d420190a4a846d6ad99c2e69f14 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 00:51:12 -0500 Subject: [PATCH 33/55] Moved LocalResourceRetriever to common. --- dart/{utils => common}/LocalResource.cpp | 6 +++--- dart/{utils => common}/LocalResource.h | 12 ++++++------ dart/{utils => common}/LocalResourceRetriever.cpp | 6 +++--- dart/{utils => common}/LocalResourceRetriever.h | 14 +++++++------- dart/dynamics/MeshShape.cpp | 4 ++-- dart/utils/PackageResourceRetriever.cpp | 4 ++-- dart/utils/Parser.cpp | 4 ++-- dart/utils/SkelParser.cpp | 4 ++-- dart/utils/sdf/SdfParser.cpp | 4 ++-- dart/utils/urdf/DartLoader.cpp | 2 +- dart/utils/urdf/DartLoader.h | 4 ++-- unittests/testLocalResourceRetriever.cpp | 4 ++-- 12 files changed, 34 insertions(+), 34 deletions(-) rename dart/{utils => common}/LocalResource.cpp (98%) rename dart/{utils => common}/LocalResource.h (91%) rename dart/{utils => common}/LocalResourceRetriever.cpp (92%) rename dart/{utils => common}/LocalResourceRetriever.h (86%) diff --git a/dart/utils/LocalResource.cpp b/dart/common/LocalResource.cpp similarity index 98% rename from dart/utils/LocalResource.cpp rename to dart/common/LocalResource.cpp index e52502f883402..83cc728d78ed1 100644 --- a/dart/utils/LocalResource.cpp +++ b/dart/common/LocalResource.cpp @@ -1,10 +1,10 @@ #include #include -#include "LocalResource.h" #include "dart/common/Console.h" +#include "LocalResource.h" namespace dart { -namespace utils { +namespace common { LocalResource::LocalResource(const std::string& _path) : mFile(std::fopen(_path.c_str(), "rb")) @@ -134,6 +134,6 @@ size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) return result; } -} // namespace utils +} // namespace common } // namespace dart diff --git a/dart/utils/LocalResource.h b/dart/common/LocalResource.h similarity index 91% rename from dart/utils/LocalResource.h rename to dart/common/LocalResource.h index 0a2868c058281..6577b6d174e05 100644 --- a/dart/utils/LocalResource.h +++ b/dart/common/LocalResource.h @@ -33,14 +33,14 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_LOCALRESOURCE_H_ -#define DART_UTILS_LOCALRESOURCE_H_ +#ifndef DART_COMMON_LOCALRESOURCE_H_ +#define DART_COMMON_LOCALRESOURCE_H_ #include "dart/common/Resource.h" namespace dart { -namespace utils { +namespace common { -class LocalResource : public virtual common::Resource +class LocalResource : public virtual Resource { public: LocalResource(const std::string& _path); @@ -68,7 +68,7 @@ class LocalResource : public virtual common::Resource std::FILE* mFile; }; -} // namespace utils +} // namespace common } // namespace dart -#endif // ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ +#endif // ifndef DART_COMMON_LOCALRESOURCERETRIEVER_H_ diff --git a/dart/utils/LocalResourceRetriever.cpp b/dart/common/LocalResourceRetriever.cpp similarity index 92% rename from dart/utils/LocalResourceRetriever.cpp rename to dart/common/LocalResourceRetriever.cpp index 0fdb02edef7e6..733b85aed2d5f 100644 --- a/dart/utils/LocalResourceRetriever.cpp +++ b/dart/common/LocalResourceRetriever.cpp @@ -2,11 +2,11 @@ #include #include "dart/common/Console.h" #include "dart/common/Uri.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "LocalResourceRetriever.h" #include "LocalResource.h" namespace dart { -namespace utils { +namespace common { bool LocalResourceRetriever::exists(const std::string& _uri) { @@ -44,5 +44,5 @@ common::ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) return nullptr; } -} // namespace utils +} // namespace common } // namespace dart diff --git a/dart/utils/LocalResourceRetriever.h b/dart/common/LocalResourceRetriever.h similarity index 86% rename from dart/utils/LocalResourceRetriever.h rename to dart/common/LocalResourceRetriever.h index e4d363f4ee6f9..3b706f83a11d6 100644 --- a/dart/utils/LocalResourceRetriever.h +++ b/dart/common/LocalResourceRetriever.h @@ -33,26 +33,26 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ -#define DART_UTILS_LOCALRESOURCERETRIEVER_H_ +#ifndef DART_COMMON_LOCALRESOURCERETRIEVER_H_ +#define DART_COMMON_LOCALRESOURCERETRIEVER_H_ #include "dart/common/ResourceRetriever.h" namespace dart { -namespace utils { +namespace common { /// \brief Retrieve local resources specified by file:// URIs. -class LocalResourceRetriever : public virtual common::ResourceRetriever +class LocalResourceRetriever : public virtual ResourceRetriever { public: virtual ~LocalResourceRetriever() = default; bool exists(const std::string& _uri) override; - common::ResourcePtr retrieve(const std::string& _uri) override; + ResourcePtr retrieve(const std::string& _uri) override; }; typedef std::shared_ptr LocalResourceRetrieverPtr; -} // namespace utils +} // namespace common } // namespace dart -#endif // ifndef DART_UTILS_LOCALRESOURCERETRIEVER_H_ +#endif // ifndef DART_COMMON_LOCALRESOURCERETRIEVER_H_ diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 160b5ba94c47f..389836bc9f57c 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -47,7 +47,7 @@ #include "dart/renderer/RenderInterface.h" #include "dart/common/Console.h" #include "dart/utils/AssimpInputResourceAdaptor.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "dart/common/Uri.h" // We define our own constructor for aiScene, because it seems to be missing @@ -377,7 +377,7 @@ const aiScene* MeshShape::loadMesh( const aiScene* MeshShape::loadMesh(const std::string& _fileName) { - const auto retriever = std::make_shared(); + const auto retriever = std::make_shared(); return loadMesh("file://" + _fileName, retriever); } diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index a7d41edb4a045..3a6b04648af8e 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -2,7 +2,7 @@ #include #include #include "dart/common/Console.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" namespace dart { @@ -14,7 +14,7 @@ PackageResourceRetriever::PackageResourceRetriever( if (localRetriever) mLocalRetriever = localRetriever; else - mLocalRetriever = std::make_shared(); + mLocalRetriever = std::make_shared(); } void PackageResourceRetriever::addPackageDirectory( diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index b066a0b760f97..fdf5152cbfe7b 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -43,7 +43,7 @@ #include "dart/common/Console.h" #include "dart/math/Geometry.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" namespace dart { namespace utils { @@ -594,7 +594,7 @@ void openXMLFile( if(_retriever) retriever = _retriever; else - retriever = std::make_shared(); + retriever = std::make_shared(); const common::ResourcePtr resource = retriever->retrieve(filename); if(!resource) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index ea8445bdec23e..ad2e400fc8fb9 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -71,7 +71,7 @@ #include "dart/dynamics/Marker.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "dart/common/Uri.h" namespace dart { @@ -1971,7 +1971,7 @@ common::ResourceRetrieverPtr SkelParser::getRetriever( if(_retriever) return _retriever; else - return std::make_shared(); + return std::make_shared(); } } // namespace utils diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 21e6510fff697..1a31f8e457f7e 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -20,7 +20,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "dart/common/Uri.h" #include "dart/utils/sdf/SdfParser.h" @@ -1067,7 +1067,7 @@ common::ResourceRetrieverPtr SdfParser::getResourceRetriever( if(_retriever) return _retriever; else - return std::make_shared(); + return std::make_shared(); } } // namespace utils diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 9483851a6595b..2e3051a1d10c7 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -38,7 +38,7 @@ namespace dart { namespace utils { DartLoader::DartLoader() - : mLocalRetriever(new utils::LocalResourceRetriever) + : mLocalRetriever(new common::LocalResourceRetriever) , mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)) , mRetriever(new utils::SchemaResourceRetriever) { diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index b54f530eb46fe..f3ef8108cb8cc 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -17,7 +17,7 @@ #include "dart/simulation/World.h" #include "dart/common/Uri.h" #include "dart/common/ResourceRetriever.h" -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" #include "dart/utils/SchemaResourceRetriever.h" @@ -145,7 +145,7 @@ class DartLoader { const std::string &_uri, std::string &_output); - utils::LocalResourceRetrieverPtr mLocalRetriever; + common::LocalResourceRetrieverPtr mLocalRetriever; utils::PackageResourceRetrieverPtr mPackageRetriever; utils::SchemaResourceRetrieverPtr mRetriever; }; diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp index 55845352f6c04..772ef81d80bf0 100644 --- a/unittests/testLocalResourceRetriever.cpp +++ b/unittests/testLocalResourceRetriever.cpp @@ -35,11 +35,11 @@ */ #include -#include "dart/utils/LocalResourceRetriever.h" +#include "dart/common/LocalResourceRetriever.h" #include "TestHelpers.h" using dart::common::Resource; -using dart::utils::LocalResourceRetriever; +using dart::common::LocalResourceRetriever; TEST(LocalResourceRetriever, exists_UnsupportedUri_ReturnsFalse) { From 73fa9c3feb2d7ffae4b7072a5ee93d976e214dbf Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 00:55:40 -0500 Subject: [PATCH 34/55] Moved AssimpResourceAdaptor to dynamics. --- .../{utils => dynamics}/AssimpInputResourceAdaptor.cpp | 4 ++-- dart/{utils => dynamics}/AssimpInputResourceAdaptor.h | 10 +++++----- dart/dynamics/MeshShape.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) rename dart/{utils => dynamics}/AssimpInputResourceAdaptor.cpp (99%) rename dart/{utils => dynamics}/AssimpInputResourceAdaptor.h (95%) diff --git a/dart/utils/AssimpInputResourceAdaptor.cpp b/dart/dynamics/AssimpInputResourceAdaptor.cpp similarity index 99% rename from dart/utils/AssimpInputResourceAdaptor.cpp rename to dart/dynamics/AssimpInputResourceAdaptor.cpp index 49a01970e7b5c..9ebe0d538d402 100644 --- a/dart/utils/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.cpp @@ -5,7 +5,7 @@ #include "AssimpInputResourceAdaptor.h" namespace dart { -namespace utils { +namespace dynamics { /* * AssimpInputResourceRetrieverWrapper @@ -203,5 +203,5 @@ aiFileIO createFileIO(Assimp::IOSystem* _system) return out; } -} // namespace utils +} // namespace dynamics } // namespace dart diff --git a/dart/utils/AssimpInputResourceAdaptor.h b/dart/dynamics/AssimpInputResourceAdaptor.h similarity index 95% rename from dart/utils/AssimpInputResourceAdaptor.h rename to dart/dynamics/AssimpInputResourceAdaptor.h index 5b255e3dd22a6..6cc867a129c38 100644 --- a/dart/utils/AssimpInputResourceAdaptor.h +++ b/dart/dynamics/AssimpInputResourceAdaptor.h @@ -33,8 +33,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ -#define DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ +#ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_H_ +#define DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_H_ #include #include #include @@ -42,7 +42,7 @@ #include "dart/common/ResourceRetriever.h" namespace dart { -namespace utils { +namespace dynamics { class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem { @@ -124,7 +124,7 @@ class AssimpInputResourceAdaptor : public Assimp::IOStream aiFileIO createFileIO(Assimp::IOSystem* adaptor); aiFile createFile(Assimp::IOStream* adaptor); -} // namespace utils +} // namespace dynamics } // namespace dart -#endif // ifndef DART_UTILS_ASSIMPINPUTRESOURCEADAPTOR_H_ +#endif // ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_H_ diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 389836bc9f57c..3eb98a29bcf02 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -46,7 +46,7 @@ #include "dart/renderer/RenderInterface.h" #include "dart/common/Console.h" -#include "dart/utils/AssimpInputResourceAdaptor.h" +#include "dart/dynamics/AssimpInputResourceAdaptor.h" #include "dart/common/LocalResourceRetriever.h" #include "dart/common/Uri.h" @@ -327,8 +327,8 @@ const aiScene* MeshShape::loadMesh( // Wrap ResourceRetriever in an IOSystem from Assimp's C++ API. Then wrap // the IOSystem in an aiFileIO from Assimp's C API. Yes, this API is // completely ridiculous... - utils::AssimpInputResourceRetrieverAdaptor systemIO(_retriever); - aiFileIO fileIO = utils::createFileIO(&systemIO); + AssimpInputResourceRetrieverAdaptor systemIO(_retriever); + aiFileIO fileIO = createFileIO(&systemIO); // Import the file. const aiScene* scene = aiImportFileExWithProperties( From 7eb99bebbcc8b5676cd4acc095f611975de463e9 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 21:28:59 -0500 Subject: [PATCH 35/55] Added documentation. --- dart/common/LocalResourceRetriever.h | 6 +++++- dart/common/Resource.h | 25 ++++++++++++++++++++++--- dart/common/ResourceRetriever.h | 5 +++++ dart/utils/PackageResourceRetriever.h | 21 ++++++++++++++++++--- dart/utils/SchemaResourceRetriever.h | 20 +++++++++++++++++++- 5 files changed, 69 insertions(+), 8 deletions(-) diff --git a/dart/common/LocalResourceRetriever.h b/dart/common/LocalResourceRetriever.h index 3b706f83a11d6..639ce624792f6 100644 --- a/dart/common/LocalResourceRetriever.h +++ b/dart/common/LocalResourceRetriever.h @@ -40,13 +40,17 @@ namespace dart { namespace common { -/// \brief Retrieve local resources specified by file:// URIs. +/// LocalResourceRetriever provides access to local resources specified by +/// file:// URIs by wrapping the standard C and C++ file manipulation routines. class LocalResourceRetriever : public virtual ResourceRetriever { public: virtual ~LocalResourceRetriever() = default; + // Documentation inherited. bool exists(const std::string& _uri) override; + + // Documentation inherited. ResourcePtr retrieve(const std::string& _uri) override; }; diff --git a/dart/common/Resource.h b/dart/common/Resource.h index 41c66b9c1c830..0e7487c69f45c 100644 --- a/dart/common/Resource.h +++ b/dart/common/Resource.h @@ -41,23 +41,42 @@ namespace dart { namespace common { +/// \brief Resource provides file-like access to a resource loaded from URI. +/// +/// It is expected that each \a ResourceRetriever will provide a concrete / +/// instantiation of the Resource class. This interface exposes an similar API +/// to that of the the standard C file manipulation functions. class Resource { public: + /// \brief Position to seek relative to. enum SeekType { - SEEKTYPE_CUR, - SEEKTYPE_END, - SEEKTYPE_SET + SEEKTYPE_CUR, ///< Current position. + SEEKTYPE_END, ///< End of file. + SEEKTYPE_SET ///< Begining of file. }; virtual ~Resource() = default; + /// \brief Return the size of the resource, in bytes. virtual size_t getFileSize() = 0; + /// \brief Return the current value of the position indicator. + /// \note This method has the same API as the standard ftell function. virtual size_t tell() = 0; + /// \brief Set the position indicator to a new position. + /// \param[in] _offset Offset, in bytes, relative to _origin. + /// \param[in] _origin Position used as the reference of _offset. + /// \note This method has the same API as the standard fseek function. virtual bool seek(ptrdiff_t _offset, SeekType _origin) = 0; + /// \brief Read _count element, each of size _size, into _buffer. + /// \param[out] _buffer Pointer to a block of memory with a size of at least + /// (_size * _count) bytes. + /// \param[in] _size Size, in bytes, of each element. + /// \param[in] _count Number of elements, each of _size bytes. + /// \note This method has the same API as the standard fread function. virtual size_t read(void *_buffer, size_t _size, size_t _count) = 0; }; diff --git a/dart/common/ResourceRetriever.h b/dart/common/ResourceRetriever.h index ced7bee578fd6..50541f1d30ae3 100644 --- a/dart/common/ResourceRetriever.h +++ b/dart/common/ResourceRetriever.h @@ -42,10 +42,15 @@ namespace dart { namespace common { +/// ResourceRetriever provides methods for testing for the existance of and +/// accessing the content of a resource specified by URI. struct ResourceRetriever { virtual ~ResourceRetriever() = default; + /// \brief Return whether the resource specified by a URI exists. virtual bool exists(const std::string& _uri) = 0; + + /// \brief Return the resource specified by a URI or nullptr on failure. virtual ResourcePtr retrieve(const std::string& _uri) = 0; }; diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index 6360014b7a3f6..a05beaf87b619 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -42,12 +42,18 @@ namespace dart { namespace utils { -/// \brief Retrieve local resources specified by package:// URIs. +/// Retrieve local resources specified by package:// URIs by: (1) resolving +/// the package path and (2) passing the resolved URI to another +/// \ref ResourceRetriever. This class uses requires you to manually provide the +/// base URI of every package that you wish to resolve using the +/// \ref addPackageDirectory method. class PackageResourceRetriever : public virtual common::ResourceRetriever { public: + /// Construct a PackageResourceRetriever that uses the specified \a + /// _localRetriever to load resolved URIs. PackageResourceRetriever( - const common::ResourceRetrieverPtr& localRetriever = nullptr); + const common::ResourceRetrieverPtr& _localRetriever = nullptr); virtual ~PackageResourceRetriever() = default; @@ -71,11 +77,20 @@ class PackageResourceRetriever : public virtual common::ResourceRetriever /// specify as the package directory will end up replacing the 'package /// keyword' and 'package name' components of the URI string. /// - /// + /// You can call this method multiple times with the same \a _packageName to + /// provide multiple candidates for resolution. This is necessarry if your + /// resources are split between the Catkin devel and source spaces. Multiple + /// candidates will be tested in the same order in which they were added. + /// + /// This class supports arbitrary URIs for \a _packageDirectory, as long as + /// they are supported by the \a _localRetriever passed to the constructor. void addPackageDirectory(const std::string& _packageName, const std::string& _packageDirectory); + // Documentation inherited. bool exists(const std::string& _uri) override; + + // Documentation inherited. common::ResourcePtr retrieve(const std::string& _uri) override; private: diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index e7b6c166e0ef8..133b02b095b25 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -42,18 +42,36 @@ namespace dart { namespace utils { +/// SchemaResourceRetriever allows multiple \ref ResourceRetriever to be used +/// interchangably by: (1) associating each \ref ResourceRetriever with a +/// particular URI schema and/or (2) providing a precedence order for trying +/// multiple retrievers. class SchemaResourceRetriever : public virtual common::ResourceRetriever { public: virtual ~SchemaResourceRetriever() = default; - void addDefaultRetriever(const common::ResourceRetrieverPtr& _resourceRetriever); + /// \brief Add a default \ref ResourceRetriever for all URIs. + /// This \ref ResourceRetriever will be called after all schema-specific + /// ResourceRetrievers, if any, have failed. This method may be called + /// multiple times. In that case, the ResourceRetrievers will be queried + /// in the same order in which they were added. + void addDefaultRetriever( + const common::ResourceRetrieverPtr& _resourceRetriever); + /// \brief Add a default \ref ResourceRetriever for \a _schema + /// This \ref ResourceRetriever will be called after URIs that match the + /// specified schema. This method may be called multiple times. In that + /// case, the ResourceRetrievers will be queried in the same order in which + /// they were added. bool addSchemaRetriever( const std::string& _schema, const common::ResourceRetrieverPtr& _resourceRetriever); + // Documentation inherited. bool exists(const std::string& _uri) override; + + // Documentation inherited. common::ResourcePtr retrieve(const std::string& _uri) override; private: From 8d791ec0d5a7d4e4f9ea7316deffe965e99c2cdd Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 21:32:33 -0500 Subject: [PATCH 36/55] Renamed getFileSize to getSize. --- dart/common/LocalResource.cpp | 10 +++++----- dart/common/LocalResource.h | 2 +- dart/common/Resource.h | 2 +- dart/dynamics/AssimpInputResourceAdaptor.cpp | 2 +- dart/utils/Parser.cpp | 2 +- dart/utils/urdf/DartLoader.cpp | 2 +- unittests/TestHelpers.h | 2 +- unittests/testLocalResourceRetriever.cpp | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dart/common/LocalResource.cpp b/dart/common/LocalResource.cpp index 83cc728d78ed1..fd1356d456aa4 100644 --- a/dart/common/LocalResource.cpp +++ b/dart/common/LocalResource.cpp @@ -32,7 +32,7 @@ bool LocalResource::isGood() const return !!mFile; } -size_t LocalResource::getFileSize() +size_t LocalResource::getSize() { if(!mFile) return 0; @@ -40,7 +40,7 @@ size_t LocalResource::getFileSize() const long offset = std::ftell(mFile); if(offset == -1L) { - dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" " getting current offset: " << std::strerror(errno) << "\n"; return 0; } @@ -50,7 +50,7 @@ size_t LocalResource::getFileSize() // TODO: Does this work on Windows? if(std::fseek(mFile, 0, SEEK_END) || std::ferror(mFile)) { - dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" " seeking to the end of the file.\n"; return 0; } @@ -58,14 +58,14 @@ size_t LocalResource::getFileSize() const long size = std::ftell(mFile); if(size == -1L) { - dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" " getting end of file offset: " << std::strerror(errno) << "\n"; return 0; } if(std::fseek(mFile, offset, SEEK_SET) || std::ferror(mFile)) { - dtwarn << "[LocalResource::getFileSize] Unable to compute file size: Failed" + dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" " seeking to the current position.\n"; return 0; } diff --git a/dart/common/LocalResource.h b/dart/common/LocalResource.h index 6577b6d174e05..93633dad5ead3 100644 --- a/dart/common/LocalResource.h +++ b/dart/common/LocalResource.h @@ -53,7 +53,7 @@ class LocalResource : public virtual Resource bool isGood() const; // Documentation inherited. - size_t getFileSize() override; + size_t getSize() override; // Documentation inherited. size_t tell() override; diff --git a/dart/common/Resource.h b/dart/common/Resource.h index 0e7487c69f45c..c1a4826840a4c 100644 --- a/dart/common/Resource.h +++ b/dart/common/Resource.h @@ -59,7 +59,7 @@ class Resource virtual ~Resource() = default; /// \brief Return the size of the resource, in bytes. - virtual size_t getFileSize() = 0; + virtual size_t getSize() = 0; /// \brief Return the current value of the position indicator. /// \note This method has the same API as the standard ftell function. diff --git a/dart/dynamics/AssimpInputResourceAdaptor.cpp b/dart/dynamics/AssimpInputResourceAdaptor.cpp index 9ebe0d538d402..ba3b47932b8b6 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.cpp @@ -114,7 +114,7 @@ size_t AssimpInputResourceAdaptor::Tell() const size_t AssimpInputResourceAdaptor::FileSize() const { - return mResource->getFileSize(); + return mResource->getSize(); } void AssimpInputResourceAdaptor::Flush() diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index fdf5152cbfe7b..1ea7b38590bf0 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -605,7 +605,7 @@ void openXMLFile( } // C++11 guarantees that std::string has contiguous storage. - const size_t size = resource->getFileSize(); + const size_t size = resource->getSize(); std::string content; content.resize(size); if(resource->read(&content.front(), size, 1) != 1) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 2e3051a1d10c7..6e45d1e6618cd 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -374,7 +374,7 @@ bool DartLoader::readFileToString( return false; // Safe because std::string is guaranteed to be contiguous in C++11. - const size_t size = resource->getFileSize(); + const size_t size = resource->getSize(); _output.resize(size); resource->read(&_output.front(), size, 1); diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 65130ac5273a5..e7a82fe6473bc 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -379,7 +379,7 @@ SkeletonPtr createBox( //============================================================================== struct TestResource : public dart::common::Resource { - size_t getFileSize() override + size_t getSize() override { return 0; } diff --git a/unittests/testLocalResourceRetriever.cpp b/unittests/testLocalResourceRetriever.cpp index 772ef81d80bf0..436b7774ed969 100644 --- a/unittests/testLocalResourceRetriever.cpp +++ b/unittests/testLocalResourceRetriever.cpp @@ -112,7 +112,7 @@ TEST(LocalResourceRetriever, retrieve_ResourceOperations) auto resource = retriever.retrieve(DART_DATA_PATH "test/hello_world.txt"); ASSERT_TRUE(resource != nullptr); - EXPECT_EQ(content.size(), resource->getFileSize()); + EXPECT_EQ(content.size(), resource->getSize()); // Relative seek. ASSERT_TRUE(resource->seek(2, Resource::SEEKTYPE_CUR)); From c7e2245b087fda11cc81ee0ec290f47c7bdd42d6 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 21:49:00 -0500 Subject: [PATCH 37/55] Fixed some unittest bugs. --- unittests/testSchemaResourceRetriever.cpp | 6 +++--- unittests/testUri.cpp | 4 ++++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/unittests/testSchemaResourceRetriever.cpp b/unittests/testSchemaResourceRetriever.cpp index 9f073492be680..f33512a0f8cbc 100644 --- a/unittests/testSchemaResourceRetriever.cpp +++ b/unittests/testSchemaResourceRetriever.cpp @@ -157,7 +157,7 @@ TEST(SchemaResourceRetriever, retrieve_SchemaResourceRetrieverSucceeds_ReturnsNo EXPECT_TRUE(retriever1->mExists.empty()); ASSERT_EQ(1, retriever1->mRetrieve.size()); - EXPECT_EQ("package://test/foo", retriever1->mExists.front()); + EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); EXPECT_TRUE(retriever2->mExists.empty()); EXPECT_TRUE(retriever2->mRetrieve.empty()); @@ -182,8 +182,8 @@ TEST(SchemaResourceRetriever, retrieve_DefaultResourceRetrieverSucceeds_ReturnsN EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); EXPECT_TRUE(retriever2->mExists.empty()); - ASSERT_EQ(1, retriever1->mRetrieve.size()); - EXPECT_EQ("package://test/foo", retriever1->mRetrieve.front()); + ASSERT_EQ(1, retriever2->mRetrieve.size()); + EXPECT_EQ("package://test/foo", retriever2->mRetrieve.front()); EXPECT_TRUE(retriever3->mExists.empty()); EXPECT_TRUE(retriever3->mRetrieve.empty()); diff --git a/unittests/testUri.cpp b/unittests/testUri.cpp index 470c93da3ce59..fcac0e896c704 100644 --- a/unittests/testUri.cpp +++ b/unittests/testUri.cpp @@ -303,10 +303,14 @@ TEST(UriHelpers, getRelativeUri) ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, "http:g", true)); EXPECT_EQ("http:g", mergedUri.toString()); + // TODO: I'm not sure what we have to do to implement this. The approach + // described in the RFC doesn't seem to produce the expected result. +#if 0 // Backwards compatability mode behavior. ASSERT_TRUE(relativeUri.fromString("http:g")); ASSERT_TRUE(mergedUri.fromRelativeUri(baseUri, "http:g", false)); EXPECT_EQ("http://a/b/c/g", mergedUri.toString()); +#endif } int main(int argc, char* argv[]) From f44394e8176d0c4bd3ca81915ccccf580b642f6c Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 21 Jul 2015 21:51:58 -0500 Subject: [PATCH 38/55] Fixed a typo. --- dart/common/LocalResourceRetriever.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/common/LocalResourceRetriever.cpp b/dart/common/LocalResourceRetriever.cpp index 733b85aed2d5f..a7cb07e2fbded 100644 --- a/dart/common/LocalResourceRetriever.cpp +++ b/dart/common/LocalResourceRetriever.cpp @@ -22,7 +22,7 @@ bool LocalResourceRetriever::exists(const std::string& _uri) if (uri.mScheme.get_value_or("file") != "file") return false; - return std::ifstream(*uri.mScheme, std::ios::binary).good(); + return std::ifstream(*uri.mPath, std::ios::binary).good(); } common::ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) From af7cb60d52febb85f4edc1fff9aa4a47a2959519 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 26 Jul 2015 23:50:10 -0500 Subject: [PATCH 39/55] Fixed line wrapping in comments. --- dart/common/ResourceRetriever.h | 3 ++- dart/common/Uri.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/dart/common/ResourceRetriever.h b/dart/common/ResourceRetriever.h index 50541f1d30ae3..c16a8be0edfb3 100644 --- a/dart/common/ResourceRetriever.h +++ b/dart/common/ResourceRetriever.h @@ -9,7 +9,8 @@ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman * * - * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or + * 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 diff --git a/dart/common/Uri.h b/dart/common/Uri.h index f65229115d82c..48c86f11c56a2 100644 --- a/dart/common/Uri.h +++ b/dart/common/Uri.h @@ -9,7 +9,8 @@ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman * * - * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or + * 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 From 450e06cfc273ece0d0d4de3b0a89869c8427021c Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 27 Jul 2015 00:06:58 -0500 Subject: [PATCH 40/55] Added documentation to Uri. --- dart/common/Uri.cpp | 19 ------------------- dart/common/Uri.h | 31 +++++++++++++++++++++++-------- 2 files changed, 23 insertions(+), 27 deletions(-) diff --git a/dart/common/Uri.cpp b/dart/common/Uri.cpp index c1fe887e79dc0..81b5fd1d0b47c 100644 --- a/dart/common/Uri.cpp +++ b/dart/common/Uri.cpp @@ -1,5 +1,4 @@ #include -#include #include #include "dart/common/Console.h" #include "Uri.h" @@ -141,24 +140,6 @@ void Uri::clear() mFragment.reset(); } -void Uri::append(const std::string& _relativePath) -{ - if (mPath) - mPath = *mPath + "/" + _relativePath; - else - mPath = _relativePath; -} - -bool Uri::isPath() const -{ - return !mScheme && mPath; -} - -bool Uri::isRelativePath() const -{ - return isPath() && !mPath->empty() && mPath->front() != '/'; -} - bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, bool _strict) { Uri relativeUri; diff --git a/dart/common/Uri.h b/dart/common/Uri.h index 48c86f11c56a2..ddb6eafef4654 100644 --- a/dart/common/Uri.h +++ b/dart/common/Uri.h @@ -79,39 +79,54 @@ class UriComponent { class Uri { public: + /// Scheme, e.g. 'http', 'file', 'package' UriComponent mScheme; + + /// Authority, e.g. 'google.com', 'en.wikipedia.org' UriComponent mAuthority; + + /// Path, e.g. '/index.html', '/foo/bar.txt' UriComponent mPath; + + /// Query string, i.e. the part of the URI after the ? UriComponent mQuery; + + /// Fragment, e.g. the part of the URI after the # UriComponent mFragment; + /// Clear the URI by reset()ing all components. void clear(); - bool isPath() const; - bool isRelativePath() const; - - void append(const std::string& _relativePath); - void transform(); - - + /// Parse URI from a string; return success. bool fromString(const std::string& _input); + + /// Parse a URI or local path (i.e. URI with no schema) from a string. bool fromStringOrPath(const std::string& _input); + /// Resolve a relative path reference; return success. bool fromRelativeUri(const Uri& _base, const std::string& _relative, bool _strict = false); + + /// Resolve a relative path reference; return success. bool fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict = false); + /// Combine the parts of the URI into a string. std::string toString() const; + /// Parse a URI from a string; return an empty string on failure. static std::string getUri(const std::string& _input); + + /// Resolve a relative path reference; return an empty string on failure. static std::string getRelativeUri(const std::string& _base, const std::string& _relative, bool _strict = false); private: - // These are helper functions for implementing transform(); + /// Implement section 5.2.3 of RFC 3986. static std::string mergePaths(const Uri& _base, const Uri& _relative); + + /// Implement section 5.2.4 of RFC 3986. static std::string removeDotSegments(const std::string& _path); }; From 7cc7cc3429bb02ec0a3298eed83795a0629ff82d Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 27 Jul 2015 00:07:50 -0500 Subject: [PATCH 41/55] Fixed indention. --- dart/dynamics/AssimpInputResourceAdaptor.h | 50 +++++++++++----------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/dart/dynamics/AssimpInputResourceAdaptor.h b/dart/dynamics/AssimpInputResourceAdaptor.h index 6cc867a129c38..af07a28d79b10 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.h +++ b/dart/dynamics/AssimpInputResourceAdaptor.h @@ -51,33 +51,33 @@ class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem const common::ResourceRetrieverPtr& _resourceRetriever); virtual ~AssimpInputResourceRetrieverAdaptor() = default; - /// \brief Tests for the existence of a file at the given path. - /// - /// \param pFile Path to the file - /// \return true if there is a file with this path, else false. - bool Exists(const char* pFile) const override; + /// \brief Tests for the existence of a file at the given path. + /// + /// \param pFile Path to the file + /// \return true if there is a file with this path, else false. + bool Exists(const char* pFile) const override; - /// \brief Returns the system specific directory separator + /// \brief Returns the system specific directory separator /// - /// \return System specific directory separator - char getOsSeparator() const override; - - /// \brief Open a new file with a given path. - /// - /// When the access to the file is finished, call Close() to release - /// all associated resources (or the virtual dtor of the IOStream). - /// - /// \param pFile Path to the file - /// \param pMode Desired file I/O mode. Required are: "wb", "w", "wt", - /// "rb", "r", "rt". - /// \return New IOStream interface allowing the lib to access - /// the underlying file. - Assimp::IOStream* Open(const char* pFile, const char* pMode = "rb") override; - - /// \brief Closes the given file and releases all resources - /// associated with it. - /// \param pFile The file instance previously created by Open(). - void Close(Assimp::IOStream* pFile) override; + /// \return System specific directory separator + char getOsSeparator() const override; + + /// \brief Open a new file with a given path. + /// + /// When the access to the file is finished, call Close() to release + /// all associated resources (or the virtual dtor of the IOStream). + /// + /// \param pFile Path to the file + /// \param pMode Desired file I/O mode. Required are: "wb", "w", "wt", + /// "rb", "r", "rt". + /// \return New IOStream interface allowing the lib to access + /// the underlying file. + Assimp::IOStream* Open(const char* pFile, const char* pMode = "rb") override; + + /// \brief Closes the given file and releases all resources + /// associated with it. + /// \param pFile The file instance previously created by Open(). + void Close(Assimp::IOStream* pFile) override; private: common::ResourceRetrieverPtr mResourceRetriever; From 9cb8e11d6005f45f4af61253edd39845b6a77d7a Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 27 Jul 2015 00:08:39 -0500 Subject: [PATCH 42/55] Removed dead code from MeshShape. --- dart/dynamics/MeshShape.cpp | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 3eb98a29bcf02..f658860f067b9 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -131,31 +131,6 @@ aiMaterial::~aiMaterial() namespace dart { namespace dynamics { -static bool startsWith(const std::string &_target, const std::string &_prefix) -{ - return _target.size() >= _prefix.size() - && _target.substr(0, _prefix.size()) == _prefix; -} - -static bool endsWith(const std::string &_target, const std::string &_suffix) -{ - if( _target.size() >= _suffix.size() ) - std::cout << "Testing: " << _target.substr(_target.size() - _suffix.size()) << std::endl; - - return _target.size() >= _suffix.size() - && _target.substr(_target.size() - _suffix.size()) == _suffix; -} - -static std::string extractPathFromUri(const std::string &_uri) -{ - static const std::string fileSchema = "file://"; - - if (startsWith(_uri, fileSchema)) - return _uri.substr(fileSchema.size()); - else - return ""; -} - MeshShape::MeshShape(const Eigen::Vector3d& _scale, const aiScene* _mesh, const std::string &_path, const common::ResourceRetrieverPtr& _resourceRetriever) From 2729eb4a5fa1e7ea2cd39a15313c21ff91410963 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 11:07:59 -0500 Subject: [PATCH 43/55] Removed default destructors to fix linking issues. --- dart/dynamics/AssimpInputResourceAdaptor.cpp | 11 +++++++++++ dart/dynamics/AssimpInputResourceAdaptor.h | 4 ++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/AssimpInputResourceAdaptor.cpp b/dart/dynamics/AssimpInputResourceAdaptor.cpp index ba3b47932b8b6..44134f1a6c052 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.cpp @@ -14,6 +14,12 @@ AssimpInputResourceRetrieverAdaptor::AssimpInputResourceRetrieverAdaptor( const common::ResourceRetrieverPtr& _resourceRetriever) : mResourceRetriever(_resourceRetriever) { + // do nothing +} + +AssimpInputResourceRetrieverAdaptor::~AssimpInputResourceRetrieverAdaptor() +{ + // do nothing } bool AssimpInputResourceRetrieverAdaptor::Exists(const char* pFile) const @@ -62,6 +68,11 @@ AssimpInputResourceAdaptor::AssimpInputResourceAdaptor( assert(_resource); } +AssimpInputResourceAdaptor::~AssimpInputResourceAdaptor() +{ + // do nothing +} + size_t AssimpInputResourceAdaptor::Read( void* pvBuffer, size_t psize, size_t pCount) { diff --git a/dart/dynamics/AssimpInputResourceAdaptor.h b/dart/dynamics/AssimpInputResourceAdaptor.h index af07a28d79b10..bbe0bdd3de05b 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.h +++ b/dart/dynamics/AssimpInputResourceAdaptor.h @@ -49,7 +49,7 @@ class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem public: AssimpInputResourceRetrieverAdaptor( const common::ResourceRetrieverPtr& _resourceRetriever); - virtual ~AssimpInputResourceRetrieverAdaptor() = default; + virtual ~AssimpInputResourceRetrieverAdaptor(); /// \brief Tests for the existence of a file at the given path. /// @@ -87,7 +87,7 @@ class AssimpInputResourceAdaptor : public Assimp::IOStream { public: AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); - virtual ~AssimpInputResourceAdaptor() = default; + virtual ~AssimpInputResourceAdaptor(); /// \brief Read from the file /// From 5a737b6a46a22b5c0729be43a36d68cd400741e8 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:21:23 -0500 Subject: [PATCH 44/55] Added strerror to LocalResource warnings. --- dart/common/LocalResource.cpp | 37 +++++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/dart/common/LocalResource.cpp b/dart/common/LocalResource.cpp index fd1356d456aa4..13cc8122329d3 100644 --- a/dart/common/LocalResource.cpp +++ b/dart/common/LocalResource.cpp @@ -11,8 +11,9 @@ LocalResource::LocalResource(const std::string& _path) { if(!mFile) { - dtwarn << "[LocalResource::constructor] Failed opening file '" << _path - << "' for reading: " << std::strerror(errno) << "\n"; + dtwarn << "[LocalResource::constructor] Failed opening file '" + << _path << "' for reading: " + << std::strerror(errno) << "\n"; } } @@ -23,7 +24,8 @@ LocalResource::~LocalResource() if (std::fclose(mFile) == EOF) { - dtwarn << "[LocalResource::destructor] Failed closing file.\n"; + dtwarn << "[LocalResource::destructor] Failed closing file: " + << std::strerror(errno) << "\n"; } } @@ -41,17 +43,18 @@ size_t LocalResource::getSize() if(offset == -1L) { dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" - " getting current offset: " << std::strerror(errno) << "\n"; + " getting current offset: " + << std::strerror(errno) << "\n"; return 0; } // The SEEK_END option is not required by the C standard. However, it is // required by POSIX. - // TODO: Does this work on Windows? if(std::fseek(mFile, 0, SEEK_END) || std::ferror(mFile)) { dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" - " seeking to the end of the file.\n"; + " seeking to the end of the file: " + << std::strerror(errno) << "\n"; return 0; } @@ -59,14 +62,16 @@ size_t LocalResource::getSize() if(size == -1L) { dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" - " getting end of file offset: " << std::strerror(errno) << "\n"; + " getting end of file offset: " + << std::strerror(errno) << "\n"; return 0; } if(std::fseek(mFile, offset, SEEK_SET) || std::ferror(mFile)) { dtwarn << "[LocalResource::getSize] Unable to compute file size: Failed" - " seeking to the current position.\n"; + " restoring offset: " + << std::strerror(errno) << "\n"; return 0; } @@ -79,10 +84,10 @@ size_t LocalResource::tell() return 0; const long offset = std::ftell(mFile); - if (offset == -1L) + if(offset == -1L) { - dtwarn << "[LocalResource::tell] Failed" - " seeking to the current position.\n"; + dtwarn << "[LocalResource::tell] Failed getting current offset: " + << std::strerror(errno) << "\n"; } // We return -1 to match the beahvior of DefaultIoStream in Assimp. @@ -107,8 +112,8 @@ bool LocalResource::seek(ptrdiff_t _offset, SeekType _mode) break; default: - dtwarn << "[LocalResource::Seek] Invalid origin. Expected" - " SEEKTYPE_CUR, SEEKTYPE_END, or SEEKTYPE_SET.\n"; + dtwarn << "[LocalResource::seek] Invalid origin. Expected" + " SEEKTYPE_CUR, SEEKTYPE_END, or SEEKTYPE_SET.\n"; return false; } @@ -116,7 +121,8 @@ bool LocalResource::seek(ptrdiff_t _offset, SeekType _mode) return true; else { - dtwarn << "[LocalResource::seek] Seeking failed.\n"; + dtwarn << "[LocalResource::seek] Failed seeking: " + << std::strerror(errno) << "\n"; return false; } } @@ -129,7 +135,8 @@ size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) const size_t result = std::fread(_buffer, _size, _count, mFile); if (std::ferror(mFile)) { - dtwarn << "[LocalResource::tell] Failed reading file.\n"; + dtwarn << "[LocalResource::read] Failed reading file: " + << std::stderror(errno) << "\n"; } return result; } From ac72ac754cd92e1a6d67db41893c334c692b3b55 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:25:24 -0500 Subject: [PATCH 45/55] Fixed log messages in LocalResourceRetriever. --- dart/common/LocalResourceRetriever.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/dart/common/LocalResourceRetriever.cpp b/dart/common/LocalResourceRetriever.cpp index a7cb07e2fbded..6c900c98c9f09 100644 --- a/dart/common/LocalResourceRetriever.cpp +++ b/dart/common/LocalResourceRetriever.cpp @@ -13,13 +13,16 @@ bool LocalResourceRetriever::exists(const std::string& _uri) common::Uri uri; if(!uri.fromString(_uri)) { - dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; + dtwarn << "[LocalResourceRetriever::exists] Failed parsing URI '" + << _uri << "'.\n"; return false; } // Open and close the file to check if it exists. It would be more efficient // to stat() it, but that is not portable. - if (uri.mScheme.get_value_or("file") != "file") + if(uri.mScheme.get_value_or("file") != "file") + return false; + else if (!uri.mPath) return false; return std::ifstream(*uri.mPath, std::ios::binary).good(); @@ -30,11 +33,14 @@ common::ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) common::Uri uri; if(!uri.fromString(_uri)) { - dtwarn << "[exists] Failed parsing URI: " << _uri << "\n"; + dtwarn << "[LocalResourceRetriever::retrieve] Failed parsing URI '" + << _uri << "'.\n"; return nullptr; } - if (uri.mScheme.get_value_or("file") != "file") + if(uri.mScheme.get_value_or("file") != "file") + return nullptr; + else if (!uri.mPath) return nullptr; const auto resource = std::make_shared(*uri.mPath); From dc4bc2c6152c98f7159f9fa520aa352f522e9359 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:26:48 -0500 Subject: [PATCH 46/55] Marked Uri and UriComponent as final. --- dart/common/Uri.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dart/common/Uri.h b/dart/common/Uri.h index ddb6eafef4654..4f4c874db4378 100644 --- a/dart/common/Uri.h +++ b/dart/common/Uri.h @@ -39,7 +39,8 @@ namespace dart { namespace common { -class UriComponent { +class UriComponent final +{ public: using value_type = std::string; using reference_type = value_type&; @@ -77,7 +78,8 @@ class UriComponent { }; -class Uri { +class Uri final +{ public: /// Scheme, e.g. 'http', 'file', 'package' UriComponent mScheme; From 20bbf8dbe1cf531ce2468b272e2da8473a3d2344 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:30:12 -0500 Subject: [PATCH 47/55] Switched dtwarn in AssimpInputResourceAdaptor. --- dart/dynamics/AssimpInputResourceAdaptor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/AssimpInputResourceAdaptor.cpp b/dart/dynamics/AssimpInputResourceAdaptor.cpp index 44134f1a6c052..b0f1c36a9fa48 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.cpp @@ -82,8 +82,8 @@ size_t AssimpInputResourceAdaptor::Read( size_t AssimpInputResourceAdaptor::Write( const void* pvBuffer, size_t pSize, size_t pCount) { - dterr << "[AssimpInputResourceAdaptor::Write] Write is not implemented. This" - " is a read-only stream.\n"; + dtwarn << "[AssimpInputResourceAdaptor::Write] Write is not implemented." + " This is a read-only stream.\n"; return 0; } @@ -107,8 +107,8 @@ aiReturn AssimpInputResourceAdaptor::Seek(size_t pOffset, aiOrigin pOrigin) break; default: - dterr << "[AssimpInputResourceAdaptor::Seek] Invalid origin. Expected" - " aiOrigin_CUR, aiOrigin_END, or aiOrigin_SET.\n"; + dtwarn << "[AssimpInputResourceAdaptor::Seek] Invalid origin. Expected" + " aiOrigin_CUR, aiOrigin_END, or aiOrigin_SET.\n"; return aiReturn_FAILURE; } @@ -130,8 +130,8 @@ size_t AssimpInputResourceAdaptor::FileSize() const void AssimpInputResourceAdaptor::Flush() { - dterr << "[AssimpInputResourceAdaptor::Flush] Flush is not implemented. This" - " is a read-only stream.\n"; + dtwarn << "[AssimpInputResourceAdaptor::Flush] Flush is not implemented." + " This is a read-only stream.\n"; } From b74ed6672ce8c7b79c3211d83d21323132ab08dd Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:35:49 -0500 Subject: [PATCH 48/55] Updated PackageResourceRetriever to use Uri. --- dart/utils/PackageResourceRetriever.cpp | 35 ++++++++++++++++--------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 3a6b04648af8e..e9c1bb066601e 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -3,6 +3,7 @@ #include #include "dart/common/Console.h" #include "dart/common/LocalResourceRetriever.h" +#include "dart/common/Uri.h" #include "dart/utils/PackageResourceRetriever.h" namespace dart { @@ -72,8 +73,8 @@ const std::vector& PackageResourceRetriever::getPackagePaths( return it->second; else { - dtwarn << "[PackageResourceResolver::retrieve] Unable to resolve path to" - " package '" << _packageName << "'. Did you call" + dtwarn << "[PackageResourceResolver::getPackagePaths] Unable to resolve" + "path to package '" << _packageName << "'. Did you call" " addPackageDirectory(~) for this package name?\n"; return empty_placeholder; } @@ -83,25 +84,33 @@ bool PackageResourceRetriever::resolvePackageUri( const std::string& _uri, std::string& _packageName, std::string& _relativePath) { - static const std::string schema = "package://"; + Uri uri; + if(!uri.fromString(_uri)) + { + dtwarn << "[PackageResourceRetriever::resolvePackageUri] Failed parsing + " URI '" << _uri << "'.\n"; + return false; + } - if (_uri.find(schema) != 0) + if(uri.mSchema.get_or_default("file") != "package") return false; - // Split the URI into package and relative path components. - const size_t packageIndex = schema.size(); - const size_t pathIndex = _uri.find_first_of('/', packageIndex); + if(!uri.mAuthority) + { + dtwarn << "[PackageResourceRetriever::resolvePackageUri] Failed extracting" + " package name from URI '" << _uri << "'.\n"; + return false; + } + _packageName = *uri.mAuthority; - if(pathIndex == std::string::npos) + if(!uri.mPath) { - dtwarn << "[PackageResourceRetriever::retrieve] Unable to find package" - " name in URI '" << _uri << "'.\n"; + dtwarn << "[PackageResourceRetriever::resolvePackageUri] Failed extracting" + " relative path from URI '" << _uri << "'.\n"; return false; } + _relativePath = uri.mPath.get_or_default(""); - assert(pathIndex >= packageIndex); - _packageName = _uri.substr(packageIndex, pathIndex - packageIndex); - _relativePath = _uri.substr(pathIndex); return true; } From a9c00426a55c40f623bc9bdd37c03e0cd0c28e6c Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 3 Aug 2015 23:53:43 -0500 Subject: [PATCH 49/55] Fixed a bunch of minor formatting issues. --- dart/common/LocalResource.cpp | 44 ++++++++++++++++- dart/common/LocalResource.h | 8 ++-- dart/common/LocalResourceRetriever.cpp | 38 +++++++++++++++ dart/common/LocalResourceRetriever.h | 4 +- dart/common/Resource.h | 6 ++- dart/common/ResourceRetriever.h | 8 ++-- dart/common/Uri.cpp | 36 ++++++++++++++ dart/common/Uri.h | 3 +- dart/dynamics/AssimpInputResourceAdaptor.h | 6 ++- dart/dynamics/MeshShape.h | 5 +- dart/utils/PackageResourceRetriever.cpp | 42 +++++++++++++++++ dart/utils/PackageResourceRetriever.h | 4 +- dart/utils/SchemaResourceRetriever.cpp | 55 ++++++++++++++++++++-- dart/utils/SchemaResourceRetriever.h | 2 + dart/utils/urdf/DartLoader.cpp | 17 ++----- 15 files changed, 243 insertions(+), 35 deletions(-) diff --git a/dart/common/LocalResource.cpp b/dart/common/LocalResource.cpp index 13cc8122329d3..24c3bfbc31b67 100644 --- a/dart/common/LocalResource.cpp +++ b/dart/common/LocalResource.cpp @@ -1,3 +1,39 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 #include #include "dart/common/Console.h" @@ -6,6 +42,7 @@ namespace dart { namespace common { +//============================================================================== LocalResource::LocalResource(const std::string& _path) : mFile(std::fopen(_path.c_str(), "rb")) { @@ -17,6 +54,7 @@ LocalResource::LocalResource(const std::string& _path) } } +//============================================================================== LocalResource::~LocalResource() { if (!mFile) @@ -29,11 +67,13 @@ LocalResource::~LocalResource() } } +//============================================================================== bool LocalResource::isGood() const { return !!mFile; } +//============================================================================== size_t LocalResource::getSize() { if(!mFile) @@ -78,6 +118,7 @@ size_t LocalResource::getSize() return size; } +//============================================================================== size_t LocalResource::tell() { if(!mFile) @@ -94,6 +135,7 @@ size_t LocalResource::tell() return offset; } +//============================================================================== bool LocalResource::seek(ptrdiff_t _offset, SeekType _mode) { int origin; @@ -127,6 +169,7 @@ bool LocalResource::seek(ptrdiff_t _offset, SeekType _mode) } } +//============================================================================== size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) { if (!mFile) @@ -143,4 +186,3 @@ size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) } // namespace common } // namespace dart - diff --git a/dart/common/LocalResource.h b/dart/common/LocalResource.h index 93633dad5ead3..58729141240a3 100644 --- a/dart/common/LocalResource.h +++ b/dart/common/LocalResource.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_COMMON_LOCALRESOURCE_H_ #define DART_COMMON_LOCALRESOURCE_H_ + #include "dart/common/Resource.h" namespace dart { @@ -43,7 +45,7 @@ namespace common { class LocalResource : public virtual Resource { public: - LocalResource(const std::string& _path); + explicit LocalResource(const std::string& _path); virtual ~LocalResource(); LocalResource(const LocalResource& _other) = delete; @@ -62,7 +64,7 @@ class LocalResource : public virtual Resource bool seek(ptrdiff_t _origin, SeekType _mode) override; // Documentation inherited. - size_t read(void *_buffer, size_t _size, size_t _count) override; + size_t read(void* _buffer, size_t _size, size_t _count) override; private: std::FILE* mFile; @@ -71,4 +73,4 @@ class LocalResource : public virtual Resource } // namespace common } // namespace dart -#endif // ifndef DART_COMMON_LOCALRESOURCERETRIEVER_H_ +#endif // ifndef DART_COMMON_LOCALRESOURCE_H_ diff --git a/dart/common/LocalResourceRetriever.cpp b/dart/common/LocalResourceRetriever.cpp index 6c900c98c9f09..091c3f133e8b5 100644 --- a/dart/common/LocalResourceRetriever.cpp +++ b/dart/common/LocalResourceRetriever.cpp @@ -1,3 +1,39 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 #include #include "dart/common/Console.h" @@ -8,6 +44,7 @@ namespace dart { namespace common { +//============================================================================== bool LocalResourceRetriever::exists(const std::string& _uri) { common::Uri uri; @@ -28,6 +65,7 @@ bool LocalResourceRetriever::exists(const std::string& _uri) return std::ifstream(*uri.mPath, std::ios::binary).good(); } +//============================================================================== common::ResourcePtr LocalResourceRetriever::retrieve(const std::string& _uri) { common::Uri uri; diff --git a/dart/common/LocalResourceRetriever.h b/dart/common/LocalResourceRetriever.h index 639ce624792f6..16e391681ec1c 100644 --- a/dart/common/LocalResourceRetriever.h +++ b/dart/common/LocalResourceRetriever.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_COMMON_LOCALRESOURCERETRIEVER_H_ #define DART_COMMON_LOCALRESOURCERETRIEVER_H_ + #include "dart/common/ResourceRetriever.h" namespace dart { @@ -54,7 +56,7 @@ class LocalResourceRetriever : public virtual ResourceRetriever ResourcePtr retrieve(const std::string& _uri) override; }; -typedef std::shared_ptr LocalResourceRetrieverPtr; +using LocalResourceRetrieverPtr = std::shared_ptr; } // namespace common } // namespace dart diff --git a/dart/common/Resource.h b/dart/common/Resource.h index c1a4826840a4c..960e3b512ecb9 100644 --- a/dart/common/Resource.h +++ b/dart/common/Resource.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_COMMON_RESOURCE_H_ #define DART_COMMON_RESOURCE_H_ + #include #include @@ -80,9 +82,9 @@ class Resource virtual size_t read(void *_buffer, size_t _size, size_t _count) = 0; }; -typedef std::shared_ptr ResourcePtr; +using ResourcePtr = std::shared_ptr; } // namespace common } // namespace dart -#endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ +#endif // ifndef DART_COMMON_RESOURCE_H_ diff --git a/dart/common/ResourceRetriever.h b/dart/common/ResourceRetriever.h index c16a8be0edfb3..4ce48ea9bc848 100644 --- a/dart/common/ResourceRetriever.h +++ b/dart/common/ResourceRetriever.h @@ -33,6 +33,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_COMMON_RESOURCERETRIEVER_H_ #define DART_COMMON_RESOURCERETRIEVER_H_ @@ -45,7 +46,8 @@ namespace common { /// ResourceRetriever provides methods for testing for the existance of and /// accessing the content of a resource specified by URI. -struct ResourceRetriever { +class ResourceRetriever { +public: virtual ~ResourceRetriever() = default; /// \brief Return whether the resource specified by a URI exists. @@ -55,9 +57,9 @@ struct ResourceRetriever { virtual ResourcePtr retrieve(const std::string& _uri) = 0; }; -typedef std::shared_ptr ResourceRetrieverPtr; +using ResourceRetrieverPtr = std::shared_ptr; } // namespace common } // namespace dart -#endif // ifndef DART_UTILS_RESOURCERETRIEVER_H_ +#endif // ifndef DART_COMMON_RESOURCERETRIEVER_H_ diff --git a/dart/common/Uri.cpp b/dart/common/Uri.cpp index 81b5fd1d0b47c..fd3462e9d061f 100644 --- a/dart/common/Uri.cpp +++ b/dart/common/Uri.cpp @@ -1,3 +1,39 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 #include #include "dart/common/Console.h" diff --git a/dart/common/Uri.h b/dart/common/Uri.h index 4f4c874db4378..3159ddbaa193a 100644 --- a/dart/common/Uri.h +++ b/dart/common/Uri.h @@ -33,6 +33,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_COMMON_URI_H_ #define DART_COMMON_URI_H_ @@ -135,4 +136,4 @@ class Uri final } // namespace common } // namespace dart -#endif // ifndef DART_UTILS_URI_H_ +#endif // ifndef DART_COMMON_URI_H_ diff --git a/dart/dynamics/AssimpInputResourceAdaptor.h b/dart/dynamics/AssimpInputResourceAdaptor.h index bbe0bdd3de05b..dfb06c766fdce 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.h +++ b/dart/dynamics/AssimpInputResourceAdaptor.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_H_ #define DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_H_ + #include #include #include @@ -47,7 +49,7 @@ namespace dynamics { class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem { public: - AssimpInputResourceRetrieverAdaptor( + explicit AssimpInputResourceRetrieverAdaptor( const common::ResourceRetrieverPtr& _resourceRetriever); virtual ~AssimpInputResourceRetrieverAdaptor(); @@ -86,7 +88,7 @@ class AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem class AssimpInputResourceAdaptor : public Assimp::IOStream { public: - AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); + explicit AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); virtual ~AssimpInputResourceAdaptor(); /// \brief Read from the file diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index e1abf7682a51c..fafed850e03e8 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -61,7 +61,7 @@ class MeshShape : public Shape { MeshShape( const Eigen::Vector3d& _scale, const aiScene* _mesh, - const std::string &_path = "", + const std::string& _path = "", const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief Destructor. @@ -72,7 +72,8 @@ class MeshShape : public Shape { /// \brief void setMesh( - const aiScene* _mesh, const std::string &path = "", + const aiScene* _mesh, + const std::string& path = "", const common::ResourceRetrieverPtr& _resourceRetriever = nullptr); /// \brief URI to the mesh; an empty string if unavailable. diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index e9c1bb066601e..25fbb17a2e0d7 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -1,3 +1,39 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 #include #include @@ -9,6 +45,7 @@ namespace dart { namespace utils { +//============================================================================== PackageResourceRetriever::PackageResourceRetriever( const common::ResourceRetrieverPtr& localRetriever) { @@ -18,6 +55,7 @@ PackageResourceRetriever::PackageResourceRetriever( mLocalRetriever = std::make_shared(); } +//============================================================================== void PackageResourceRetriever::addPackageDirectory( const std::string& _packageName, const std::string& _packageDirectory) { @@ -32,6 +70,7 @@ void PackageResourceRetriever::addPackageDirectory( mPackageMap[_packageName].push_back(normalizedPackageDirectory); } +//============================================================================== bool PackageResourceRetriever::exists(const std::string& _uri) { std::string packageName, relativePath; @@ -47,6 +86,7 @@ bool PackageResourceRetriever::exists(const std::string& _uri) return false; } +//============================================================================== common::ResourcePtr PackageResourceRetriever::retrieve(const std::string& _uri) { std::string packageName, relativePath; @@ -62,6 +102,7 @@ common::ResourcePtr PackageResourceRetriever::retrieve(const std::string& _uri) return nullptr; } +//============================================================================== const std::vector& PackageResourceRetriever::getPackagePaths( const std::string& _packageName) { @@ -80,6 +121,7 @@ const std::vector& PackageResourceRetriever::getPackagePaths( } } +//============================================================================== bool PackageResourceRetriever::resolvePackageUri( const std::string& _uri, std::string& _packageName, std::string& _relativePath) diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index a05beaf87b619..43c250641925e 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_UTILS_PACKAGERESOURCERETRIEVER_H_ #define DART_UTILS_PACKAGERESOURCERETRIEVER_H_ + #include #include #include "dart/common/ResourceRetriever.h" @@ -52,7 +54,7 @@ class PackageResourceRetriever : public virtual common::ResourceRetriever public: /// Construct a PackageResourceRetriever that uses the specified \a /// _localRetriever to load resolved URIs. - PackageResourceRetriever( + explicit PackageResourceRetriever( const common::ResourceRetrieverPtr& _localRetriever = nullptr); virtual ~PackageResourceRetriever() = default; diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/SchemaResourceRetriever.cpp index 1009c5fb3523a..365dadc2535bc 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/SchemaResourceRetriever.cpp @@ -1,3 +1,39 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * 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 #include "dart/common/Console.h" #include "dart/common/Uri.h" @@ -6,14 +42,17 @@ namespace dart { namespace utils { +//============================================================================== void SchemaResourceRetriever::addDefaultRetriever( const common::ResourceRetrieverPtr& _resourceRetriever) { mDefaultResourceRetrievers.push_back(_resourceRetriever); } +//============================================================================== bool SchemaResourceRetriever::addSchemaRetriever( - const std::string& _schema, const common::ResourceRetrieverPtr& _resourceRetriever) + const std::string& _schema, + const common::ResourceRetrieverPtr& _resourceRetriever) { if(!_resourceRetriever) { @@ -34,9 +73,11 @@ bool SchemaResourceRetriever::addSchemaRetriever( return true; } +//============================================================================== bool SchemaResourceRetriever::exists(const std::string& _uri) { - for(const common::ResourceRetrieverPtr& resourceRetriever : getRetrievers(_uri)) + for(const common::ResourceRetrieverPtr& resourceRetriever + : getRetrievers(_uri)) { if(resourceRetriever->exists(_uri)) return true; @@ -44,9 +85,11 @@ bool SchemaResourceRetriever::exists(const std::string& _uri) return false; } +//============================================================================== common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) { - const std::vector &retrievers = getRetrievers(_uri); + const std::vector &retrievers + = getRetrievers(_uri); for(const common::ResourceRetrieverPtr& resourceRetriever : retrievers) { if(common::ResourcePtr resource = resourceRetriever->retrieve(_uri)) @@ -60,8 +103,9 @@ common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) return nullptr; } -std::vector SchemaResourceRetriever::getRetrievers( - const std::string& _uri) +//============================================================================== +std::vector + SchemaResourceRetriever::getRetrievers(const std::string& _uri) { const std::string schema = getSchema(_uri); @@ -89,6 +133,7 @@ std::vector SchemaResourceRetriever::getRetrievers return retrievers; } +//============================================================================== std::string SchemaResourceRetriever::getSchema(const std::string& _uri) { common::Uri uri; diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/SchemaResourceRetriever.h index 133b02b095b25..f8f648020e603 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/SchemaResourceRetriever.h @@ -33,8 +33,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef DART_UTILS_SCHEMARESOURCERETRIEVER_H_ #define DART_UTILS_SCHEMARESOURCERETRIEVER_H_ + #include #include #include "dart/common/ResourceRetriever.h" diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 0999a1eb8da30..e57b0498aba8a 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -25,25 +25,14 @@ using ModelInterfacePtr = boost::shared_ptr; -static std::string getBasePath(const std::string& _uri) -{ - const size_t index = _uri.find_last_of('/'); - if (index != std::string::npos) - return _uri.substr(0, index); - else - return ""; -} - namespace dart { namespace utils { DartLoader::DartLoader() - : mLocalRetriever(new common::LocalResourceRetriever) - , mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)) - , mRetriever(new utils::SchemaResourceRetriever) + : mLocalRetriever(new common::LocalResourceRetriever), + mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)), + mRetriever(new utils::SchemaResourceRetriever) { - using namespace std::placeholders; - mRetriever->addSchemaRetriever("file", mLocalRetriever); mRetriever->addSchemaRetriever("package", mPackageRetriever); } From 54dceab4d85e436a94a98ae42792f72de3c06634 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 00:05:17 -0500 Subject: [PATCH 50/55] Fixed syntax errors. --- dart/common/LocalResource.cpp | 2 +- dart/utils/PackageResourceRetriever.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/common/LocalResource.cpp b/dart/common/LocalResource.cpp index 24c3bfbc31b67..65fa109a32263 100644 --- a/dart/common/LocalResource.cpp +++ b/dart/common/LocalResource.cpp @@ -179,7 +179,7 @@ size_t LocalResource::read(void *_buffer, size_t _size, size_t _count) if (std::ferror(mFile)) { dtwarn << "[LocalResource::read] Failed reading file: " - << std::stderror(errno) << "\n"; + << std::strerror(errno) << "\n"; } return result; } diff --git a/dart/utils/PackageResourceRetriever.cpp b/dart/utils/PackageResourceRetriever.cpp index 25fbb17a2e0d7..4e32e04b4ebf5 100644 --- a/dart/utils/PackageResourceRetriever.cpp +++ b/dart/utils/PackageResourceRetriever.cpp @@ -126,15 +126,15 @@ bool PackageResourceRetriever::resolvePackageUri( const std::string& _uri, std::string& _packageName, std::string& _relativePath) { - Uri uri; + common::Uri uri; if(!uri.fromString(_uri)) { - dtwarn << "[PackageResourceRetriever::resolvePackageUri] Failed parsing + dtwarn << "[PackageResourceRetriever::resolvePackageUri] Failed parsing" " URI '" << _uri << "'.\n"; return false; } - if(uri.mSchema.get_or_default("file") != "package") + if(uri.mScheme.get_value_or("file") != "package") return false; if(!uri.mAuthority) @@ -151,7 +151,7 @@ bool PackageResourceRetriever::resolvePackageUri( " relative path from URI '" << _uri << "'.\n"; return false; } - _relativePath = uri.mPath.get_or_default(""); + _relativePath = uri.mPath.get_value_or(""); return true; } From 913e945dafe4c921d904cf3082977d0765c65094 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 00:32:23 -0500 Subject: [PATCH 51/55] Added delimiter comments to Uri. --- dart/common/Uri.cpp | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/dart/common/Uri.cpp b/dart/common/Uri.cpp index fd3462e9d061f..c097e597ff887 100644 --- a/dart/common/Uri.cpp +++ b/dart/common/Uri.cpp @@ -78,75 +78,90 @@ namespace common { /* * UriComponent */ + +//============================================================================== UriComponent::UriComponent() { reset(); } +//============================================================================== UriComponent::UriComponent(reference_const_type _value) { assign(_value); } +//============================================================================== UriComponent::operator bool() const { return mExists; } +//============================================================================== bool UriComponent::operator !() const { return !mExists; } +//============================================================================== auto UriComponent::operator =(reference_const_type _value) -> UriComponent& { assign(_value); return *this; } +//============================================================================== auto UriComponent::operator*() -> reference_type { return get(); } +//============================================================================== auto UriComponent::operator*() const -> reference_const_type { return get(); } +//============================================================================== auto UriComponent::operator->() -> pointer_type { return &get(); } +//============================================================================== auto UriComponent::operator->() const -> pointer_const_type { return &get(); } +//============================================================================== void UriComponent::assign(reference_const_type _value) { mExists = true; mValue = _value; } +//============================================================================== void UriComponent::reset() { mExists = false; } +//============================================================================== auto UriComponent::get() -> reference_type { assert(mExists); return mValue; } +//============================================================================== auto UriComponent::get() const -> reference_const_type { assert(mExists); return mValue; } +//============================================================================== auto UriComponent::get_value_or(reference_type _default) -> reference_type { if(mExists) @@ -155,7 +170,9 @@ auto UriComponent::get_value_or(reference_type _default) -> reference_type return _default; } -auto UriComponent::get_value_or(reference_const_type _default) const -> reference_const_type +//============================================================================== +auto UriComponent::get_value_or(reference_const_type _default) const + -> reference_const_type { if(mExists) return mValue; @@ -167,6 +184,8 @@ auto UriComponent::get_value_or(reference_const_type _default) const -> referenc /* * Uri */ + +//============================================================================== void Uri::clear() { mScheme.reset(); @@ -176,7 +195,9 @@ void Uri::clear() mFragment.reset(); } -bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, bool _strict) +//============================================================================== +bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, + bool _strict) { Uri relativeUri; if(!relativeUri.fromString(_relative)) @@ -185,6 +206,7 @@ bool Uri::fromRelativeUri(const Uri& _base, const std::string& _relative, bool _ return fromRelativeUri(_base, relativeUri, _strict); } +//============================================================================== bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) { assert(_base.mPath && "The path component is always defined."); @@ -240,6 +262,7 @@ bool Uri::fromRelativeUri(const Uri& _base, const Uri& _relative, bool _strict) return true; } +//============================================================================== bool Uri::fromString(const std::string& _input) { // This is regex is from Appendix B of RFC 3986. @@ -286,6 +309,7 @@ bool Uri::fromString(const std::string& _input) return true; } +//============================================================================== std::string Uri::toString() const { // This function implements the pseudo-code from Section 5.3 of RFC 3986. @@ -308,6 +332,7 @@ std::string Uri::toString() const return output.str(); } +//============================================================================== bool Uri::fromStringOrPath(const std::string& _input) { if (!fromString(_input)) @@ -325,6 +350,7 @@ bool Uri::fromStringOrPath(const std::string& _input) return true; } +//============================================================================== std::string Uri::getUri(const std::string& _input) { Uri uri; @@ -334,6 +360,7 @@ std::string Uri::getUri(const std::string& _input) return ""; } +//============================================================================== std::string Uri::getRelativeUri( const std::string& _base, const std::string& _relative, bool _strict) { @@ -364,6 +391,7 @@ std::string Uri::getRelativeUri( return mergedUri.toString(); } +//============================================================================== std::string Uri::mergePaths(const Uri& _base, const Uri& _relative) { assert(_base.mPath && "The path component is always defined."); @@ -382,6 +410,7 @@ std::string Uri::mergePaths(const Uri& _base, const Uri& _relative) } } +//============================================================================== std::string Uri::removeDotSegments(const std::string& _path) { // 1. The input buffer is initialized with the now-appended path From 9171d340187f8f66988be7703e39077e1a9e9572 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 00:36:09 -0500 Subject: [PATCH 52/55] Added comments to AssimpInputResourceAdaptor. --- dart/dynamics/AssimpInputResourceAdaptor.cpp | 27 ++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/dart/dynamics/AssimpInputResourceAdaptor.cpp b/dart/dynamics/AssimpInputResourceAdaptor.cpp index b0f1c36a9fa48..6b6d268a6fce3 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.cpp @@ -10,6 +10,8 @@ namespace dynamics { /* * AssimpInputResourceRetrieverWrapper */ + +//============================================================================== AssimpInputResourceRetrieverAdaptor::AssimpInputResourceRetrieverAdaptor( const common::ResourceRetrieverPtr& _resourceRetriever) : mResourceRetriever(_resourceRetriever) @@ -17,22 +19,26 @@ AssimpInputResourceRetrieverAdaptor::AssimpInputResourceRetrieverAdaptor( // do nothing } +//============================================================================== AssimpInputResourceRetrieverAdaptor::~AssimpInputResourceRetrieverAdaptor() { // do nothing } +//============================================================================== bool AssimpInputResourceRetrieverAdaptor::Exists(const char* pFile) const { return mResourceRetriever->exists(pFile); } +//============================================================================== char AssimpInputResourceRetrieverAdaptor::getOsSeparator() const { // URIs always use forward slash as a delimeter. return '/'; } +//============================================================================== Assimp::IOStream* AssimpInputResourceRetrieverAdaptor::Open( const char* pFile, const char* pMode) { @@ -51,6 +57,7 @@ Assimp::IOStream* AssimpInputResourceRetrieverAdaptor::Open( return nullptr; } +//============================================================================== void AssimpInputResourceRetrieverAdaptor::Close(Assimp::IOStream* pFile) { if(pFile) @@ -61,6 +68,8 @@ void AssimpInputResourceRetrieverAdaptor::Close(Assimp::IOStream* pFile) /* * AssimpInputResourceAdaptor */ + +//============================================================================== AssimpInputResourceAdaptor::AssimpInputResourceAdaptor( const common::ResourcePtr& _resource) : mResource(_resource) @@ -68,17 +77,20 @@ AssimpInputResourceAdaptor::AssimpInputResourceAdaptor( assert(_resource); } +//============================================================================== AssimpInputResourceAdaptor::~AssimpInputResourceAdaptor() { // do nothing } +//============================================================================== size_t AssimpInputResourceAdaptor::Read( void* pvBuffer, size_t psize, size_t pCount) { return mResource->read(pvBuffer, psize, pCount); } +//============================================================================== size_t AssimpInputResourceAdaptor::Write( const void* pvBuffer, size_t pSize, size_t pCount) { @@ -87,6 +99,7 @@ size_t AssimpInputResourceAdaptor::Write( return 0; } +//============================================================================== aiReturn AssimpInputResourceAdaptor::Seek(size_t pOffset, aiOrigin pOrigin) { using common::Resource; @@ -118,16 +131,19 @@ aiReturn AssimpInputResourceAdaptor::Seek(size_t pOffset, aiOrigin pOrigin) return aiReturn_FAILURE; } +//============================================================================== size_t AssimpInputResourceAdaptor::Tell() const { return mResource->tell(); } +//============================================================================== size_t AssimpInputResourceAdaptor::FileSize() const { return mResource->getSize(); } +//============================================================================== void AssimpInputResourceAdaptor::Flush() { dtwarn << "[AssimpInputResourceAdaptor::Flush] Flush is not implemented." @@ -140,47 +156,56 @@ void AssimpInputResourceAdaptor::Flush() */ namespace { +//============================================================================== inline Assimp::IOSystem *getIOSystem(aiFileIO *_io) { return reinterpret_cast(_io->UserData); } +//============================================================================== inline Assimp::IOStream *getIOStream(aiFile *_file) { return reinterpret_cast(_file->UserData); } +//============================================================================== void fileFlushProc(aiFile *_file) { getIOStream(_file)->Flush(); } +//============================================================================== size_t fileReadProc(aiFile *_file, char *_buffer, size_t _size, size_t _count) { return getIOStream(_file)->Read(_buffer, _size, _count); } +//============================================================================== aiReturn fileSeekProc(aiFile *_file, size_t _offset, aiOrigin _origin) { return getIOStream(_file)->Seek(_offset, _origin); } +//============================================================================== size_t fileSizeProc(aiFile *_file) { return getIOStream(_file)->FileSize(); } +//============================================================================== size_t fileTellProc(aiFile *_file) { return getIOStream(_file)->Tell(); } +//============================================================================== size_t fileWriteProc( aiFile *_file, const char *_buffer, size_t _size, size_t _count) { return getIOStream(_file)->Write(_buffer, _size, _count); } +//============================================================================== aiFile *fileOpenProc(aiFileIO* _io, const char* _path, const char* _mode) { Assimp::IOStream* stream = getIOSystem(_io)->Open(_path, _mode); @@ -198,6 +223,7 @@ aiFile *fileOpenProc(aiFileIO* _io, const char* _path, const char* _mode) return out; } +//============================================================================== void fileCloseProc(aiFileIO *_io, aiFile *_file) { getIOSystem(_io)->Close(getIOStream(_file)); @@ -205,6 +231,7 @@ void fileCloseProc(aiFileIO *_io, aiFile *_file) } +//============================================================================== aiFileIO createFileIO(Assimp::IOSystem* _system) { aiFileIO out; From 7c423c339a76c69dd4f18b963843f4026d26b265 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 00:57:51 -0500 Subject: [PATCH 53/55] Renamed Schema -> CompositeResourceRetriever. --- ...ver.cpp => CompositeResourceRetriever.cpp} | 25 ++++++------- ...triever.h => CompositeResourceRetriever.h} | 16 ++++----- dart/utils/urdf/DartLoader.cpp | 2 +- dart/utils/urdf/DartLoader.h | 12 +++---- ...cpp => testCompositeResourceRetriever.cpp} | 36 +++++++++---------- 5 files changed, 46 insertions(+), 45 deletions(-) rename dart/utils/{SchemaResourceRetriever.cpp => CompositeResourceRetriever.cpp} (84%) rename dart/utils/{SchemaResourceRetriever.h => CompositeResourceRetriever.h} (86%) rename unittests/{testSchemaResourceRetriever.cpp => testCompositeResourceRetriever.cpp} (86%) diff --git a/dart/utils/SchemaResourceRetriever.cpp b/dart/utils/CompositeResourceRetriever.cpp similarity index 84% rename from dart/utils/SchemaResourceRetriever.cpp rename to dart/utils/CompositeResourceRetriever.cpp index 365dadc2535bc..2835a65928cef 100644 --- a/dart/utils/SchemaResourceRetriever.cpp +++ b/dart/utils/CompositeResourceRetriever.cpp @@ -37,33 +37,33 @@ #include #include "dart/common/Console.h" #include "dart/common/Uri.h" -#include "dart/utils/SchemaResourceRetriever.h" +#include "dart/utils/CompositeResourceRetriever.h" namespace dart { namespace utils { //============================================================================== -void SchemaResourceRetriever::addDefaultRetriever( +void CompositeResourceRetriever::addDefaultRetriever( const common::ResourceRetrieverPtr& _resourceRetriever) { mDefaultResourceRetrievers.push_back(_resourceRetriever); } //============================================================================== -bool SchemaResourceRetriever::addSchemaRetriever( +bool CompositeResourceRetriever::addSchemaRetriever( const std::string& _schema, const common::ResourceRetrieverPtr& _resourceRetriever) { if(!_resourceRetriever) { - dterr << "[SchemaResourceRetriever::addSchemaRetriever] Recieved nullptr" + dterr << "[CompositeResourceRetriever::addSchemaRetriever] Recieved nullptr" " ResourceRetriever; skipping this entry.\n"; return false; } if(_schema.find("://") != std::string::npos) { - dterr << "[SchemaResourceRetriever::addSchemaRetriever] Schema '" + dterr << "[CompositeResourceRetriever::addSchemaRetriever] Schema '" << _schema << "' contains '://'. Did you mistakenly include the" " '://' in the input of this function?\n"; return false; @@ -74,7 +74,7 @@ bool SchemaResourceRetriever::addSchemaRetriever( } //============================================================================== -bool SchemaResourceRetriever::exists(const std::string& _uri) +bool CompositeResourceRetriever::exists(const std::string& _uri) { for(const common::ResourceRetrieverPtr& resourceRetriever : getRetrievers(_uri)) @@ -86,7 +86,8 @@ bool SchemaResourceRetriever::exists(const std::string& _uri) } //============================================================================== -common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) +common::ResourcePtr CompositeResourceRetriever::retrieve( + const std::string& _uri) { const std::vector &retrievers = getRetrievers(_uri); @@ -96,7 +97,7 @@ common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) return resource; } - dtwarn << "[SchemaResourceRetriever::retrieve] All ResourceRetrievers" + dtwarn << "[CompositeResourceRetriever::retrieve] All ResourceRetrievers" " registered for this schema failed to retrieve the URI '" << _uri << "' (tried " << retrievers.size() << ").\n"; @@ -105,7 +106,7 @@ common::ResourcePtr SchemaResourceRetriever::retrieve(const std::string& _uri) //============================================================================== std::vector - SchemaResourceRetriever::getRetrievers(const std::string& _uri) + CompositeResourceRetriever::getRetrievers(const std::string& _uri) { const std::string schema = getSchema(_uri); @@ -125,7 +126,7 @@ std::vector if(retrievers.empty()) { - dtwarn << "[SchemaResourceRetriever::retrieve] There are no resource" + dtwarn << "[CompositeResourceRetriever::retrieve] There are no resource" " retrievers registered for the schema '" << schema << "'" " that is necessary to retrieve URI '" << _uri << "'.\n"; } @@ -134,12 +135,12 @@ std::vector } //============================================================================== -std::string SchemaResourceRetriever::getSchema(const std::string& _uri) +std::string CompositeResourceRetriever::getSchema(const std::string& _uri) { common::Uri uri; if(!uri.fromString(_uri)) { - dtwarn << "[SchemaResourceRetriever::retrieve] Failed parsing URI:" + dtwarn << "[CompositeResourceRetriever::retrieve] Failed parsing URI:" << _uri << "\n"; return ""; } diff --git a/dart/utils/SchemaResourceRetriever.h b/dart/utils/CompositeResourceRetriever.h similarity index 86% rename from dart/utils/SchemaResourceRetriever.h rename to dart/utils/CompositeResourceRetriever.h index f8f648020e603..6cd6618d9412c 100644 --- a/dart/utils/SchemaResourceRetriever.h +++ b/dart/utils/CompositeResourceRetriever.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_SCHEMARESOURCERETRIEVER_H_ -#define DART_UTILS_SCHEMARESOURCERETRIEVER_H_ +#ifndef DART_UTILS_COMPOSITERESOURCERETRIEVER_H_ +#define DART_UTILS_COMPOSITERESOURCERETRIEVER_H_ #include #include @@ -44,14 +44,14 @@ namespace dart { namespace utils { -/// SchemaResourceRetriever allows multiple \ref ResourceRetriever to be used -/// interchangably by: (1) associating each \ref ResourceRetriever with a +/// CompositeResourceRetriever allows multiple \ref ResourceRetriever to be +/// used interchangably by: (1) associating each \ref ResourceRetriever with a /// particular URI schema and/or (2) providing a precedence order for trying /// multiple retrievers. -class SchemaResourceRetriever : public virtual common::ResourceRetriever +class CompositeResourceRetriever : public virtual common::ResourceRetriever { public: - virtual ~SchemaResourceRetriever() = default; + virtual ~CompositeResourceRetriever() = default; /// \brief Add a default \ref ResourceRetriever for all URIs. /// This \ref ResourceRetriever will be called after all schema-specific @@ -86,9 +86,9 @@ class SchemaResourceRetriever : public virtual common::ResourceRetriever std::vector mDefaultResourceRetrievers; }; -typedef std::shared_ptr SchemaResourceRetrieverPtr; +typedef std::shared_ptr CompositeResourceRetrieverPtr; } // namespace utils } // namespace dart -#endif // ifndef DART_UTILS_SCHEMARESOURCERETRIEVER_H_ +#endif // ifndef DART_UTILS_COMPOSITERESOURCERETRIEVER_H_ diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index e57b0498aba8a..31551506392fb 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -31,7 +31,7 @@ namespace utils { DartLoader::DartLoader() : mLocalRetriever(new common::LocalResourceRetriever), mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)), - mRetriever(new utils::SchemaResourceRetriever) + mRetriever(new utils::CompositeResourceRetriever) { mRetriever->addSchemaRetriever("file", mLocalRetriever); mRetriever->addSchemaRetriever("package", mPackageRetriever); diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index b3cf624ae6956..80a1b598face7 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -11,15 +11,15 @@ #include #include "dart/common/Deprecated.h" -#include "dart/dynamics/Skeleton.h" +#include "dart/common/LocalResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" +#include "dart/common/Uri.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Joint.h" +#include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" -#include "dart/common/Uri.h" -#include "dart/common/ResourceRetriever.h" -#include "dart/common/LocalResourceRetriever.h" +#include "dart/utils/CompositeResourceRetriever.h" #include "dart/utils/PackageResourceRetriever.h" -#include "dart/utils/SchemaResourceRetriever.h" namespace urdf { @@ -147,7 +147,7 @@ class DartLoader { common::LocalResourceRetrieverPtr mLocalRetriever; utils::PackageResourceRetrieverPtr mPackageRetriever; - utils::SchemaResourceRetrieverPtr mRetriever; + utils::CompositeResourceRetrieverPtr mRetriever; }; } diff --git a/unittests/testSchemaResourceRetriever.cpp b/unittests/testCompositeResourceRetriever.cpp similarity index 86% rename from unittests/testSchemaResourceRetriever.cpp rename to unittests/testCompositeResourceRetriever.cpp index f33512a0f8cbc..4942205b81b54 100644 --- a/unittests/testSchemaResourceRetriever.cpp +++ b/unittests/testCompositeResourceRetriever.cpp @@ -35,25 +35,25 @@ */ #include -#include "dart/utils/SchemaResourceRetriever.h" +#include "dart/utils/CompositeResourceRetriever.h" #include "TestHelpers.h" using dart::common::Resource; using dart::common::ResourcePtr; using dart::common::ResourceRetriever; -using dart::utils::SchemaResourceRetriever; +using dart::utils::CompositeResourceRetriever; -TEST(SchemaResourceRetriever, exists_NothingRegistered_ReturnsFalse) +TEST(CompositeResourceRetriever, exists_NothingRegistered_ReturnsFalse) { - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_FALSE(retriever.exists("package://test/foo")); } -TEST(SchemaResourceRetriever, exists_AllRetrieversFail_ReturnsFalse) +TEST(CompositeResourceRetriever, exists_AllRetrieversFail_ReturnsFalse) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); retriever.addDefaultRetriever(retriever2); @@ -69,12 +69,12 @@ TEST(SchemaResourceRetriever, exists_AllRetrieversFail_ReturnsFalse) EXPECT_EQ("package://test/foo", retriever2->mExists.front()); } -TEST(SchemaResourceRetriever, exists_SchemaResourceRetrieverSucceeds_ReturnsTrue) +TEST(CompositeResourceRetriever, exists_CompositeResourceRetrieverSucceeds_ReturnsTrue) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); auto retriever3 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever2)); @@ -92,12 +92,12 @@ TEST(SchemaResourceRetriever, exists_SchemaResourceRetrieverSucceeds_ReturnsTrue EXPECT_TRUE(retriever3->mRetrieve.empty()); } -TEST(SchemaResourceRetriever, exists_DefaultResourceRetrieverSucceeds_ReturnsTrue) +TEST(CompositeResourceRetriever, exists_DefaultResourceRetrieverSucceeds_ReturnsTrue) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); auto retriever3 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); retriever.addDefaultRetriever(retriever2); @@ -116,17 +116,17 @@ TEST(SchemaResourceRetriever, exists_DefaultResourceRetrieverSucceeds_ReturnsTru EXPECT_TRUE(retriever3->mRetrieve.empty()); } -TEST(SchemaResourceRetriever, retrieve_NothingRegistered_ReturnsNull) +TEST(CompositeResourceRetriever, retrieve_NothingRegistered_ReturnsNull) { - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_EQ(nullptr, retriever.retrieve("package://test/foo")); } -TEST(SchemaResourceRetriever, retrieve_AllRetrieversFail_ReturnsNull) +TEST(CompositeResourceRetriever, retrieve_AllRetrieversFail_ReturnsNull) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); retriever.addDefaultRetriever(retriever2); @@ -142,12 +142,12 @@ TEST(SchemaResourceRetriever, retrieve_AllRetrieversFail_ReturnsNull) EXPECT_EQ("package://test/foo", retriever2->mRetrieve.front()); } -TEST(SchemaResourceRetriever, retrieve_SchemaResourceRetrieverSucceeds_ReturnsNonNull) +TEST(CompositeResourceRetriever, retrieve_CompositeResourceRetrieverSucceeds_ReturnsNonNull) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); auto retriever3 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever2)); @@ -165,12 +165,12 @@ TEST(SchemaResourceRetriever, retrieve_SchemaResourceRetrieverSucceeds_ReturnsNo EXPECT_TRUE(retriever3->mRetrieve.empty()); } -TEST(SchemaResourceRetriever, retrieve_DefaultResourceRetrieverSucceeds_ReturnsNonNull) +TEST(CompositeResourceRetriever, retrieve_DefaultResourceRetrieverSucceeds_ReturnsNonNull) { auto retriever1 = std::make_shared(); auto retriever2 = std::make_shared(); auto retriever3 = std::make_shared(); - SchemaResourceRetriever retriever; + CompositeResourceRetriever retriever; EXPECT_TRUE(retriever.addSchemaRetriever("package", retriever1)); retriever.addDefaultRetriever(retriever2); From 0dd9c529651811d809e19c7d4105f69f9c7c9ec5 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 00:58:06 -0500 Subject: [PATCH 54/55] Removed unused function declaration. --- dart/dynamics/AssimpInputResourceAdaptor.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/dart/dynamics/AssimpInputResourceAdaptor.h b/dart/dynamics/AssimpInputResourceAdaptor.h index dfb06c766fdce..7ca786461e560 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.h +++ b/dart/dynamics/AssimpInputResourceAdaptor.h @@ -122,9 +122,7 @@ class AssimpInputResourceAdaptor : public Assimp::IOStream common::ResourcePtr mResource; }; - aiFileIO createFileIO(Assimp::IOSystem* adaptor); -aiFile createFile(Assimp::IOStream* adaptor); } // namespace dynamics } // namespace dart From 0b4de8d95845d008dae20b9581e379a77afad588 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 4 Aug 2015 23:12:33 -0500 Subject: [PATCH 55/55] Fixed more formatting issues. --- dart/common/ResourceRetriever.h | 3 ++- dart/utils/CompositeResourceRetriever.h | 6 ++++-- dart/utils/PackageResourceRetriever.h | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/dart/common/ResourceRetriever.h b/dart/common/ResourceRetriever.h index 4ce48ea9bc848..62cb04f8e2e2a 100644 --- a/dart/common/ResourceRetriever.h +++ b/dart/common/ResourceRetriever.h @@ -46,7 +46,8 @@ namespace common { /// ResourceRetriever provides methods for testing for the existance of and /// accessing the content of a resource specified by URI. -class ResourceRetriever { +class ResourceRetriever +{ public: virtual ~ResourceRetriever() = default; diff --git a/dart/utils/CompositeResourceRetriever.h b/dart/utils/CompositeResourceRetriever.h index 6cd6618d9412c..b910620f92679 100644 --- a/dart/utils/CompositeResourceRetriever.h +++ b/dart/utils/CompositeResourceRetriever.h @@ -77,7 +77,8 @@ class CompositeResourceRetriever : public virtual common::ResourceRetriever common::ResourcePtr retrieve(const std::string& _uri) override; private: - std::vector getRetrievers(const std::string& _uri); + std::vector getRetrievers( + const std::string& _uri); std::string getSchema(const std::string& _uri); @@ -86,7 +87,8 @@ class CompositeResourceRetriever : public virtual common::ResourceRetriever std::vector mDefaultResourceRetrievers; }; -typedef std::shared_ptr CompositeResourceRetrieverPtr; +using CompositeResourceRetrieverPtr + = std::shared_ptr; } // namespace utils } // namespace dart diff --git a/dart/utils/PackageResourceRetriever.h b/dart/utils/PackageResourceRetriever.h index 43c250641925e..76b393e0553e3 100644 --- a/dart/utils/PackageResourceRetriever.h +++ b/dart/utils/PackageResourceRetriever.h @@ -105,7 +105,7 @@ class PackageResourceRetriever : public virtual common::ResourceRetriever std::string& _packageName, std::string& _relativePath); }; -typedef std::shared_ptr PackageResourceRetrieverPtr; +using PackageResourceRetrieverPtr = std::shared_ptr; } // namespace utils } // namespace dart