fcl_discrete_managers.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COLLISION_FCL_DISCRETE_MANAGERS_H
#define TESSERACT_COLLISION_FCL_DISCRETE_MANAGERS_H
 
#include <tesseract_collision/core/discrete_contact_manager.h>
#include <tesseract_collision/fcl/fcl_utils.h>
 
namespace tesseract_collision::tesseract_collision_fcl
{
class FCLDiscreteBVHManager : public DiscreteContactManager
{
public:
  using Ptr = std::shared_ptr<FCLDiscreteBVHManager>;
  using ConstPtr = std::shared_ptr<const FCLDiscreteBVHManager>;
  using UPtr = std::unique_ptr<FCLDiscreteBVHManager>;
  using ConstUPtr = std::unique_ptr<const FCLDiscreteBVHManager>;
 
  FCLDiscreteBVHManager(std::string name = "FCLDiscreteBVHManager");
  ~FCLDiscreteBVHManager() override = default;
  FCLDiscreteBVHManager(const FCLDiscreteBVHManager&) = delete;
  FCLDiscreteBVHManager& operator=(const FCLDiscreteBVHManager&) = delete;
  FCLDiscreteBVHManager(FCLDiscreteBVHManager&&) = delete;
  FCLDiscreteBVHManager& operator=(FCLDiscreteBVHManager&&) = delete;
 
  std::string getName() const override final;
 
  DiscreteContactManager::UPtr clone() const override final;
 
  bool addCollisionObject(const std::string& name,
                          const int& mask_id,
                          const CollisionShapesConst& shapes,
                          const tesseract_common::VectorIsometry3d& shape_poses,
                          bool enabled = true) override final;
 
  const CollisionShapesConst& getCollisionObjectGeometries(const std::string& name) const override final;
 
  const tesseract_common::VectorIsometry3d&
  getCollisionObjectGeometriesTransforms(const std::string& name) const override final;
 
  bool hasCollisionObject(const std::string& name) const override final;
 
  bool removeCollisionObject(const std::string& name) override final;
 
  bool enableCollisionObject(const std::string& name) override final;
 
  bool disableCollisionObject(const std::string& name) override final;
 
  bool isCollisionObjectEnabled(const std::string& name) const override final;
 
  void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override final;
 
  void setCollisionObjectsTransform(const std::vector<std::string>& names,
                                    const tesseract_common::VectorIsometry3d& poses) override final;
 
  void setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) override final;
 
  const std::vector<std::string>& getCollisionObjects() const override final;
 
  void setActiveCollisionObjects(const std::vector<std::string>& names) override final;
 
  const std::vector<std::string>& getActiveCollisionObjects() const override final;
 
  void setCollisionMarginData(CollisionMarginData collision_margin_data) override final;
 
  const CollisionMarginData& getCollisionMarginData() const override final;
 
  void setCollisionMarginPairData(
      const CollisionMarginPairData& pair_margin_data,
      CollisionMarginPairOverrideType override_type = CollisionMarginPairOverrideType::REPLACE) override final;
 
  void setDefaultCollisionMargin(double default_collision_margin) override final;
 
  void setCollisionMarginPair(const std::string& name1,
                              const std::string& name2,
                              double collision_margin) override final;
 
  void incrementCollisionMargin(double increment) override final;
 
  void
  setContactAllowedValidator(std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator) override final;
 
  std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
 
  void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
 
  void addCollisionObject(const COW::Ptr& cow);
 
private:
  std::string name_;
 
  std::unique_ptr<fcl::BroadPhaseCollisionManagerd> static_manager_;
 
  std::unique_ptr<fcl::BroadPhaseCollisionManagerd> dynamic_manager_;
 
  Link2COW link2cow_;               
  std::vector<std::string> active_; 
  std::vector<std::string> collision_objects_; 
  CollisionMarginData collision_margin_data_;  
  std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator_; 
  std::size_t fcl_co_count_{ 0 }; 
  std::vector<CollisionObjectRawPtr> static_update_;
 
  std::vector<CollisionObjectRawPtr> dynamic_update_;
 
  void onCollisionMarginDataChanged();
};
 
}  // namespace tesseract_collision::tesseract_collision_fcl
#endif  // TESSERACT_COLLISION_FCL_DISCRETE_MANAGERS_H