MoveIt: How to initialize collision_detection::CollisionEnvFCL with the currently active environment so I can use CollisionEnvFCL::distanceRobot()?

Question

I would like to use the collision_detection::CollisionEnvFCL::distanceRobot() method from MoveIt to compute the minimum distance between a robot and an object (along with some other info that this method provides).

However, I do not know how to properly initialize collision_detection::CollisionEnvFCL with the currently active environment that holds the robot and the object.

This is my attempt so far:

#include <ros/ros.h>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit_msgs/GetPlanningScene.h>


int main(int argc, char** argv)
{
  ros::init(argc, argv, "panda_coll_detection");
  ros::AsyncSpinner spinner(8);
  spinner.start();

  // ---------------------------------------------------------------------------
  // Define interfaces
  // ---------------------------------------------------------------------------

  // Define move_group_interface & planning_scene_interface
  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

  // Define robot model
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();

  // ---------------------------------------------------------------------------
  // Add a collision object
  // ---------------------------------------------------------------------------

  // Define a collision object ROS message for the robot to avoid
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group_interface.getPlanningFrame();
  collision_object.id = "box1";

  // Define a box to add to the world
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.4;
  primitive.dimensions[primitive.BOX_Y] = 0.02;
  primitive.dimensions[primitive.BOX_Z] = 0.02;

  // Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 1.0;//0.5
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.95; //0.35

  // Add box to collision_object
  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  // Define a vector of collision objects that could contain additional objects
  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // Add the collision object into the world  
  planning_scene_interface.applyCollisionObjects(collision_objects);
    
  // ---------------------------------------------------------------------------
  // Get (updated) planning scene
  // ---------------------------------------------------------------------------
  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(kinematic_model);

  ros::NodeHandle h;
  ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");

  ros::Duration timeout(1.0);
  if (client.waitForExistence(timeout)) {
    moveit_msgs::GetPlanningScene::Request req;
    moveit_msgs::GetPlanningScene::Response res;

    if (client.call(req, res))
        planning_scene->setPlanningSceneMsg(res.scene); // apply result to actual PlanningScene
  }

    
  // ---------------------------------------------------------------------------
  // Collision Checking with CollisionEnvFCL.distanceRobot()
  // ---------------------------------------------------------------------------
  
  // Define distance request & result
  auto distance_request = collision_detection::DistanceRequest();
  auto distance_result = collision_detection::DistanceResult();
  distance_request.enable_nearest_points =true;

  // Get robot state 
  moveit::core::RobotState copied_state2 = planning_scene->getCurrentState();
  
  // Get the active collision environment as a CollisionEnvFCL type
  // -------- This is where I need help ---------
  const collision_detection::CollisionEnvConstPtr c_env = planning_scene->getCollisionEnv();
  collision_detection::CollisionEnvFCL collision_env = ???
  // --------------------------------------------

  
  // Compute the shortest distance between the robot and the world
  collision_env.distanceRobot(distance_request, distance_result, copied_state2);

  // Show result
  ROS_INFO_STREAM("Current state is " << (distance_result.collision ? "in" : "not in") << " collision");
  ROS_INFO_STREAM("Min distance between two bodies=" << distance_result.minimum_distance.distance << "\n");

  ros::shutdown();
  return 0;
}

So, how can I initialize a collision_detection::CollisionEnvFCL environment with the currently active environment? Do I need to cast c_env to CollisionEnvFCL and if yes, how?

Answer 1

In case anyone else is interested, this is how I solved it:

First of all, it turns out that the active collision environment was indeed the FCL environment, so I could perform the collision checking with just this:

const collision_detection::CollisionEnvConstPtr collision_env = planning_scene->getCollisionEnv(); 

However, in general, you can change the active environment to be the FCL one with:

#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>

planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);
ROS_INFO_STREAM("Current active collision env is: " << planning_scene->getActiveCollisionDetectorName());