PR Review changes:

  * using MoveIt macros for typedefs
  * removing namespaces
  * general cleanup
  * removed auto in loops
  * adaption of reported continuous results
  * removed parent classes of collision managers
  * removed typedef for ObjectKeyPair
  * changed member name in CastHullShape
  * removing unnecessary asserts
  * premature end of collision checking after max_number_contacts per pair added
  * removed mutable of managers and use clone instead for collision checking
  * managers as pointer members of world and robot
  * improving documentation of tesseract code
  * panda home position in test defined
  * renamed enums for clarity
  * random number package used
  * documentation updated
  * removed empty collision common file

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

@@ -52,10 +52,10 @@ MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);
 
 enum class ContinuousCollisionType
 {
-  CCType_None,
-  CCType_Time0,
-  CCType_Time1,
-  CCType_Between
+  None,
+  Time0,
+  Time1,
+  Between
 };
 
 /** \brief The types of bodies that are considered for collision */
@@ -104,14 +104,11 @@ struct Contact
   /** \brief The type of the second body involved in the contact */
   BodyType body_type_2;
 
-  /** \brief The time until collision */
-  double cc_time;
+  /** \brief The distance percentage between casted poses until collision */
+  double percent_interpolation;
 
   /** \brief The continous collision type */
   ContinuousCollisionType cc_type;
-
-  Eigen::Vector3d nearest_points[2];
-  Eigen::Vector3d cc_nearest_points[2];
 };
 
 /** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a

moveit_core/collision_detection_bullet/CMakeLists.txt

@@ -1,7 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_collision_detection_bullet)
 
 add_library(${MOVEIT_LIB_NAME}
-  src/collision_common.cpp
   src/collision_robot_bt.cpp
   src/collision_world_bt.cpp
   src/tesseract/bullet_utils.cpp

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h

@@ -37,10 +37,8 @@
 #ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_
 #define MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_
 
-#include <moveit/collision_detection_bullet/collision_common.h>
 #include <moveit/collision_detection_bullet/tesseract/bullet_discrete_bvh_manager.h>
-#include <moveit/collision_detection_bullet/tesseract/bullet_cast_bvh_manager.h>
-#include <urdf/model.h>
+#include <moveit/collision_detection/collision_robot.h>
 
 namespace collision_detection
 {
@@ -84,33 +82,36 @@ class CollisionRobotBt : public CollisionRobot
                      const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override;
 
 protected:
-  /** \brief Updates the poses of the objects in the manager according to given robot state. */
-  void updateTransformsFromState(const robot_state::RobotState& state) const;
-  void updateTransformsFromStateCCD(const robot_state::RobotState& state1, const robot_state::RobotState& state2) const;
+  /** \brief Updates the poses of the objects in the manager according to given robot state */
+  void updateTransformsFromState(const robot_state::RobotState& state,
+                                 tesseract::BulletDiscreteBVHManagerPtr manager) const;
 
+  /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links
+   */
   void updatedPaddingOrScaling(const std::vector<std::string>& links) override;
 
-  void addAttachedOjects(const robot_state::RobotState& state,
-                         std::vector<tesseract::tesseract_bullet::COWPtr>& cows) const;
+  /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */
+  void addAttachedOjects(const robot_state::RobotState& state, std::vector<tesseract::COWPtr>& cows) const;
 
+  /** \brief Bundles the different checkSelfCollision functions into a single function */
   void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
                                 const AllowedCollisionMatrix* acm) const;
 
+  /** \brief Bundles the different continuous checkSelfCollision functions into a single function */
   void checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res,
                                    const robot_state::RobotState& state1, const robot_state::RobotState& state2,
                                    const AllowedCollisionMatrix* acm) const;
 
+  /** \brief Bundles the different checkOtherCollision functions into a single function */
   void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res,
                                  const robot_state::RobotState& state, const CollisionRobot& other_robot,
                                  const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const;
 
+  /** \brief Construts a bullet collision object out of a robot link */
   void addLinkAsCOW(const urdf::LinkSharedPtr link);
 
-  /** @brief Bullet collision manager taken from tesseract*/
-  mutable tesseract::tesseract_bullet::BulletDiscreteBVHManager bt_manager_;
-
-  tesseract::tesseract_bullet::Link2Cow link2cow_;
-  tesseract::tesseract_bullet::Link2Cow link2cow_CCD_;
+  /** \brief Handles all self collision checks */
+  tesseract::BulletDiscreteBVHManagerPtr bt_manager_;
 };
 }
 

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h

@@ -38,8 +38,9 @@
 #define MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_
 
 #include <moveit/collision_detection_bullet/collision_robot_bt.h>
+#include <moveit/collision_detection/collision_world.h>
 #include <moveit/collision_detection_bullet/tesseract/bullet_discrete_bvh_manager.h>
-
+#include <moveit/collision_detection_bullet/tesseract/bullet_cast_bvh_manager.h>
 #include <memory>
 
 namespace collision_detection
@@ -48,8 +49,11 @@ class CollisionWorldBt : public CollisionWorld
 {
 public:
   CollisionWorldBt();
+
   explicit CollisionWorldBt(const WorldPtr& world);
+
   CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world);
+
   ~CollisionWorldBt() override;
 
   void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
@@ -74,24 +78,39 @@ class CollisionWorldBt : public CollisionWorld
   void setWorld(const WorldPtr& world) override;
 
 protected:
+  /** \brief Bundles the different checkWorldCollision functions into a single function */
   void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
                                  const AllowedCollisionMatrix* acm) const;
+
+  /** \brief Bundles the different checkRobotCollision functions into a single function */
   void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
                                  const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
 
+  /** \brief Bundles the different continuous checkRobotCollision functions into a single function */
   void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
                                     const robot_state::RobotState& state1, const robot_state::RobotState& state2,
                                     const AllowedCollisionMatrix* acm) const;
 
-  void addToManager(const World::Object* obj) const;
+  /** \brief Adds a world object to the collision managers */
+  void addToManager(const World::Object* obj);
+
+  /** \brief Updates a managed collision object with its world representation.
+   *
+   * We have three cases: 1) the object is part of the manager and not of world --> delete it
+   *                      2) the object is not in the manager, therefore register to manager,
+   *                      3) the object is in the manager then delete and add the modified */
   void updateManagedObject(const std::string& id);
 
-  mutable tesseract::tesseract_bullet::BulletDiscreteBVHManager bt_manager_;
-  mutable tesseract::tesseract_bullet::BulletCastBVHManager bt_manager_CCD_;
+  /** \brief Handles all discrete collision checks */
+  tesseract::BulletDiscreteBVHManagerPtr bt_manager_;
+
+  /** \brief Handles all continuous collision checks */
+  tesseract::BulletCastBVHManagerPtr bt_manager_CCD_;
 
 private:
-  void initialize();
+  /** \brief Callback function executed for each change to the world environment */
   void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
+
   World::ObserverHandle observer_handle_;
 };
 }

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

@@ -74,7 +74,14 @@ struct ContactTestData
   ContactTestData(const std::vector<std::string>& active, const double& contact_distance, const IsContactAllowedFn& fn,
                   collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req,
                   const collision_detection::AllowedCollisionMatrix* acm)
-    : active(active), contact_distance(contact_distance), fn(fn), acm(acm), res(res), req(req), done(false)
+    : active(active)
+    , contact_distance(contact_distance)
+    , fn(fn)
+    , acm(acm)
+    , res(res)
+    , req(req)
+    , done(false)
+    , pair_done(false)
   {
   }
 
@@ -93,6 +100,9 @@ struct ContactTestData
 
   /// Indicate if search is finished
   bool done;
+
+  /// Indicate if search between a single pair is finished
+  bool pair_done;
 };
 }