search

flexible-collision-library / fcl

Flexible Collision Library
https://flexible-collision-library.github.io/
Other
1.39k stars 417 forks source link

creating a collision object from octree #602

Closed tejalbarnwal closed 1 year ago

tejalbarnwal commented 1 year ago

Hi, I am actually trying to use FCL to detect collision between octree and a robot. For this, I created a sample cpp file. It can be found below

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

#include <fcl/narrowphase/collision_object.h>
#include <fcl/geometry/octree/octree.h>
#include <fcl/geometry/shape/box.h>
#include <fcl/narrowphase/collision_request.h>
#include <fcl/narrowphase/collision_result.h>
#include <fcl/narrowphase/collision.h>

int main(int argc, char const *argv[])
{
    // read an binary file to obtain an Octree
    const std::string filename = "/home/gourav-ast/slam_repos/octomap_tutorial/examples/build/simple_tree.bt";
    octomap::OcTree temp_tree(0.1);
    bool success = temp_tree.readBinary(filename);

    std::cout << "Read octree?: " << success << "\n";

    // // use octree to create a collision object map to be used with FCL
    fcl::OcTree<float>* tree = new fcl::OcTree<float>(std::shared_ptr<const octomap::OcTree>(&temp_tree));
    fcl::CollisionObject<float> treeCollisionObj((std::shared_ptr<fcl::CollisionGeometry<float>>(tree)));

    // // we model a sample robot with box and sphere collision  geometry
    // std::shared_ptr<fcl::CollisionGeometry<float>> robot(new fcl::Box<float>(0.3, 0.3, 0.1));
    // fcl::CollisionObject<float> robotCollisionObj(robot);

    // // perform collision checking between collision object tree and collision object robot
    // fcl::Vector3f translation(10.0, 10.0, 10.0);
    // fcl::Quaternionf rotation(0.988, 0.094, 0.079, 0.094);
    // robotCollisionObj.setTransform(rotation, translation);
    // fcl::CollisionRequest<float> requestType(1, false, 1, false);
    // fcl::CollisionResult<float> collisionResult;
    // fcl::collide(&robotCollisionObj, &treeCollisionObj, requestType, collisionResult);

    // std::cout << "Is collision detected:\t" << collisionResult.isCollision();
    return 0;
}

I was actually getting output as

Read octree?: 1
double free or corruption (out)
Aborted (core dumped)

I havent used valgrind before, so i thought to use it. With that I was able to know that the error is coming from the line fcl::CollisionObject<float> treeCollisionObj((std::shared_ptr<fcl::CollisionGeometry<float>>(tree))); but i wasnt able to debug it. Please help me. For your reference I have attached the valgrind output below

Valgring Output 
==29227== Memcheck, a memory error detector
==29227== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==29227== Using Valgrind-3.15.0 and LibVEX; rerun with -h for copyright info
==29227== Command: ./readOctomap
==29227== 
Read octree?: 1
==29227== Invalid free() / delete / delete[] / realloc()
==29227==    at 0x483D1CF: operator delete(void*, unsigned long) (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==29227==    by 0x17A72A: octomap::OcTree::~OcTree() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x18E96B: std::_Sp_counted_ptr<octomap::OcTree*, (__gnu_cxx::_Lock_policy)2>::_M_dispose() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17E3C9: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C3C0: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AE61: std::__shared_ptr<octomap::OcTree const, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AE81: std::shared_ptr<octomap::OcTree const>::~shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x18E891: fcl::OcTree<float>::~OcTree() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x18E8BD: fcl::OcTree<float>::~OcTree() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x178C81: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==  Address 0x1ffeffead0 is on thread 1's stack
==29227==  in frame #9, created by main (???:)
==29227== 
==29227== Invalid read of size 8
==29227==    at 0x18E8EF: std::_Sp_counted_ptr<fcl::OcTree<float>*, (__gnu_cxx::_Lock_policy)2>::_M_dispose() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17E3C9: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C3C0: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEA5: std::__shared_ptr<fcl::CollisionGeometry<float>, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEC5: std::shared_ptr<fcl::CollisionGeometry<float> >::~shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C6C1: fcl::CollisionObject<float>::~CollisionObject() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x178C95: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==  Address 0x592dc20 is 0 bytes inside a block of size 104 free'd
==29227==    at 0x483D1CF: operator delete(void*, unsigned long) (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==29227==    by 0x18E8CE: fcl::OcTree<float>::~OcTree() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x178C81: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==  Block was alloc'd at
==29227==    at 0x483BE63: operator new(unsigned long) (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==29227==    by 0x178C01: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227== 
==29227== Jump to the invalid address stated on the next line
==29227==    at 0x0: ???
==29227==    by 0x17E3C9: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C3C0: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEA5: std::__shared_ptr<fcl::CollisionGeometry<float>, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEC5: std::shared_ptr<fcl::CollisionGeometry<float> >::~shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C6C1: fcl::CollisionObject<float>::~CollisionObject() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x178C95: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==  Address 0x0 is not stack'd, malloc'd or (recently) free'd
==29227== 
==29227== 
==29227== Process terminating with default action of signal 11 (SIGSEGV)
==29227==  Bad permissions for mapped region at address 0x0
==29227==    at 0x0: ???
==29227==    by 0x17E3C9: std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C3C0: std::__shared_count<(__gnu_cxx::_Lock_policy)2>::~__shared_count() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEA5: std::__shared_ptr<fcl::CollisionGeometry<float>, (__gnu_cxx::_Lock_policy)2>::~__shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17AEC5: std::shared_ptr<fcl::CollisionGeometry<float> >::~shared_ptr() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x17C6C1: fcl::CollisionObject<float>::~CollisionObject() (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227==    by 0x178C95: main (in /home/gourav-ast/slam_repos/octomap_tutorial/examples/build/readOctomap)
==29227== 
==29227== HEAP SUMMARY:
==29227==     in use at exit: 6,110 bytes in 27 blocks
==29227==   total heap usage: 1,142 allocs, 1,116 frees, 3,120,870 bytes allocated
==29227== 
==29227== LEAK SUMMARY:
==29227==    definitely lost: 0 bytes in 0 blocks
==29227==    indirectly lost: 0 bytes in 0 blocks
==29227==      possibly lost: 0 bytes in 0 blocks
==29227==    still reachable: 6,110 bytes in 27 blocks
==29227==         suppressed: 0 bytes in 0 blocks
==29227== Rerun with --leak-check=full to see details of leaked memory
==29227== 
==29227== For lists of detected and suppressed errors, rerun with: -s
==29227== ERROR SUMMARY: 3 errors from 3 contexts (suppressed: 0 from 0)
Segmentation fault (core dumped)
tejalbarnwal commented 1 year ago

Hey, I was able to solve it with the following code.

std::shared_ptr<const octomap::OcTree> sharedTree = std::make_shared<const octomap::OcTree>(temp_tree);
fcl::OcTree<float>* tree = new fcl::OcTree<float>(sharedTree);

Thank you