Collision between planning scene objects (e.g. CollisionObjectA and CollisionObjectB)

edit

Thanks to @fvd for the idea.

Step 1: Make sure to add the new virtual public function in "collision_env.h".

  virtual bool checkObjectPairCollision(std::string const& object1_name, std::string const& object2_name) const
  {
    ROS_ERROR_STREAM_NAMED("collision_detection", "checkObjectPairCollision is unimplemented");
    return true;
  }

Step 2: In "collision_env_fcl.h", add the following declaration

  virtual bool checkObjectPairCollision(std::string const& object1_name, std::string const& object2_name) const override;

Step 3: In "collision_env_fcl.cpp", add the definition

bool CollisionEnvFCL::checkObjectPairCollision(const std::string& object1_name,
                                               const std::string& object2_name
                                              ) const
{
  auto fcl_obj1 = fcl_objs_.at(object1_name);
  auto fcl_obj2 = fcl_objs_.at(object2_name);

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

  // Not sure if this is an optimized implementation. This could be possibly optimized, but it works.
  fcl_obj1.registerTo(manager1.get());
  fcl_obj2.registerTo(manager2.get());

  // Perform collision detection
  CollisionRequest req;
  CollisionResult res;
  CollisionData cd;
  cd.req_ = &req;
  cd.res_ = &res;

  manager1->collide(manager2.get(), &cd, &collisionCallback);

  return cd.res_->collision;
}

Step 4: Using it in your planning scene is easy. Try this.

// Suppose that "planning_scene_" is your PlanningScenePtr.
auto env = planning_scene_->getCollisionEnv();
bool coll = env->checkObjectPairCollision("object_A", "object_B"); //
ROS_WARN_STREAM("Collision: " << coll);

Caveats/Possible improvements:

1. There are some more changes that I made for my own use case. But, the above code should work for anyone.
2. I did not implement the function for other collision detectors (i.e., bullet).
3. Instead of single objects, this could be used for two sets of objects by simply modifying the code to take in a vector of strings.

edit: in case anyone wants to track this over at GitHub MoveIt repo, here is the issue https://github.com/ros-planning/moveit/issues/2097