Pass more distance information out from FCL collision check (#2572)

* Pass more distance information out from FCL collision check

* Update moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

Co-authored-by: Sebastian Jahr <sebastian.jahr@tuta.io>

* Update moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -141,41 +141,7 @@ struct CostSource
   }
 };
 
-/** \brief Representation of a collision checking result */
-struct CollisionResult
-{
-  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /** \brief Clear a previously stored result */
-  void clear()
-  {
-    collision = false;
-    distance = std::numeric_limits<double>::max();
-    contact_count = 0;
-    contacts.clear();
-    cost_sources.clear();
-  }
-
-  /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
-  void print() const;
-
-  /** \brief True if collision was found, false otherwise */
-  bool collision = false;
-
-  /** \brief Closest distance between two bodies */
-  double distance = std::numeric_limits<double>::max();
-
-  /** \brief Number of contacts returned */
-  std::size_t contact_count = 0;
-
-  /** \brief A map returning the pairs of body ids in contact, plus their contact details */
-  ContactMap contacts;
-
-  /** \brief These are the individual cost sources when costs are computed */
-  std::set<CostSource> cost_sources;
-};
+struct CollisionResult;
 
 /** \brief Representation of a collision checking request */
 struct CollisionRequest
@@ -187,6 +153,9 @@ struct CollisionRequest
   /** \brief If true, compute proximity distance */
   bool distance = false;
 
+  /** \brief If true, return detailed distance information. Distance must be set to true as well */
+  bool detailed_distance = false;
+
   /** \brief If true, a collision cost is computed */
   bool cost = false;
 
@@ -354,4 +323,43 @@ struct DistanceResult
     distances.clear();
   }
 };
+
+/** \brief Representation of a collision checking result */
+struct CollisionResult
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /** \brief Clear a previously stored result */
+  void clear()
+  {
+    collision = false;
+    distance = std::numeric_limits<double>::max();
+    distance_result.clear();
+    contact_count = 0;
+    contacts.clear();
+    cost_sources.clear();
+  }
+
+  /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
+  void print() const;
+
+  /** \brief True if collision was found, false otherwise */
+  bool collision = false;
+
+  /** \brief Closest distance between two bodies */
+  double distance = std::numeric_limits<double>::max();
+
+  /** \brief Distance data for each link */
+  DistanceResult distance_result;
+
+  /** \brief Number of contacts returned */
+  std::size_t contact_count = 0;
+
+  /** \brief A map returning the pairs of body ids in contact, plus their contact details */
+  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
+  ContactMap contacts;
+
+  /** \brief These are the individual cost sources when costs are computed */
+  std::set<CostSource> cost_sources;
+};
 }  // namespace collision_detection

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -290,6 +290,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll
     dreq.enableGroup(getRobotModel());
     distanceSelf(dreq, dres, state);
     res.distance = dres.minimum_distance.distance;
+    if (req.detailed_distance)
+    {
+      res.distance_result = dres;
+    }
   }
 }
 
@@ -343,6 +347,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col
     dreq.enableGroup(getRobotModel());
     distanceRobot(dreq, dres, state);
     res.distance = dres.minimum_distance.distance;
+    if (req.detailed_distance)
+    {
+      res.distance_result = dres;
+    }
   }
 }