utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COLLISION_CORE_UTILS_H
#define TESSERACT_COLLISION_CORE_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <tesseract_collision/core/types.h>
#include <tesseract_collision/core/continuous_contact_manager.h>
#include <tesseract_collision/core/discrete_contact_manager.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
namespace tesseract_collision
{
IsContactAllowedFn combineContactAllowedFn(const IsContactAllowedFn& original,
                                           const IsContactAllowedFn& override,
                                           ACMOverrideType type = ACMOverrideType::OR);
 
template <typename ManagerType>
inline void applyIsContactAllowedFnOverride(ManagerType& manager,
                                            const tesseract_common::AllowedCollisionMatrix& acm,
                                            ACMOverrideType type)
{
  IsContactAllowedFn original = manager.getIsContactAllowedFn();
  IsContactAllowedFn override = [acm](const std::string& str1, const std::string& str2) {
    return acm.isCollisionAllowed(str1, str2);
  };
  manager.setIsContactAllowedFn(combineContactAllowedFn(original, override, type));
}
 
template <typename ManagerType>
inline void applyModifyObjectEnabled(ManagerType& manager,
                                     const std::unordered_map<std::string, bool>& modify_object_enabled)
{
  for (const auto& entry : modify_object_enabled)
  {
    if (entry.second)
      manager.enableCollisionObject(entry.first);
    else
      manager.disableCollisionObject(entry.first);
  }
}
}  // namespace tesseract_collision
 
#endif  // TESSERACT_COLLISION_TYPES_H