collision_helper.cpp

#include "impl/collision_helper.h"
#include <moveit/collision_detection/collision_common.h>
#include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>

bool Collision_helper::collision_detector(const std::string& object1_name, const std::string& object2_name){ 
    for (std::map<std::string, collision_detection::FCLObject>::const_iterator it = fcl_objs_.begin(); it != fcl_objs_.end(); ++it){
        std::string obj = it->first;
        ROS_INFO("object %s", obj.c_str());
    }
    /*
    auto objA = fcl_objs_.at(object1_name);
    auto objB = fcl_objs_.at(object2_name);

    

    auto manager1 = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
    auto manager2 = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();

    objA.registerTo(manager1.get());
    objB.registerTo(manager2.get());

    // Perform collision detection
    collision_detection::CollisionRequest req;
    collision_detection::CollisionResult res;
    collision_detection::CollisionData cd;
    cd.req_ = &req;
    cd.res_ = &res;
    manager1->collide(manager2.get(), &cd, &collision_detection::collisionCallback);
    */
    //ROS_INFO("läuft %i", fcl_objs_.size());
    return true;
}