26#ifndef TESSERACT_COMMON_UTILS_H
27#define TESSERACT_COMMON_UTILS_H
48#if __cplusplus < 201703L
50static std::mt19937
mersenne{
static_cast<std::mt19937::result_type
>(std::time(
nullptr)) };
54inline std::mt19937
mersenne{
static_cast<std::mt19937::result_type
>(std::time(
nullptr)) };
57template <
typename... Args>
58std::string
strFormat(
const std::string& format, Args... args)
60 int size_s = std::snprintf(
nullptr, 0, format.c_str(), args...) + 1;
62 throw std::runtime_error(
"Error during formatting.");
64 auto size =
static_cast<size_t>(size_s);
65 auto buf = std::make_unique<char[]>(size);
66 std::snprintf(buf.get(), size, format.c_str(), args...);
67 return std::string{ buf.get(), buf.get() + size - 1 };
75std::string
fileToString(
const tesseract_common::fs::path& filepath);
83void twistChangeRefPoint(Eigen::Ref<Eigen::VectorXd> twist,
const Eigen::Ref<const Eigen::Vector3d>& ref_point);
90void twistChangeBase(Eigen::Ref<Eigen::VectorXd> twist,
const Eigen::Isometry3d& change_base);
108Eigen::VectorXd
concat(
const Eigen::VectorXd& a,
const Eigen::VectorXd& b);
147Eigen::VectorXd
calcTransformError(
const Eigen::Isometry3d& t1,
const Eigen::Isometry3d& t2);
180bool isNumeric(
const std::vector<std::string>& sv);
193void ltrim(std::string&
s);
199void rtrim(std::string&
s);
205void trim(std::string&
s);
218 const std::vector<T>& vec1,
219 const std::vector<T>& vec2,
221 const std::function<
bool(
const T&,
const T&)>& equal_pred = [](
const T&
v1,
const T&
v2) {
return v1 ==
v2; },
222 const std::function<bool(
const T&,
const T&)>& comp = [](
const T&
v1,
const T&
v2) {
return v1 <
v2; })
224 if (vec1.size() != vec2.size())
228 return std::equal(vec1.begin(), vec1.end(), vec2.begin(), equal_pred);
230 std::vector<T>
v1 = vec1;
231 std::vector<T>
v2 = vec2;
232 std::sort(
v1.begin(),
v1.end(), comp);
233 std::sort(
v2.begin(),
v2.end(), comp);
234 return std::equal(
v1.begin(),
v1.end(),
v2.begin(), equal_pred);
243template <
typename KeyValueContainerType,
typename ValueType>
245 const KeyValueContainerType& map_1,
246 const KeyValueContainerType& map_2,
247 const std::function<
bool(
const ValueType&,
const ValueType&)>& value_eq =
248 [](
const ValueType&
v1,
const ValueType&
v2) {
return v1 ==
v2; })
250 if (map_1.size() != map_2.size())
253 for (
const auto& entry : map_1)
256 const auto cp = map_2.find(entry.first);
257 if (cp == map_2.end())
260 if (!value_eq(cp->second, entry.second))
272template <
typename ValueType>
274 const std::set<ValueType>& set_1,
275 const std::set<ValueType>& set_2,
276 const std::function<
bool(
const ValueType&,
const ValueType&)>& value_eq =
277 [](
const ValueType&
v1,
const ValueType&
v2) {
return v1 ==
v2; })
279 if (set_1.size() != set_2.size())
282 for (
const auto& entry : set_1)
285 const auto cp = set_2.find(entry);
286 if (cp == set_2.end())
289 if (!value_eq(*cp, entry))
301template <
typename ValueType, std::
size_t Size>
303 const std::array<ValueType, Size>& array_1,
304 const std::array<ValueType, Size>& array_2,
305 const std::function<
bool(
const ValueType&,
const ValueType&)>& value_eq =
306 [](
const ValueType&
v1,
const ValueType&
v2) {
return v1 ==
v2; })
308 if (array_1.size() != array_2.size())
311 for (std::size_t idx = 0; idx < array_1.size(); idx++)
313 if (!value_eq(array_1[idx], array_2[idx]))
326bool pointersEqual(
const std::shared_ptr<T>& p1,
const std::shared_ptr<T>& p2)
328 return (p1 && p2 && *p1 == *p2) || (!p1 && !p2);
342 return p2 !=
nullptr;
355void reorder(Eigen::Ref<Eigen::VectorXd>
v, std::vector<Eigen::Index> order);
371tinyxml2::XMLError
QueryStringText(
const tinyxml2::XMLElement* xml_element, std::string& text);
398std::string
StringAttribute(
const tinyxml2::XMLElement* xml_element,
const char*
name, std::string default_value);
458 double max_diff = 1e-6,
459 double max_rel_diff = std::numeric_limits<double>::epsilon());
474 const Eigen::Ref<const Eigen::VectorXd>&
v2,
475 double max_diff = 1e-6,
476 double max_rel_diff = std::numeric_limits<double>::epsilon());
491 const Eigen::Ref<const Eigen::VectorXd>&
v2,
492 const Eigen::Ref<const Eigen::VectorXd>& max_diff,
493 const Eigen::Ref<const Eigen::VectorXd>& max_rel_diff);
501template <
typename FloatType>
507 std::stringstream ss;
508 ss.imbue(std::locale::classic());
515 if (ss.fail() || !ss.eof())
531 bool remove_duplicates =
true);
Definition: polygon_mesh.h:46
results link_names[0]
Definition: collision_core_unit.cpp:146
Eigen::MatrixXd jacobian
Definition: kinematics_core_unit.cpp:153
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
Definition: create_convex_hull.cpp:36
Definition: allowed_collision_matrix.h:16
bool toNumeric(const std::string &s, FloatType &value)
Convert a string to a numeric value type.
Definition: utils.h:502
Eigen::Vector4d computeRandomColor()
This computes a random color RGBA [0, 1] with alpha set to 1.
Definition: utils.cpp:178
std::string getTimestampString()
Get Timestamp string.
Definition: utils.cpp:254
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;})
Checks if 2 sets are identical.
Definition: utils.h:273
bool isNumeric(const std::string &s)
Determine if a string is a number.
Definition: utils.cpp:211
tinyxml2::XMLError QueryStringAttribute(const tinyxml2::XMLElement *xml_element, const char *name, std::string &value)
Query a string attribute from an xml element.
Definition: utils.cpp:313
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;})
Check if two vector of strings are identical.
Definition: utils.h:217
Eigen::Vector3d calcRotationalError2(const Eigen::Ref< const Eigen::Matrix3d > &R)
Calculate the rotation error vector given a rotation error matrix where the angle is between [0,...
Definition: utils.cpp:148
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;})
Checks if 2 maps are identical.
Definition: utils.h:244
void twistChangeRefPoint(Eigen::Ref< Eigen::VectorXd > twist, const Eigen::Ref< const Eigen::Vector3d > &ref_point)
Change the reference point of the provided Twist.
Definition: utils.cpp:88
void jacobianChangeRefPoint(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Ref< const Eigen::Vector3d > &ref_point)
Change the reference point of the jacobian.
Definition: utils.cpp:108
bool pointersComparison(const std::shared_ptr< T > &p1, const std::shared_ptr< T > &p2)
Comparison operator for the objects 2 points point to.
Definition: utils.h:337
std::string getTempPath()
Get the host temp directory path.
Definition: utils.cpp:209
tinyxml2::XMLError QueryStringAttributeRequired(const tinyxml2::XMLElement *xml_element, const char *name, std::string &value)
Query a string attribute from an xml element and print error log.
Definition: utils.cpp:329
bool pointersEqual(const std::shared_ptr< T > &p1, const std::shared_ptr< T > &p2)
Checks if 2 pointers point to objects that are ==.
Definition: utils.h:326
Eigen::Vector3d calcRotationalError(const Eigen::Ref< const Eigen::Matrix3d > &R)
Calculate the rotation error vector given a rotation error matrix where the angle is between [-pi,...
Definition: utils.cpp:124
Eigen::VectorXd concat(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Concatenate two vector.
Definition: utils.cpp:116
std::unordered_map< tesseract_common::LinkNamesPair, std::string, tesseract_common::PairHash > AllowedCollisionEntries
Definition: allowed_collision_matrix.h:18
void reorder(Eigen::Ref< Eigen::VectorXd > v, std::vector< Eigen::Index > order)
Reorder Eigen::VectorXd implace given index list.
Definition: utils.cpp:263
std::string strFormat(const std::string &format, Args... args)
Definition: utils.h:58
Eigen::VectorXd calcTransformError(const Eigen::Isometry3d &t1, const Eigen::Isometry3d &t2)
Calculate error between two transforms expressed in t1 coordinate system.
Definition: utils.cpp:172
void twistChangeBase(Eigen::Ref< Eigen::VectorXd > twist, const Eigen::Isometry3d &change_base)
Change the base coordinate system of the Twist.
Definition: utils.cpp:95
void jacobianChangeBase(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base)
Change the base coordinate system of the jacobian.
Definition: utils.cpp:101
Eigen::VectorXd generateRandomNumber(const Eigen::Ref< const Eigen::MatrixX2d > &limits)
Given a set of limits it will generate a vector of random numbers between the limit.
Definition: utils.cpp:232
void rtrim(std::string &s)
Right trim string.
Definition: utils.cpp:246
std::vector< std::string > getAllowedCollisions(const std::vector< std::string > &link_names, const AllowedCollisionEntries &acm_entries, bool remove_duplicates=true)
Gets allowed collisions for a set of link names.
Definition: utils.cpp:440
std::string StringAttribute(const tinyxml2::XMLElement *xml_element, const char *name, std::string default_value)
Get string attribute if exist. If it does not exist it returns the default value.
Definition: utils.cpp:322
tinyxml2::XMLError QueryDoubleAttributeRequired(const tinyxml2::XMLElement *xml_element, const char *name, double &value)
Query a double attribute from an xml element and print error log.
Definition: utils.cpp:351
tinyxml2::XMLError QueryStringText(const tinyxml2::XMLElement *xml_element, std::string &text)
Query a string Text from xml element.
Definition: utils.cpp:293
void printNestedException(const std::exception &e, int level=0)
Print a nested exception.
Definition: utils.cpp:193
bool almostEqualRelativeAndAbs(double a, double b, double max_diff=1e-6, double max_rel_diff=std::numeric_limits< double >::epsilon())
Check if two double are relatively equal.
Definition: utils.cpp:393
std::string fileToString(const tesseract_common::fs::path &filepath)
Read in the contents of the file into a string.
Definition: utils.cpp:72
tinyxml2::XMLError QueryIntAttributeRequired(const tinyxml2::XMLElement *xml_element, const char *name, int &value)
Query a int attribute from an xml element and print error log.
Definition: utils.cpp:373
tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLElement *xml_element, std::string &value)
Query a string value from xml element.
Definition: utils.cpp:283
void trim(std::string &s)
Trim left and right of string.
Definition: utils.cpp:248
static std::mt19937 mersenne
Random number generator.
Definition: utils.h:50
void ltrim(std::string &s)
Left trim string.
Definition: utils.cpp:244
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;})
Checks if 2 arrays are identical.
Definition: utils.h:302
std::set< std::string > s
Definition: plugin_loader_unit.cpp:45
object value
Definition: tesseract_common_serialization_unit.cpp:495
std::vector< std::string > v2
Definition: tesseract_common_unit.cpp:418
v1["1"]
Definition: tesseract_common_unit.cpp:434
v
Definition: tesseract_common_unit.cpp:369
joint_1 limits
Definition: tesseract_scene_graph_joint_unit.cpp:153
m name
Definition: tesseract_scene_graph_link_unit.cpp:77