test_fcl_octomap_distance.cpp cannot run

[Li-zhuoran]

When detecting the distance of obj, the example uses the following method::

OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
CollisionObject<S> tree_obj(std::shared_ptr<CollisionGeometry<S>>(tree));

DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>();
manager->registerObjects(env);
manager->setup();

DefaultDistanceData<S> cdata;
test::TStruct t1;
test::Timer timer1;
timer1.start();
manager->octree_as_geometry_collide = false;
manager->octree_as_geometry_distance = false;
manager->distance(&tree_obj, &cdata, DefaultDistanceFunction);
timer1.stop();
t1.push_back(timer1.getElapsedTime());

The specific error I encountered is in the call to the distance function. The relevant function implementation is as follows:

template <typename S>
FCL_EXPORT
void DynamicAABBTreeCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
{
  DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
  if((size() == 0) || (other_manager->size() == 0)) return;
  S min_dist = std::numeric_limits<S>::max();
  detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}

I noticed that S min_dist is initialized as std::numeric_limits<S>::max(). I tried setting this value to 0, and the program seems to run fine.

[Li-zhuoran]

it looks like the calculated distance is still wrong.