From 602f983ea69f2f9ec2c4e92ef62ea0095f6ece4c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 07:06:21 -0400 Subject: [PATCH 1/5] Update API to support fcl 0.5.0 --- dart/collision/fcl/FCLCollisionDetector.cpp | 20 ++++++++++++++++---- dart/collision/fcl/FCLCollisionDetector.hpp | 15 ++++++++++++++- dart/collision/fcl/FCLCollisionObject.cpp | 4 ++++ dart/collision/fcl/FCLCollisionObject.hpp | 5 +++++ 4 files changed, 39 insertions(+), 5 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 426e50f103ca9..0e3c60f303c40 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -431,8 +431,6 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - //============================================================================== template fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, @@ -536,8 +534,6 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, return model; } -#endif - //============================================================================== template fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, @@ -782,7 +778,11 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== +#if FCL_VERSION_AT_LEAST(0,5,0) +std::shared_ptr +#else boost::shared_ptr +#endif FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -805,7 +805,11 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== +#if FCL_VERSION_AT_LEAST(0,5,0) +std::shared_ptr +#else boost::shared_ptr +#endif FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -943,9 +947,17 @@ FCLCollisionDetector::createFCLCollisionGeometry( } if (geom) +#if FCL_VERSION_AT_LEAST(0,5,0) + return std::shared_ptr(geom, deleter); +#else return boost::shared_ptr(geom, deleter); +#endif else +#if FCL_VERSION_AT_LEAST(0,5,0) + return std::shared_ptr(); +#else return boost::shared_ptr(); +#endif } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index b808619022801..4e7c55bee98d5 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -36,6 +36,7 @@ #include #include // This should be removed once we migrate to fcl 0.5 #include "dart/collision/CollisionDetector.hpp" +#include "dart/collision/fcl/FCLTypes.hpp" namespace dart { namespace collision { @@ -130,7 +131,11 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. +#if FCL_VERSION_AT_LEAST(0,5,0) + std::shared_ptr claimFCLCollisionGeometry( +#else boost::shared_ptr claimFCLCollisionGeometry( +#endif const dynamics::ConstShapePtr& shape); protected: @@ -162,7 +167,11 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter +#if FCL_VERSION_AT_LEAST(0,5,0) + std::shared_ptr createFCLCollisionGeometry( +#else boost::shared_ptr createFCLCollisionGeometry( +#endif const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -170,7 +179,11 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; +#if FCL_VERSION_AT_LEAST(0,5,0) + std::weak_ptr>; +#else + boost::weak_ptr>; +#endif // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate // to 0.5 or greater, this also should be changed to // std::weak_ptr diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index f72d62158cdfc..11e6dfce9c57d 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -63,7 +63,11 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, +#if FCL_VERSION_AT_LEAST(0,5,0) + const std::shared_ptr& fclCollGeom) +#else const boost::shared_ptr& fclCollGeom) +#endif : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObjectUserData(new UserData(this)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index ff011cf75e074..f0567af83999a 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -34,6 +34,7 @@ #include #include "dart/collision/CollisionObject.hpp" +#include "dart/collision/fcl/FCLTypes.hpp" namespace dart { namespace collision { @@ -64,7 +65,11 @@ class FCLCollisionObject : public CollisionObject /// Constructor FCLCollisionObject(CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, +#if FCL_VERSION_AT_LEAST(0,5,0) + const std::shared_ptr& fclCollGeom); +#else const boost::shared_ptr& fclCollGeom); +#endif // Documentation inherited void updateEngineData() override; From 05c59450992788c173f87f4015acefef2238024f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 07:09:11 -0400 Subject: [PATCH 2/5] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c520be5bbf0e4..24b86cd4909fc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ * Collision detection + * Added support of FCL 0.5.0: [#749](https://github.com/dartsim/dart/pull/749) * Added warnings for unsupported shape pairs of DARTCollisionDetector: [#722](https://github.com/dartsim/dart/pull/722) * Misc improvements and bug fixes From 2523d37fcd712ae76c17d5756bc8cdf125972d26 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 13:44:03 -0400 Subject: [PATCH 3/5] Use type aliasing rather than boilerplate preprocessors --- dart/collision/fcl/FCLCollisionDetector.cpp | 24 ++++----------------- dart/collision/fcl/FCLCollisionDetector.hpp | 18 +++------------- dart/collision/fcl/FCLCollisionObject.cpp | 6 +----- dart/collision/fcl/FCLCollisionObject.hpp | 6 +----- dart/collision/fcl/FCLTypes.hpp | 8 +++++++ 5 files changed, 17 insertions(+), 45 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 0e3c60f303c40..cf0ee008f1a99 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -778,11 +778,7 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== -#if FCL_VERSION_AT_LEAST(0,5,0) -std::shared_ptr -#else -boost::shared_ptr -#endif +fcl_shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -805,11 +801,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== -#if FCL_VERSION_AT_LEAST(0,5,0) -std::shared_ptr -#else -boost::shared_ptr -#endif +fcl_shared_ptr FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -947,17 +939,9 @@ FCLCollisionDetector::createFCLCollisionGeometry( } if (geom) -#if FCL_VERSION_AT_LEAST(0,5,0) - return std::shared_ptr(geom, deleter); -#else - return boost::shared_ptr(geom, deleter); -#endif + return fcl_shared_ptr(geom, deleter); else -#if FCL_VERSION_AT_LEAST(0,5,0) - return std::shared_ptr(); -#else - return boost::shared_ptr(); -#endif + return fcl_shared_ptr(); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 4e7c55bee98d5..d37c0e58ccb8f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -131,11 +131,7 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. -#if FCL_VERSION_AT_LEAST(0,5,0) - std::shared_ptr claimFCLCollisionGeometry( -#else - boost::shared_ptr claimFCLCollisionGeometry( -#endif + fcl_shared_ptr claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape); protected: @@ -167,11 +163,7 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter -#if FCL_VERSION_AT_LEAST(0,5,0) - std::shared_ptr createFCLCollisionGeometry( -#else - boost::shared_ptr createFCLCollisionGeometry( -#endif + fcl_shared_ptr createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -179,11 +171,7 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; -#else - boost::weak_ptr>; -#endif + fcl_weak_ptr>; // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate // to 0.5 or greater, this also should be changed to // std::weak_ptr diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index 11e6dfce9c57d..ec1cf15f56002 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -63,11 +63,7 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, -#if FCL_VERSION_AT_LEAST(0,5,0) - const std::shared_ptr& fclCollGeom) -#else - const boost::shared_ptr& fclCollGeom) -#endif + const fcl_shared_ptr& fclCollGeom) : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObjectUserData(new UserData(this)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index f0567af83999a..544c36eea1fbb 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -65,11 +65,7 @@ class FCLCollisionObject : public CollisionObject /// Constructor FCLCollisionObject(CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, -#if FCL_VERSION_AT_LEAST(0,5,0) - const std::shared_ptr& fclCollGeom); -#else - const boost::shared_ptr& fclCollGeom); -#endif + const fcl_shared_ptr& fclCollGeom); // Documentation inherited void updateEngineData() override; diff --git a/dart/collision/fcl/FCLTypes.hpp b/dart/collision/fcl/FCLTypes.hpp index 8fa6e88108a81..8fbf15c2c130f 100644 --- a/dart/collision/fcl/FCLTypes.hpp +++ b/dart/collision/fcl/FCLTypes.hpp @@ -46,6 +46,14 @@ (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \ (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y)))) +#if FCL_VERSION_AT_LEAST(0,5,0) +template using fcl_shared_ptr = std::shared_ptr; +template using fcl_weak_ptr = std::weak_ptr; +#else +template using fcl_shared_ptr = boost::shared_ptr; +template using fcl_weak_ptr = boost::weak_ptr; +#endif + namespace dart { namespace collision { From eaa10ae2b4e0d29de003921d868f0c62e12ed0dc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 13:56:04 -0400 Subject: [PATCH 4/5] Switch to use XML_SUCCESS rather than XML_NO_ERROR --- dart/utils/XmlHelpers.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index adaf7de17647f..bb0fcc7ca329e 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -726,7 +726,7 @@ bool getAttributeBool(const tinyxml2::XMLElement* element, const int result = element->QueryBoolAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing bool type attribute [" << attributeName << "] of an element [" << element->Name() @@ -744,7 +744,7 @@ int getAttributeInt(const tinyxml2::XMLElement* element, int val = 0; const int result = element->QueryIntAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -763,7 +763,7 @@ unsigned int getAttributeUInt(const tinyxml2::XMLElement* element, const int result = element->QueryUnsignedAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing unsiged int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -782,7 +782,7 @@ float getAttributeFloat(const tinyxml2::XMLElement* element, const int result = element->QueryFloatAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing float type attribute [" << attributeName << "] of an element [" << element->Name() @@ -801,7 +801,7 @@ double getAttributeDouble(const tinyxml2::XMLElement* element, const int result = element->QueryDoubleAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing double type attribute [" << attributeName << "] of an element [" << element->Name() From b2ac9702fa32872b16b5db70a0d7d8396c80e68f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 14:07:46 -0400 Subject: [PATCH 5/5] Update changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 24b86cd4909fc..caeca51a32c49 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,7 @@ * Collision detection - * Added support of FCL 0.5.0: [#749](https://github.com/dartsim/dart/pull/749) + * Added support of FCL 0.5 and tinyxml2 4.0: [#749](https://github.com/dartsim/dart/pull/749) * Added warnings for unsupported shape pairs of DARTCollisionDetector: [#722](https://github.com/dartsim/dart/pull/722) * Misc improvements and bug fixes