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/access.hpp>
#include <Eigen/Core>
#include <string>
#include <unordered_map>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/types.h>
#include <tesseract_common/utils.h>
 
namespace tesseract_common
{
enum class CollisionMarginOverrideType
{
  NONE,
  REPLACE,
  MODIFY,
  OVERRIDE_DEFAULT_MARGIN,
  OVERRIDE_PAIR_MARGIN,
  MODIFY_PAIR_MARGIN
};
 
using PairsCollisionMarginData =
    std::unordered_map<tesseract_common::LinkNamesPair, double, tesseract_common::PairHash>;
 
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)
    : default_collision_margin_(default_collision_margin), max_collision_margin_(default_collision_margin)
  {
  }
 
  CollisionMarginData(double default_collision_margin, PairsCollisionMarginData pair_collision_margins)
    : default_collision_margin_(default_collision_margin), lookup_table_(std::move(pair_collision_margins))
  {
    updateMaxCollisionMargin();
  }
 
  CollisionMarginData(PairsCollisionMarginData pair_collision_margins)
    : lookup_table_(std::move(pair_collision_margins))
  {
    updateMaxCollisionMargin();
  }
 
  void setDefaultCollisionMargin(double default_collision_margin)
  {
    default_collision_margin_ = default_collision_margin;
    updateMaxCollisionMargin();
  }
 
  double getDefaultCollisionMargin() const { return default_collision_margin_; }
 
  void setPairCollisionMargin(const std::string& obj1, const std::string& obj2, double collision_margin)
  {
    auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2);
    lookup_table_[key] = collision_margin;
    updateMaxCollisionMargin();
  }
 
  double getPairCollisionMargin(const std::string& obj1, const std::string& obj2) const
  {
    thread_local LinkNamesPair key;
    tesseract_common::makeOrderedLinkPair(key, obj1, obj2);
    const auto it = lookup_table_.find(key);
 
    if (it != lookup_table_.end())
      return it->second;
 
    return default_collision_margin_;
  }
 
  const PairsCollisionMarginData& getPairCollisionMargins() const { return lookup_table_; }
 
  double getMaxCollisionMargin() const { return max_collision_margin_; }
 
  void incrementMargins(const double& increment)
  {
    default_collision_margin_ += increment;
    max_collision_margin_ += increment;
    for (auto& pair : lookup_table_)
      pair.second += increment;
  }
 
  void scaleMargins(const double& scale)
  {
    default_collision_margin_ *= scale;
    max_collision_margin_ *= scale;
    for (auto& pair : lookup_table_)
      pair.second *= scale;
  }
 
  void apply(const CollisionMarginData& collision_margin_data, CollisionMarginOverrideType override_type)
  {
    switch (override_type)
    {
      case CollisionMarginOverrideType::REPLACE:
      {
        *this = collision_margin_data;
        break;
      }
      case CollisionMarginOverrideType::MODIFY:
      {
        default_collision_margin_ = collision_margin_data.default_collision_margin_;
 
        for (const auto& p : collision_margin_data.lookup_table_)
          lookup_table_[p.first] = p.second;
 
        updateMaxCollisionMargin();
        break;
      }
      case CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN:
      {
        default_collision_margin_ = collision_margin_data.default_collision_margin_;
        updateMaxCollisionMargin();
        break;
      }
      case CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN:
      {
        lookup_table_ = collision_margin_data.lookup_table_;
        updateMaxCollisionMargin();
        break;
      }
      case CollisionMarginOverrideType::MODIFY_PAIR_MARGIN:
      {
        for (const auto& p : collision_margin_data.lookup_table_)
          lookup_table_[p.first] = p.second;
 
        updateMaxCollisionMargin();
        break;
      }
      case CollisionMarginOverrideType::NONE:
      {
        break;
      }
    }
  }
 
  bool operator==(const CollisionMarginData& rhs) const;
  bool operator!=(const CollisionMarginData& rhs) const;
 
private:
  double default_collision_margin_{ 0 };
 
  double max_collision_margin_{ 0 };
 
  PairsCollisionMarginData lookup_table_;
 
  void updateMaxCollisionMargin()
  {
    max_collision_margin_ = default_collision_margin_;
    for (const auto& p : lookup_table_)
    {
      if (p.second > max_collision_margin_)
        max_collision_margin_ = p.second;
    }
  }
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
};
}  // namespace tesseract_common
#include <boost/serialization/export.hpp>
#include <boost/serialization/tracking.hpp>
BOOST_CLASS_EXPORT_KEY2(tesseract_common::CollisionMarginData, "CollisionMarginData")
 
#endif  // TESSERACT_COMMON_COLLISION_MARGIN_DATA_H