Adding documentation to the collision checking (moveit#1505)

* Adding docu to CollisionWorldFCL

* Adding documentation to collision detection

* Rephrasing to match collision_robot_fcl comments

* minor polishing

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

@@ -162,11 +162,10 @@ struct CollisionResult
   /** \brief Number of contacts returned */
   std::size_t contact_count;
 
-  /** \brief A map returning the pairs of ids of the bodies in contact, plus information about the contacts themselves
-   */
+  /** \brief A map returning the pairs of body ids in contact, plus their contact details */
   ContactMap contacts;
 
-  /** \brief When costs are computed, the individual cost sources are  */
+  /** \brief These are the individual cost sources when costs are computed */
   std::set<CostSource> cost_sources;
 };
 
@@ -197,7 +196,7 @@ struct CollisionRequest
   /** \brief If true, a collision cost is computed */
   bool cost;
 
-  /** \brief If true, compute contacts */
+  /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */
   bool contacts;
 
   /** \brief Overall maximum number of contacts to compute */
@@ -224,14 +223,15 @@ namespace DistanceRequestTypes
 {
 enum DistanceRequestType
 {
-  GLOBAL,   /// Find the global minimum
-  SINGLE,   /// Find the global minimum for each pair
-  LIMITED,  /// Find a limited(max_contacts_per_body) set of contacts for a given pair
-  ALL       /// Find all the contacts for a given pair
+  GLOBAL,   ///< Find the global minimum
+  SINGLE,   ///< Find the global minimum for each pair
+  LIMITED,  ///< Find a limited(max_contacts_per_body) set of contacts for a given pair
+  ALL       ///< Find all the contacts for a given pair
 };
 }
 typedef DistanceRequestTypes::DistanceRequestType DistanceRequestType;
 
+/** \brief Representation of a distance-reporting request */
 struct DistanceRequest
 {
   DistanceRequest()
@@ -264,7 +264,7 @@ struct DistanceRequest
 
   /// Indicate the type of distance request. If using type=ALL, it is
   /// recommended to set max_contacts_per_body to the expected number
-  /// of contacts per pair becaused it is uesed to reserving space.
+  /// of contacts per pair because it is used to reserve space.
   DistanceRequestType type;
 
   /// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
@@ -280,7 +280,7 @@ struct DistanceRequest
   const AllowedCollisionMatrix* acm;
 
   /// Only calculate distances for objects within this threshold to each other.
-  /// If set this can significantly to reduce number of queries.
+  /// If set this can significantly reduce the number of queries.
   double distance_threshold;
 
   /// Log debug information
@@ -291,6 +291,7 @@ struct DistanceRequest
   bool compute_gradient;
 };
 
+/** \brief Generic representation of the distance information for a pair of objects */
 struct DistanceResultsData
 {
   DistanceResultsData()
@@ -356,8 +357,10 @@ struct DistanceResultsData
   }
 };
 
+/** \brief Mapping between the names of the collision objects and the DistanceResultData. */
 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> > DistanceMap;
 
+/** \brief Result of a distance request. */
 struct DistanceResult
 {
   DistanceResult() : collision(false)

moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h

@@ -101,7 +101,7 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
     return CollisionRobotPtr(new CollisionRobotType(dynamic_cast<const CollisionRobotType&>(*orig)));
   }
 
-  /** Create an allocator for FCL collision detectors */
+  /** Create an allocator for collision detectors. */
   static CollisionDetectorAllocatorPtr create()
   {
     return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType());

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -288,7 +288,7 @@ class World
   /** The objects maintained in the world */
   std::map<std::string, ObjectPtr> objects_;
 
-  /* observers to call when something changes */
+  /** Wrapper for a callback function to call when something changes in the world */
   class Observer
   {
   public:
@@ -297,6 +297,8 @@ class World
     }
     ObserverCallbackFn callback_;
   };
+
+  /// All registered observers of this world representation
   std::vector<Observer*> observers_;
 };
 }

moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h

@@ -80,20 +80,31 @@ class CollisionWorldFCL : public CollisionWorld
   void setWorld(const WorldPtr& world) override;
 
 protected:
+  /** \brief Common helper used by the different checkWorldCollision functions. */
   void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
                                  const AllowedCollisionMatrix* acm) const;
+
+  /** \brief Common helper used by the different checkRobotCollision functions. */
   void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
                                  const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
 
+  /** \brief Construct an FCL collision object from MoveIt's World::Object. */
   void constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const;
+
+  /** \brief Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
+   *
+   *  If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there. */
   void updateFCLObject(const std::string& id);
 
+  /// FCL collision manager which handles the collision checking process
   std::unique_ptr<fcl::BroadPhaseCollisionManagerd> manager_;
+
   std::map<std::string, FCLObject> fcl_objs_;
 
 private:
-  void initialize();
+  /// Callback function that is called on any change to the world
   void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
+
   World::ObserverHandle observer_handle_;
 };
 }