humanoid-path-planner/hpp-fcl

Can't register Plane or Halfplane to DynamicAABBTreeCollisionManager

salihmarangoz opened this issue · 4 comments

salihmarangoz commented

DynamicAABBTreeCollisionManager works well with spheres but when I try it with Plane or Halfplane the process crashes. Looks similar to flexible-collision-library/fcl#455

Package version (ROS Noetic): 2.4.0-1focal.20231204.185100

Output of AddressSanitizer:

==195736==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000000 (pc 0x7f189b57df89 bp 0x60e0000370a8 sp 0x7f18133d09c0 T37)
==195736==The signal is caused by a READ memory access.
==195736==Hint: address points to the zero page.
    #0 0x7f189b57df88 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:307
    #1 0x7f189b57edd4 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:397
    #2 0x7f189b57edab in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:391
    #3 0x7f189b57edbd in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:392
    #4 0x7f189b57edab in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:391
    #5 0x7f189b57f044 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::init_0(std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> >&) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:468
    #6 0x7f189b57ab51 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::init(std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> >&, int) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:83
    #7 0x7f189b57ab51 in hpp::fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector<hpp::fcl::CollisionObject*, std::allocator<hpp::fcl::CollisionObject*> > const&) src/broadphase/broadphase_dynamic_AABB_tree.cpp:471
jcarpent commented

Could you share a reproducible example please?

salihmarangoz commented

Did some tests:

  • Spheres -> ok
  • Inflated spheres -> ok
  • Spheres + Inflated spheres -> ok
  • Spheres + plane -> ok
  • Inflated spheres + plane -> segmentation fault

Multiple inflated spheres are ok. But planes don't work together with inflated spheres. Perhaps is there a problem with using the copy constructor of Sphere?

#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/broadphase/broadphase.h>

using hpp::fcl::DynamicAABBTreeCollisionManager;
using hpp::fcl::Vec3f;
using hpp::fcl::CollisionObject;
using hpp::fcl::Sphere;
using hpp::fcl::Plane;


int main()
{
  DynamicAABBTreeCollisionManager collision_manager;

  std::vector<CollisionObject*> collision_objects;

  // auto o1 = new CollisionObject(hpp::fcl::make_shared<Sphere>(0.1));
  // o1->setTranslation(Vec3f(0.2, 0.1, 0.4));
  // o1->computeAABB();
  // collision_objects.push_back(o1);

  // auto o2 = new CollisionObject(hpp::fcl::make_shared<Sphere>(0.2));
  // o2->setTranslation(Vec3f(0.3, 0.2, 0.4));
  // o2->computeAABB();
  // collision_objects.push_back(o2);

  auto o3 = new CollisionObject(hpp::fcl::make_shared<Plane>(0.0, 0.0, 1.0, 0.0));
  o3->computeAABB();
  collision_objects.push_back(o3);

  auto o4 = new CollisionObject(hpp::fcl::make_shared<Sphere>(Sphere(0.05).inflated(0.05).first));
  o4->setTranslation(Vec3f(0.1, 0.3, 0.4));
  o4->computeAABB();
  collision_objects.push_back(o4);

  collision_manager.registerObjects(collision_objects);
  collision_manager.update();
}

AddressSanitizer output:

AddressSanitizer:DEADLYSIGNAL
=================================================================
==218156==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000000 (pc 0x7f5b4ff8df89 bp 0x602000000270 sp 0x7ffd27381ec0 T0)
==218156==The signal is caused by a READ memory access.
==218156==Hint: address points to the zero page.
    #0 0x7f5b4ff8df88 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:307
    #1 0x7f5b4ff8edd4 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:397
    #2 0x7f5b4ff8f044 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::init_0(std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> >&) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:468
    #3 0x7f5b4ff8ab51 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::init(std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> >&, int) include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:83
    #4 0x7f5b4ff8ab51 in hpp::fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector<hpp::fcl::CollisionObject*, std::allocator<hpp::fcl::CollisionObject*> > const&) src/broadphase/broadphase_dynamic_AABB_tree.cpp:471
    #5 0x555ca7021717 in main (/home/salih/catkin_ws/devel_release/.private/hpp_fcl_issue_514/lib/hpp_fcl_issue_514/hpp_fcl_issue_514_node+0x6717)
    #6 0x7f5b4f84c082 in __libc_start_main ../csu/libc-start.c:308
    #7 0x555ca7021d3d in _start (/home/salih/catkin_ws/devel_release/.private/hpp_fcl_issue_514/lib/hpp_fcl_issue_514/hpp_fcl_issue_514_node+0x6d3d)

AddressSanitizer can not provide additional info.
SUMMARY: AddressSanitizer: SEGV include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h:307 in hpp::fcl::detail::HierarchyTree<hpp::fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>**, std::vector<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*, std::allocator<hpp::fcl::detail::NodeBase<hpp::fcl::AABB>*> > >)
==218156==ABORTING
lmontaut commented

Hi @salihmarangoz,
Thanks for reporting this issue! The problem was due do uninitialized iterators, it should be fixed with #515.

  • 🎉1
jcarpent commented

Solved via #515