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.