utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COMMON_UTILS_H
#define TESSERACT_COMMON_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <array>
#include <vector>
#include <set>
#include <string>
#include <sstream>
#include <stdexcept>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/allowed_collision_matrix.h>
#include <filesystem>
 
namespace tinyxml2
{
class XMLElement;  // NOLINT
class XMLAttribute;
}  // namespace tinyxml2
 
namespace tesseract_common
{
#if __cplusplus < 201703L
 
static std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
#else
 
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
inline std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
#endif
 
template <typename... Args>
std::string strFormat(const std::string& format, Args... args)
{
  int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + 1;  // Extra space for '\0'
  if (size_s <= 0)
    throw std::runtime_error("Error during formatting.");
 
  auto size = static_cast<size_t>(size_s);
  auto buf = std::make_unique<char[]>(size);  // NOLINT
  std::snprintf(buf.get(), size, format.c_str(), args...);
  return std::string{ buf.get(), buf.get() + size - 1 };  // We don't want the '\0' inside
}
 
std::string fileToString(const std::filesystem::path& filepath);
 
void twistChangeRefPoint(Eigen::Ref<Eigen::VectorXd> twist, const Eigen::Ref<const Eigen::Vector3d>& ref_point);
 
void twistChangeBase(Eigen::Ref<Eigen::VectorXd> twist, const Eigen::Isometry3d& change_base);
 
void jacobianChangeBase(Eigen::Ref<Eigen::MatrixXd> jacobian, const Eigen::Isometry3d& change_base);
 
void jacobianChangeRefPoint(Eigen::Ref<Eigen::MatrixXd> jacobian, const Eigen::Ref<const Eigen::Vector3d>& ref_point);
 
Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b);
 
Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R);
 
Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);
 
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
                                               const Eigen::Isometry3d& source,
                                               const Eigen::Isometry3d& source_perturbed);
 
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
                                               const Eigen::Isometry3d& target_perturbed,
                                               const Eigen::Isometry3d& source,
                                               const Eigen::Isometry3d& source_perturbed);
 
Eigen::Vector4d computeRandomColor();
 
void printNestedException(const std::exception& e, int level = 0);
 
std::string getTempPath();
 
bool isNumeric(const std::string& s);
 
bool isNumeric(const std::vector<std::string>& sv);
 
Eigen::VectorXd generateRandomNumber(const Eigen::Ref<const Eigen::MatrixX2d>& limits);
 
void ltrim(std::string& s);
 
void rtrim(std::string& s);
 
void trim(std::string& s);
 
template <typename T>
bool isIdentical(
    const std::vector<T>& vec1,
    const std::vector<T>& vec2,
    bool ordered = true,
    const std::function<bool(const T&, const T&)>& equal_pred = [](const T& v1, const T& v2) { return v1 == v2; },
    const std::function<bool(const T&, const T&)>& comp = [](const T& v1, const T& v2) { return v1 < v2; })
{
  if (vec1.size() != vec2.size())
    return false;
 
  if (ordered)
    return std::equal(vec1.begin(), vec1.end(), vec2.begin(), equal_pred);
 
  std::vector<T> v1 = vec1;
  std::vector<T> v2 = vec2;
  std::sort(v1.begin(), v1.end(), comp);
  std::sort(v2.begin(), v2.end(), comp);
  return std::equal(v1.begin(), v1.end(), v2.begin(), equal_pred);
}
 
template <typename KeyValueContainerType, typename ValueType>
bool isIdenticalMap(
    const KeyValueContainerType& map_1,
    const KeyValueContainerType& map_2,
    const std::function<bool(const ValueType&, const ValueType&)>& value_eq =
        [](const ValueType& v1, const ValueType& v2) { return v1 == v2; })
{
  if (map_1.size() != map_2.size())
    return false;
 
  for (const auto& entry : map_1)
  {
    // Check if the key exists
    const auto cp = map_2.find(entry.first);
    if (cp == map_2.end())
      return false;
    // Check if the value is the same
    if (!value_eq(cp->second, entry.second))
      return false;
  }
  return true;
}
 
template <typename ValueType>
bool isIdenticalSet(
    const std::set<ValueType>& set_1,
    const std::set<ValueType>& set_2,
    const std::function<bool(const ValueType&, const ValueType&)>& value_eq =
        [](const ValueType& v1, const ValueType& v2) { return v1 == v2; })
{
  if (set_1.size() != set_2.size())
    return false;
 
  for (const auto& entry : set_1)
  {
    // Check if the key exists
    const auto cp = set_2.find(entry);
    if (cp == set_2.end())
      return false;
    // Check if the value is the same
    if (!value_eq(*cp, entry))
      return false;
  }
  return true;
}
 
template <typename ValueType, std::size_t Size>
bool isIdenticalArray(
    const std::array<ValueType, Size>& array_1,
    const std::array<ValueType, Size>& array_2,
    const std::function<bool(const ValueType&, const ValueType&)>& value_eq =
        [](const ValueType& v1, const ValueType& v2) { return v1 == v2; })
{
  if (array_1.size() != array_2.size())
    return false;
 
  for (std::size_t idx = 0; idx < array_1.size(); idx++)
  {
    if (!value_eq(array_1[idx], array_2[idx]))
      return false;
  }
  return true;
}
 
template <typename T>
bool pointersEqual(const std::shared_ptr<T>& p1, const std::shared_ptr<T>& p2)
{
  return (p1 && p2 && *p1 == *p2) || (!p1 && !p2);
}
template <typename T>
bool pointersComparison(const std::shared_ptr<T>& p1, const std::shared_ptr<T>& p2)
{
  if (p1 && p2)
    return *p1 < *p2;
  // If p2 is !nullptr then return true. p1 or both are nullptr should be false
  return p2 != nullptr;
}
std::string getTimestampString();
 
void reorder(Eigen::Ref<Eigen::VectorXd> v, std::vector<Eigen::Index> order);
 
int QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value);
 
int QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text);
 
int QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value);
 
int QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value);
 
std::string StringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string default_value);
 
int QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value);
 
int QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, double& value);
 
int QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value);
 
bool almostEqualRelativeAndAbs(double a,
                               double b,
                               double max_diff = 1e-6,
                               double max_rel_diff = std::numeric_limits<double>::epsilon());
 
bool almostEqualRelativeAndAbs(const Eigen::Ref<const Eigen::VectorXd>& v1,
                               const Eigen::Ref<const Eigen::VectorXd>& v2,
                               double max_diff = 1e-6,
                               double max_rel_diff = std::numeric_limits<double>::epsilon());
 
bool almostEqualRelativeAndAbs(const Eigen::Ref<const Eigen::VectorXd>& v1,
                               const Eigen::Ref<const Eigen::VectorXd>& v2,
                               const Eigen::Ref<const Eigen::VectorXd>& max_diff,
                               const Eigen::Ref<const Eigen::VectorXd>& max_rel_diff);
 
template <typename FloatType>
bool toNumeric(const std::string& s, FloatType& value)
{
  if (s.empty())
    return false;
 
  std::stringstream ss;
  ss.imbue(std::locale::classic());
 
  ss << s;
 
  FloatType out;
  ss >> out;
 
  if (ss.fail() || !ss.eof())
    return false;
 
  value = out;
  return true;
}
 
std::vector<std::string> getAllowedCollisions(const std::vector<std::string>& link_names,
                                              const AllowedCollisionEntries& acm_entries,
                                              bool remove_duplicates = true);
 
}  // namespace tesseract_common
#endif  // TESSERACT_COMMON_UTILS_H