collision_margin_data.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COMMON_COLLISION_MARGIN_DATA_H
#define TESSERACT_COMMON_COLLISION_MARGIN_DATA_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/export.hpp>
#include <Eigen/Core>
#include <string>
#include <unordered_map>
#include <optional>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/types.h>
#include <tesseract_common/utils.h>
 
namespace boost::serialization
{
class access;
}
 
namespace tesseract_common
{
enum class CollisionMarginPairOverrideType : std::uint8_t
{
  NONE,
  REPLACE,
  MODIFY
};
 
using PairsCollisionMarginData =
    std::unordered_map<tesseract_common::LinkNamesPair, double, tesseract_common::PairHash>;
 
class CollisionMarginPairData
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  CollisionMarginPairData() = default;
  CollisionMarginPairData(const PairsCollisionMarginData& pair_margins);
 
  void setCollisionMargin(const std::string& obj1, const std::string& obj2, double margin);
 
  std::optional<double> getCollisionMargin(const std::string& obj1, const std::string& obj2) const;
 
  double getMaxCollisionMargin() const;
 
  const PairsCollisionMarginData& getCollisionMargins() const;
 
  void incrementMargins(double increment);
 
  void scaleMargins(double scale);
 
  bool empty() const;
 
  void clear();
 
  void apply(const CollisionMarginPairData& pair_margin_data, CollisionMarginPairOverrideType override_type);
 
  bool operator==(const CollisionMarginPairData& rhs) const;
  bool operator!=(const CollisionMarginPairData& rhs) const;
 
private:
  PairsCollisionMarginData lookup_table_;
 
  double max_collision_margin_{ std::numeric_limits<double>::lowest() };
 
  void updateCollisionMarginMax();
 
  friend class boost::serialization::access;
  friend struct tesseract_common::Serialization;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
};
 
class CollisionMarginData
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  using Ptr = std::shared_ptr<CollisionMarginData>;
  using ConstPtr = std::shared_ptr<const CollisionMarginData>;
 
  CollisionMarginData(double default_collision_margin = 0);
  CollisionMarginData(double default_collision_margin, CollisionMarginPairData pair_collision_margins);
  CollisionMarginData(CollisionMarginPairData pair_collision_margins);
 
  void setDefaultCollisionMargin(double default_collision_margin);
 
  double getDefaultCollisionMargin() const;
 
  void setCollisionMargin(const std::string& obj1, const std::string& obj2, double collision_margin);
 
  double getCollisionMargin(const std::string& obj1, const std::string& obj2) const;
 
  const CollisionMarginPairData& getCollisionMarginPairData() const;
 
  double getMaxCollisionMargin() const;
 
  void incrementMargins(double increment);
 
  void scaleMargins(double scale);
 
  void apply(const CollisionMarginPairData& pair_margin_data, CollisionMarginPairOverrideType override_type);
 
  bool operator==(const CollisionMarginData& rhs) const;
  bool operator!=(const CollisionMarginData& rhs) const;
 
private:
  double default_collision_margin_{ 0 };
 
  CollisionMarginPairData pair_margins_;
 
  friend class boost::serialization::access;
  friend struct tesseract_common::Serialization;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
};
}  // namespace tesseract_common
 
BOOST_CLASS_EXPORT_KEY(tesseract_common::CollisionMarginPairData)
BOOST_CLASS_EXPORT_KEY(tesseract_common::CollisionMarginData)
#endif  // TESSERACT_COMMON_COLLISION_MARGIN_DATA_H