Memory error during destruction of ShapeNodePtr

[azeey]

Bug Report

Environment

• DART version: 6.9.2 and later
• OS name and version name(or number): Ubuntu 18.04
• Compiler name and version number: GCC 8.4.0

Expected Behavior

No error during destruction.

Current Behavior

When a ShapeNodePtr outlives a SkeletonPtr from which it was obtained, some sort memory error occurs during the destruction of the ShapeNodePtr. ASAN indicates it's a use-after-free, but I'm not certain. Here's a snippet of code that demonstrates how this could occur:

int main(int argc, char *argv[])
{
  SkeletonPtr sphere = createSphere();

  ShapeNodePtr shape1 = sphere->getBodyNode(0)->getShapeNode(0);
  // Do something with shape1

  // Delete sphere before shape1
  sphere.reset();
  return 0;
}

The error causes an intermittent segfault. Running it under valgrind reliably yields a few errors. I have also reproduced it using ASAN in gcc. Here's a sample the valgrind error (full version https://gist.github.com/azeey/cca4055cc191d13742cd90becc845479)

==4215== Invalid read of size 8
==4215==    at 0x51E5202: std::_Hashtable<dart::dynamics::JacobianNode*, dart::dynamics::JacobianNode*, std::allocator<dart::dynamics::JacobianNode*>, std::__detail::_Identity, std::equal_to<dart::dynamics::JacobianNode*>, std::hash<dart::dynamics::JacobianNode*>, std::__detail::_Mod_range_hashing, std::__detail::_Default_ranged_hash, std::__detail::_Prime_rehash_policy, std::__detail::_Hashtable_traits<false, true, true> >::_M_erase(std::integral_constant<bool, true>, dart::dynamics::JacobianNode* const&) [clone .isra.50] (hashtable.h:1907)
==4215==    by 0x51E5879: erase (hashtable.h:759)
==4215==    by 0x51E5879: erase (unordered_set.h:544)
==4215==    by 0x51E5879: dart::dynamics::JacobianNode::~JacobianNode() (JacobianNode.cpp:46)
==4215==    by 0x5156FD6: ~TemplatedJacobianNode (TemplatedJacobianNode.hpp:50)
==4215==    by 0x5156FD6: ~EntityNodeBase (EntityNodeAspect.hpp:78)
==4215==    by 0x5156FD6: ~EntityNode (EntityNode.hpp:60)
==4215==    by 0x5156FD6: ~CompositeJoiner (CompositeJoiner.hpp:70)
==4215==    by 0x5156FD6: dart::dynamics::FixedJacobianNode::~FixedJacobianNode() (FixedJacobianNode.hpp:41)
==4215==    by 0x5251D53: ~CompositeJoiner (CompositeJoiner.hpp:70)
==4215==    by 0x5251D53: ~CompositeStateNode (CompositeNode.hpp:44)
==4215==    by 0x5251D53: ~CompositePropertiesNode (CompositeNode.hpp:71)
==4215==    by 0x5251D53: dart::dynamics::CompositeNode<dart::common::CompositeJoiner<dart::dynamics::FixedJacobianNode, dart::dynamics::ShapeFrame> >::~CompositeNode() (CompositeNode.hpp:99)
==4215==    by 0x5252992: dart::dynamics::ShapeNode::~ShapeNode() (ShapeNode.hpp:73)
==4215==    by 0x5252B80: dart::dynamics::ShapeNode::~ShapeNode() (ShapeNode.hpp:73)
==4215==    by 0x5215271: dart::dynamics::NodeDestructor::~NodeDestructor() (Node.cpp:55)
==4215==    by 0x513A899: std::_Sp_counted_ptr<dart::dynamics::NodeDestructor*, (__gnu_cxx::_Lock_policy)2>::_M_dispose() (shared_ptr_base.h:376)
==4215==    by 0x119F43: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/developer/build/test1)
==4215==    by 0x118C98: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/developer/build/test1)
==4215==    by 0x118391: std::__shared_ptr<dart::dynamics::NodeDestructor, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/developer/build/test1)
==4215==    by 0x1183AD: std::shared_ptr<dart::dynamics::NodeDestructor>::~shared_ptr() (in /home/developer/build/test1)
==4215==  Address 0xb885f90 is 720 bytes inside a block of size 3,760 free'd
==4215==    at 0x4C30D3B: free (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==4215==    by 0x5133528: aligned_free (Memory.h:177)
==4215==    by 0x5133528: conditional_aligned_free<true> (Memory.h:230)
==4215==    by 0x5133528: operator delete (BodyNode.hpp:1199)
==4215==    by 0x5133528: dart::dynamics::BodyNode::~BodyNode() (BodyNode.cpp:242)
==4215==    by 0x5265B7E: dart::dynamics::Skeleton::~Skeleton() (Skeleton.cpp:395)
==4215==    by 0x5265FF0: dart::dynamics::Skeleton::~Skeleton() (Skeleton.cpp:396)
==4215==    by 0x526DD92: std::_Sp_counted_ptr<dart::dynamics::Skeleton*, (__gnu_cxx::_Lock_policy)2>::_M_dispose() (shared_ptr_base.h:376)
==4215==    by 0x119F43: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/developer/build/test1)
==4215==    by 0x118C98: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/developer/build/test1)
==4215==    by 0x1182DD: std::__shared_ptr<dart::dynamics::Skeleton, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/developer/build/test1)
==4215==    by 0x119979: std::__shared_ptr<dart::dynamics::Skeleton, (__gnu_cxx::_Lock_policy)2>::reset() (in /home/developer/build/test1)
==4215==    by 0x117B31: main (in /home/developer/build/test1)
==4215==  Block was alloc'd at
==4215==    at 0x4C2FB0F: malloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==4215==    by 0x118025: Eigen::internal::aligned_malloc(unsigned long) (in /home/developer/build/test1)
==4215==    by 0x11871A: void* Eigen::internal::conditional_aligned_malloc<true>(unsigned long) (in /home/developer/build/test1)
==4215==    by 0x118252: dart::dynamics::BodyNode::operator new(unsigned long) (in /home/developer/build/test1)
==4215==    by 0x119651: std::pair<dart::dynamics::FreeJoint*, dart::dynamics::BodyNode*> dart::dynamics::Skeleton::createJointAndBodyNodePair<dart::dynamics::FreeJoint, dart::dynamics::BodyNode>(dart::dynamics::BodyNode*, dart::dynamics::FreeJoint::Properties const&, dart::dynamics::BodyNode::Properties const&) (in /home/developer/build/test1)
==4215==    by 0x11779F: createObject() (in /home/developer/build/test1)
==4215==    by 0x117981: createSphere() (in /home/developer/build/test1)
==4215==    by 0x117AE6: main (in /home/developer/build/test1)

Steps to Reproduce

https://github.com/azeey/sandbox/tree/master/dart-ShapeNodePtr