diff --git a/dart/dynamics/ArrowShape.cpp b/dart/dynamics/ArrowShape.cpp index 953ad99f98448..a382618965fd7 100644 --- a/dart/dynamics/ArrowShape.cpp +++ b/dart/dynamics/ArrowShape.cpp @@ -252,10 +252,9 @@ ShapePtr ArrowShape::clone() const new_shape->mHead = mHead; new_shape->mProperties = mProperties; - new_shape->mMesh = new_scene; - new_shape->mMeshUri = mMeshUri; + new_shape->setMesh( + new_scene, MeshOwnership::Copied, mMeshUri, mResourceRetriever); new_shape->mMeshPath = mMeshPath; - new_shape->mResourceRetriever = mResourceRetriever; new_shape->mDisplayList = mDisplayList; new_shape->mScale = mScale; new_shape->mColorMode = mColorMode; @@ -387,7 +386,7 @@ void ArrowShape::instantiate(std::size_t resolution) face->mIndices[2] = 2 * resolution; } - mMesh = scene; + setMesh(scene, MeshOwnership::Manual, common::Uri(), nullptr); // setColor(mColor); // TODO(JS) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index d0ce2a10723b1..53dd54da7ebc2 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -141,6 +141,25 @@ MeshShape::MeshShape( const Eigen::Vector3d& scale, const aiScene* mesh, const common::Uri& path, + common::ResourceRetrieverPtr resourceRetriever, + MeshOwnership ownership) + : Shape(MESH), + mMesh(nullptr), + mMeshOwnership(MeshOwnership::None), + mDisplayList(0), + mColorMode(MATERIAL_COLOR), + mAlphaMode(BLEND), + mColorIndex(0) +{ + setMesh(mesh, ownership, path, std::move(resourceRetriever)); + setScale(scale); +} + +//============================================================================== +MeshShape::MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr mesh, + const common::Uri& path, common::ResourceRetrieverPtr resourceRetriever) : Shape(MESH), mMesh(nullptr), @@ -150,7 +169,7 @@ MeshShape::MeshShape( mAlphaMode(BLEND), mColorIndex(0) { - setMesh(mesh, path, std::move(resourceRetriever)); + setMesh(std::move(mesh), path, std::move(resourceRetriever)); setScale(scale); } @@ -163,22 +182,7 @@ MeshShape::~MeshShape() //============================================================================== void MeshShape::releaseMesh() { - if (!mMesh) - return; - - switch (mMeshOwnership) { - case MeshOwnership::Imported: - aiReleaseImport(const_cast(mMesh)); - break; - case MeshOwnership::Copied: - aiFreeScene(const_cast(mMesh)); - break; - case MeshOwnership::None: - default: - break; - } - - mMesh = nullptr; + mMesh.reset(); mMeshOwnership = MeshOwnership::None; } @@ -198,7 +202,7 @@ const std::string& MeshShape::getStaticType() //============================================================================== const aiScene* MeshShape::getMesh() const { - return mMesh; + return mMesh.get(); } //============================================================================== @@ -237,7 +241,11 @@ void MeshShape::setMesh( const std::string& path, common::ResourceRetrieverPtr resourceRetriever) { - setMesh(mesh, common::Uri(path), std::move(resourceRetriever)); + setMesh( + mesh, + MeshOwnership::Imported, + common::Uri(path), + std::move(resourceRetriever)); } //============================================================================== @@ -246,15 +254,95 @@ void MeshShape::setMesh( const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever) { - if (mesh == mMesh) { + setMesh(mesh, MeshOwnership::Imported, uri, std::move(resourceRetriever)); +} + +//============================================================================== +namespace { + +std::shared_ptr makeMeshHandle( + const aiScene* mesh, MeshShape::MeshOwnership ownership) +{ + if (!mesh) + return nullptr; + + switch (ownership) { + case MeshShape::MeshOwnership::Imported: + return std::shared_ptr(mesh, [](const aiScene* scene) { // + aiReleaseImport(const_cast(scene)); + }); + case MeshShape::MeshOwnership::Copied: + return std::shared_ptr(mesh, [](const aiScene* scene) { + aiFreeScene(const_cast(scene)); + }); + case MeshShape::MeshOwnership::Manual: + return std::shared_ptr(mesh, [](const aiScene* scene) { + delete const_cast(scene); + }); + case MeshShape::MeshOwnership::Custom: + case MeshShape::MeshOwnership::None: + default: + return std::shared_ptr( + mesh, [](const aiScene*) { /* no-op */ }); + } +} + +} // namespace + +//============================================================================== +void MeshShape::setMesh( + const aiScene* mesh, + MeshOwnership ownership, + const common::Uri& uri, + common::ResourceRetrieverPtr resourceRetriever) +{ + if (mesh == mMesh.get() && ownership == mMeshOwnership) { // Nothing to do. return; } releaseMesh(); - mMesh = mesh; - mMeshOwnership = mesh ? MeshOwnership::Imported : MeshOwnership::None; + mMesh = makeMeshHandle(mesh, ownership); + mMeshOwnership = mesh ? ownership : MeshOwnership::None; + + if (!mMesh) { + mMeshUri.clear(); + mMeshPath.clear(); + mResourceRetriever = nullptr; + return; + } + + mMeshUri = uri; + + if (uri.mScheme.get_value_or("file") == "file" && uri.mPath) { + mMeshPath = uri.getFilesystemPath(); + } else if (resourceRetriever) { + DART_SUPPRESS_DEPRECATED_BEGIN + mMeshPath = resourceRetriever->getFilePath(uri); + DART_SUPPRESS_DEPRECATED_END + } else { + mMeshPath.clear(); + } + + mResourceRetriever = std::move(resourceRetriever); + + incrementVersion(); +} + +//============================================================================== +void MeshShape::setMesh( + std::shared_ptr mesh, + const common::Uri& uri, + common::ResourceRetrieverPtr resourceRetriever) +{ + if (mesh == mMesh && mMeshOwnership == MeshOwnership::Custom) { + return; + } + + releaseMesh(); + mMesh = std::move(mesh); + mMeshOwnership = mMesh ? MeshOwnership::Custom : MeshOwnership::None; if (!mMesh) { mMeshUri.clear(); @@ -363,8 +451,7 @@ ShapePtr MeshShape::clone() const aiScene* new_scene = cloneMesh(); auto new_shape = std::make_shared( - mScale, new_scene, mMeshUri, mResourceRetriever); - new_shape->mMeshOwnership = MeshOwnership::Copied; + mScale, new_scene, mMeshUri, mResourceRetriever, MeshOwnership::Copied); new_shape->mMeshPath = mMeshPath; new_shape->mDisplayList = mDisplayList; new_shape->mColorMode = mColorMode; @@ -418,7 +505,7 @@ aiScene* MeshShape::cloneMesh() const return nullptr; aiScene* new_scene = nullptr; - aiCopyScene(mMesh, &new_scene); + aiCopyScene(mMesh.get(), &new_scene); return new_scene; } diff --git a/dart/dynamics/MeshShape.hpp b/dart/dynamics/MeshShape.hpp index 9a45158293831..976d8b266131a 100644 --- a/dart/dynamics/MeshShape.hpp +++ b/dart/dynamics/MeshShape.hpp @@ -39,6 +39,7 @@ #include +#include #include namespace dart { @@ -47,6 +48,15 @@ namespace dynamics { class DART_API MeshShape : public Shape { public: + enum class MeshOwnership + { + None, + Imported, // from aiImportFile* family; free with aiReleaseImport + Copied, // from aiCopyScene; free with aiFreeScene + Manual, // from manual new aiScene; free with delete + Custom // managed externally via shared_ptr deleter + }; + enum ColorMode { MATERIAL_COLOR = 0, ///< Use the colors specified by the Mesh's material @@ -82,6 +92,15 @@ class DART_API MeshShape : public Shape const Eigen::Vector3d& scale, const aiScene* mesh, const common::Uri& uri = "", + common::ResourceRetrieverPtr resourceRetriever = nullptr, + MeshOwnership ownership = MeshOwnership::Imported); + + /// Constructor that accepts a shared_ptr so callers can supply a custom + /// deleter for aiScene. + MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr mesh, + const common::Uri& uri = "", common::ResourceRetrieverPtr resourceRetriever = nullptr); /// Destructor. @@ -106,11 +125,24 @@ class DART_API MeshShape : public Shape const std::string& path = "", common::ResourceRetrieverPtr resourceRetriever = nullptr); + /// Sets the mesh pointer with explicit ownership semantics. + void setMesh( + const aiScene* mesh, + MeshOwnership ownership, + const common::Uri& path, + common::ResourceRetrieverPtr resourceRetriever = nullptr); + void setMesh( const aiScene* mesh, const common::Uri& path, common::ResourceRetrieverPtr resourceRetriever = nullptr); + /// Sets the mesh using a shared_ptr so callers can provide a custom deleter. + void setMesh( + std::shared_ptr mesh, + const common::Uri& path = "", + common::ResourceRetrieverPtr resourceRetriever = nullptr); + /// Returns URI to the mesh as std::string; an empty string if unavailable. std::string getMeshUri() const; // TODO(DART 7): Replace with getMeshUri2(). @@ -192,16 +224,9 @@ class DART_API MeshShape : public Shape aiScene* cloneMesh() const; - enum class MeshOwnership - { - None, - Imported, // from aiImportFile* family; free with aiReleaseImport - Copied // from aiCopyScene; free with aiFreeScene - }; - void releaseMesh(); - const aiScene* mMesh; + std::shared_ptr mMesh; MeshOwnership mMeshOwnership{MeshOwnership::None}; /// URI the mesh, if available). diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index a9a7c62f8d267..64c6407eab474 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -130,7 +130,7 @@ class DART_API MetaSkeleton : public common::Subject /// Deprecated BodyNode list getter kept for downstream consumers (e.g., /// gz-physics) until they migrate away from it. DART_DEPRECATED(6.13) - virtual const std::vector& getBodyNodes() = 0; + virtual std::vector& getBodyNodes() = 0; /// Deprecated BodyNode list getter kept for downstream consumers (e.g., /// gz-physics) until they migrate away from it. diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 2f7671b01670e..9d85bb650a55f 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -139,7 +139,7 @@ static std::vector& convertVector( } //============================================================================== -const std::vector& ReferentialSkeleton::getBodyNodes() +std::vector& ReferentialSkeleton::getBodyNodes() { return convertVector(mBodyNodes, mRawBodyNodes); } diff --git a/dart/dynamics/ReferentialSkeleton.hpp b/dart/dynamics/ReferentialSkeleton.hpp index fba3ba1372dfe..5d521d5a252ae 100644 --- a/dart/dynamics/ReferentialSkeleton.hpp +++ b/dart/dynamics/ReferentialSkeleton.hpp @@ -109,7 +109,7 @@ class ReferentialSkeleton : public MetaSkeleton const BodyNode* getBodyNode(const std::string& name) const override; // Documentation inherited - const std::vector& getBodyNodes() override; + std::vector& getBodyNodes() override; // Documentation inherited const std::vector& getBodyNodes() const override; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index b519859c90cf9..3b451d5448075 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -954,7 +954,7 @@ static std::vector& convertToConstPtrVector( } //============================================================================== -const std::vector& Skeleton::getBodyNodes() +std::vector& Skeleton::getBodyNodes() { return mSkelCache.mBodyNodes; } diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index 6fb39bdfd6841..03efe1293bd38 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -359,7 +359,7 @@ class DART_API Skeleton /// Deprecated list getter retained for backward compatibility until /// gz-physics migrates. DART_DEPRECATED(6.13) - const std::vector& getBodyNodes() override; + std::vector& getBodyNodes() override; /// Deprecated list getter retained for backward compatibility until /// gz-physics migrates. diff --git a/dart/gui/InteractiveFrame.cpp b/dart/gui/InteractiveFrame.cpp index ad19dbc1eb757..a332c474e2aaa 100644 --- a/dart/gui/InteractiveFrame.cpp +++ b/dart/gui/InteractiveFrame.cpp @@ -445,7 +445,12 @@ void InteractiveFrame::createStandardVisualizationShapes( scene->mRootNode = node; std::shared_ptr shape( - new dart::dynamics::MeshShape(Eigen::Vector3d::Ones(), scene)); + new dart::dynamics::MeshShape( + Eigen::Vector3d::Ones(), + scene, + common::Uri(), + nullptr, + dart::dynamics::MeshShape::MeshOwnership::Manual)); shape->setColorMode(dart::dynamics::MeshShape::COLOR_INDEX); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); @@ -529,7 +534,12 @@ void InteractiveFrame::createStandardVisualizationShapes( scene->mRootNode = node; std::shared_ptr shape( - new dart::dynamics::MeshShape(Eigen::Vector3d::Ones(), scene)); + new dart::dynamics::MeshShape( + Eigen::Vector3d::Ones(), + scene, + common::Uri(), + nullptr, + dart::dynamics::MeshShape::MeshOwnership::Manual)); shape->setColorMode(dart::dynamics::MeshShape::COLOR_INDEX); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); diff --git a/scripts/patch_gz_physics.py b/scripts/patch_gz_physics.py index 18174dcb4fff4..a3ba2ee43148c 100755 --- a/scripts/patch_gz_physics.py +++ b/scripts/patch_gz_physics.py @@ -319,12 +319,61 @@ def inject_make_and_register_overload(gtest_root: Path) -> bool: return True +def patch_custom_mesh_shape(mesh_shape_path: Path) -> bool: + """ + Update CustomMeshShape to use MeshShape::setMesh with explicit ownership + now that MeshShape stores meshes in shared_ptr. + """ + if not mesh_shape_path.exists(): + print(f"Error: CustomMeshShape not found at {mesh_shape_path}", file=sys.stderr) + return False + + text = mesh_shape_path.read_text() + include_snippet = "#include \n" + uri_include = "#include \n" + if include_snippet in text and uri_include not in text: + text = text.replace(include_snippet, include_snippet + "\n" + uri_include, 1) + + old_tail = ( + " this->mMesh = scene;\n" + " this->mIsBoundingBoxDirty = true;\n" + " this->mIsVolumeDirty = true;\n" + "}\n" + ) + new_tail = ( + " this->setMesh(\n" + " scene,\n" + " dart::dynamics::MeshShape::MeshOwnership::Manual,\n" + " dart::common::Uri(),\n" + " nullptr);\n" + " this->mIsBoundingBoxDirty = true;\n" + " this->mIsVolumeDirty = true;\n" + "}\n" + ) + if old_tail not in text: + if new_tail in text: + print("✓ CustomMeshShape already patched") + mesh_shape_path.write_text(text) + return True + print( + "Error: Failed to locate CustomMeshShape mesh assignment block", + file=sys.stderr, + ) + return False + + text = text.replace(old_tail, new_tail, 1) + mesh_shape_path.write_text(text) + print(f"✓ Patched CustomMeshShape to use MeshShape::setMesh in {mesh_shape_path}") + return True + + def main(): """Main entry point for the script.""" # Get the gz-physics CMakeLists.txt path repo_root = Path(__file__).parent.parent cmake_file = repo_root / ".deps" / "gz-physics" / "CMakeLists.txt" gtest_root = cmake_file.parent / "test" / "gtest_vendor" + custom_mesh_shape = cmake_file.parent / "dartsim" / "src" / "CustomMeshShape.cc" # Patch versions old_version = "6.10" @@ -338,6 +387,8 @@ def main(): if success: success = inject_make_and_register_overload(gtest_root) and success + if success: + success = patch_custom_mesh_shape(custom_mesh_shape) and success # Exit with appropriate code sys.exit(0 if success else 1) diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index ca0797ead46e0..25cd9d0a0ab77 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -73,6 +73,7 @@ dart_add_test( "unit" UNIT_dynamics_BodyNodePotentialEnergy dynamics/test_BodyNodePotentialEnergy.cpp) dart_add_test("unit" UNIT_dynamics_ShapeNodeInertia dynamics/test_ShapeNodeInertia.cpp) dart_add_test("unit" UNIT_dynamics_SkeletonClone dynamics/test_SkeletonClone.cpp) +dart_add_test("unit" UNIT_dynamics_SkeletonAccessors dynamics/test_SkeletonAccessors.cpp) dart_add_test("unit" UNIT_dynamics_Noexcept dynamics/test_Noexcept.cpp) dart_add_test( "unit" UNIT_dynamics_BodyNodeCollisionSignals dynamics/test_BodyNodeCollisionSignals.cpp) diff --git a/tests/unit/dynamics/test_MeshShape.cpp b/tests/unit/dynamics/test_MeshShape.cpp index ccf0dcb73ab1b..87560143d487d 100644 --- a/tests/unit/dynamics/test_MeshShape.cpp +++ b/tests/unit/dynamics/test_MeshShape.cpp @@ -1,14 +1,17 @@ #include "dart/common/LocalResourceRetriever.hpp" #include "dart/common/Uri.hpp" #include "dart/config.hpp" +#include "dart/dynamics/ArrowShape.hpp" #include "dart/dynamics/AssimpInputResourceAdaptor.hpp" #include "dart/dynamics/MeshShape.hpp" +#include #include #include #include #include +#include #include #include #include @@ -53,6 +56,28 @@ class AliasUriResourceRetriever final : public common::ResourceRetriever common::LocalResourceRetrieverPtr mDelegate; }; +class RecordingRetriever final : public common::ResourceRetriever +{ +public: + bool exists(const common::Uri&) override + { + return true; + } + + common::ResourcePtr retrieve(const common::Uri&) override + { + return nullptr; + } + + std::string getFilePath(const common::Uri& uri) override + { + lastUri = uri.toString(); + return "/virtual/path/from/retriever"; + } + + std::string lastUri; +}; + const aiScene* loadMeshWithOverrides( const std::string& uri, const common::ResourceRetrieverPtr& retriever, @@ -227,3 +252,82 @@ TEST(MeshShapeTest, ColladaUriWithoutExtensionStillLoads) << "aliasExtents=" << aliasExtents.transpose() << ", canonicalExtents=" << canonicalExtents.transpose(); } + +TEST(MeshShapeTest, RespectsCustomMeshDeleter) +{ + std::atomic deleted{0}; + + { + auto scene = std::shared_ptr( + new aiScene, [&deleted](const aiScene* mesh) { + ++deleted; + delete const_cast(mesh); + }); + + dynamics::MeshShape shape(Eigen::Vector3d::Ones(), scene); + EXPECT_EQ(shape.getMesh(), scene.get()); + } + + EXPECT_EQ(deleted.load(), 1); +} + +TEST(MeshShapeTest, TracksOwnershipAndUriMetadata) +{ + auto retriever = std::make_shared(); + const common::Uri fileUri + = common::Uri::createFromStringOrPath("/tmp/manual-mesh.dae"); + + auto* manualScene = new aiScene; + dynamics::MeshShape shape( + Eigen::Vector3d::Ones(), + manualScene, + fileUri, + retriever, + dynamics::MeshShape::MeshOwnership::Manual); + EXPECT_EQ(shape.getMesh(), manualScene); + EXPECT_EQ(shape.getMeshPath(), fileUri.getFilesystemPath()); + EXPECT_EQ(shape.getMeshUri(), fileUri.toString()); + + const common::Uri retrieverUri("package://example/mesh.dae"); + auto* retrieverScene = new aiScene; + shape.setMesh( + retrieverScene, + dynamics::MeshShape::MeshOwnership::Manual, + retrieverUri, + retriever); + EXPECT_EQ(retriever->lastUri, retrieverUri.toString()); + EXPECT_EQ(shape.getMeshPath(), "/virtual/path/from/retriever"); + EXPECT_EQ(shape.getMesh(), retrieverScene); + + // No-op when the mesh pointer and ownership are unchanged. + shape.setMesh( + shape.getMesh(), + dynamics::MeshShape::MeshOwnership::Manual, + retrieverUri, + retriever); + + // Clearing the mesh resets related metadata. + shape.setMesh( + nullptr, + dynamics::MeshShape::MeshOwnership::Manual, + common::Uri(), + nullptr); + EXPECT_EQ(shape.getMesh(), nullptr); + EXPECT_TRUE(shape.getMeshPath().empty()); + EXPECT_TRUE(shape.getMeshUri().empty()); +} + +TEST(ArrowShapeTest, CloneUsesMeshOwnershipSemantics) +{ + dynamics::ArrowShape arrow( + Eigen::Vector3d::Zero(), + Eigen::Vector3d::UnitX(), + dynamics::ArrowShape::Properties(), + Eigen::Vector4d::Ones(), + 4); + + auto cloned = std::dynamic_pointer_cast(arrow.clone()); + ASSERT_TRUE(cloned); + ASSERT_NE(cloned->getMesh(), nullptr); + EXPECT_NE(cloned->getMesh(), arrow.getMesh()); +} diff --git a/tests/unit/dynamics/test_SkeletonAccessors.cpp b/tests/unit/dynamics/test_SkeletonAccessors.cpp new file mode 100644 index 0000000000000..75130916abac6 --- /dev/null +++ b/tests/unit/dynamics/test_SkeletonAccessors.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * 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 + +#include + +#include + +using namespace dart::dynamics; + +//============================================================================== +TEST(SkeletonAccessors, ReturnsMutableBodyNodeVector) +{ + auto skeleton = Skeleton::create("skeleton"); + auto pair = skeleton->createJointAndBodyNodePair(); + auto* body = pair.second; + + DART_SUPPRESS_DEPRECATED_BEGIN + auto& nodes = skeleton->getBodyNodes(); + DART_SUPPRESS_DEPRECATED_END + ASSERT_EQ(nodes.size(), 1u); + EXPECT_EQ(nodes.front(), body); +} + +//============================================================================== +TEST(ReferentialSkeletonAccessors, ReturnsMutableBodyNodeVector) +{ + auto skeleton = Skeleton::create("skeleton"); + auto pair = skeleton->createJointAndBodyNodePair(); + + auto group = Group::create("group"); + ASSERT_TRUE(group->addComponent(pair.second)); + + DART_SUPPRESS_DEPRECATED_BEGIN + auto& nodes = group->getBodyNodes(); + DART_SUPPRESS_DEPRECATED_END + ASSERT_EQ(nodes.size(), 1u); + EXPECT_EQ(nodes.front(), pair.second); +}