utils.cpp
Go to the documentation of this file.
1 
27 #include <tesseract_common/macros.h>
28 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
29 #include <ctime>
30 #include <string>
31 #include <type_traits>
32 #include <console_bridge/console.h>
33 #include <fstream>
34 #include <iostream>
35 #include <iomanip>
36 #include <tinyxml2.h>
37 TESSERACT_COMMON_IGNORE_WARNINGS_POP
38 
39 #include <tesseract_common/utils.h>
40 
41 namespace tesseract_common
42 {
43 // explicit Instantiation
44 template bool toNumeric<double>(const std::string&, double&);
45 template bool toNumeric<float>(const std::string&, float&);
46 template bool toNumeric<int>(const std::string&, int&);
47 template bool toNumeric<long>(const std::string&, long&);
48 template bool isIdentical<std::string>(const std::vector<std::string>&,
49  const std::vector<std::string>&,
50  bool,
51  const std::function<bool(const std::string&, const std::string&)>&,
52  const std::function<bool(const std::string&, const std::string&)>&);
53 template bool isIdentical<Eigen::Index>(const std::vector<Eigen::Index>&,
54  const std::vector<Eigen::Index>&,
55  bool,
56  const std::function<bool(const Eigen::Index&, const Eigen::Index&)>&,
57  const std::function<bool(const Eigen::Index&, const Eigen::Index&)>&);
58 
59 // Similar to rethrow_if_nested
60 // but does nothing instead of calling std::terminate
61 // when std::nested_exception is nullptr.
62 template <typename E>
63 std::enable_if_t<!std::is_polymorphic<E>::value> my_rethrow_if_nested(const E&)
64 {
65 }
66 
67 template <typename E>
68 std::enable_if_t<std::is_polymorphic<E>::value> my_rethrow_if_nested(const E& e)
69 {
70  const auto* p = dynamic_cast<const std::nested_exception*>(std::addressof(e));
71  if (p && p->nested_ptr())
72  p->rethrow_nested();
73 }
74 
75 std::string fileToString(const std::filesystem::path& filepath)
76 {
77  std::ifstream ifs(filepath.c_str());
78  std::string contents;
79 
80  ifs.seekg(0, std::ios::end);
81  contents.reserve(ifs.tellg());
82  ifs.seekg(0, std::ios::beg);
83 
84  contents.assign(std::istreambuf_iterator<char>(ifs), std::istreambuf_iterator<char>());
85  ifs.close();
86  return contents;
87 }
88 
89 // LCOV_EXCL_START
90 // These are tested in both tesseract_kinematics and tesseract_state_solver
91 void twistChangeRefPoint(Eigen::Ref<Eigen::VectorXd> twist, const Eigen::Ref<const Eigen::Vector3d>& ref_point)
92 {
93  twist(0) += twist(4) * ref_point(2) - twist(5) * ref_point(1);
94  twist(1) += twist(5) * ref_point(0) - twist(3) * ref_point(2);
95  twist(2) += twist(3) * ref_point(1) - twist(4) * ref_point(0);
96 }
97 
98 void twistChangeBase(Eigen::Ref<Eigen::VectorXd> twist, const Eigen::Isometry3d& change_base)
99 {
100  twist.head(3) = change_base.linear() * twist.head(3);
101  twist.tail(3) = change_base.linear() * twist.tail(3);
102 }
103 
104 void jacobianChangeBase(Eigen::Ref<Eigen::MatrixXd> jacobian, const Eigen::Isometry3d& change_base)
105 {
106  assert(jacobian.rows() == 6);
107  for (int i = 0; i < jacobian.cols(); i++)
108  twistChangeBase(jacobian.col(i), change_base);
109 }
110 
111 void jacobianChangeRefPoint(Eigen::Ref<Eigen::MatrixXd> jacobian, const Eigen::Ref<const Eigen::Vector3d>& ref_point)
112 {
113  assert(jacobian.rows() == 6);
114  for (int i = 0; i < jacobian.cols(); i++)
115  twistChangeRefPoint(jacobian.col(i), ref_point);
116 }
117 // LCOV_EXCL_STOP
118 
119 Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b)
120 {
121  Eigen::VectorXd out(a.size() + b.size());
122  out.topRows(a.size()) = a;
123  out.middleRows(a.size(), b.size()) = b;
124  return out;
125 }
126 
127 std::pair<Eigen::Vector3d, double> calcRotationalErrorDecomposed(const Eigen::Ref<const Eigen::Matrix3d>& R)
128 {
129  Eigen::Quaterniond q(R);
130  Eigen::AngleAxisd r12(q);
131 
132  // Eigen angle axis flips the sign of axis so rotation is always positive which is
133  // not ideal for numerical differentiation.
134  int s = (q.vec().dot(r12.axis()) < 0) ? -1 : 1;
135 
136  // Make sure that the angle is on [-pi, pi]
137  const static double two_pi = 2.0 * M_PI;
138  double angle = s * r12.angle();
139  Eigen::Vector3d axis{ s * r12.axis() };
140  angle = copysign(fmod(fabs(angle), two_pi), angle);
141  if (angle < -M_PI)
142  angle += two_pi;
143  else if (angle > M_PI)
144  angle -= two_pi;
145 
146  assert(std::abs(angle) <= M_PI);
147 
148  return { axis, angle };
149 }
150 
151 Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
152 {
153  std::pair<Eigen::Vector3d, double> data = calcRotationalErrorDecomposed(R);
154  return data.first * data.second;
155 }
156 
157 Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2)
158 {
159  Eigen::Isometry3d pose_err = t1.inverse() * t2;
160  return concat(pose_err.translation(), calcRotationalError(pose_err.rotation()));
161 }
162 
163 Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
164  const Eigen::Isometry3d& source,
165  const Eigen::Isometry3d& source_perturbed)
166 {
167  Eigen::Isometry3d pose_err = target.inverse() * source;
168  std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
169  Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);
170 
171  Eigen::Isometry3d perturbed_pose_err = target.inverse() * source_perturbed;
172  std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
173  calcRotationalErrorDecomposed(perturbed_pose_err.rotation());
174 
175  // They should always pointing in the same direction, but when error is close to zero this can flip so we will correct
176  if (perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
177  {
178  perturbed_pose_rotation_err.first *= -1;
179  perturbed_pose_rotation_err.second *= -1;
180  }
181 
182  // Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference
183  Eigen::VectorXd perturbed_err;
184  if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2)
185  perturbed_err = concat(perturbed_pose_err.translation(),
186  perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - (2 * M_PI)));
187  else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2)
188  perturbed_err = concat(perturbed_pose_err.translation(),
189  perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + (2 * M_PI)));
190  else
191  perturbed_err = concat(perturbed_pose_err.translation(),
192  perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);
193 
194  return (perturbed_err - err);
195 }
196 
197 Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
198  const Eigen::Isometry3d& target_perturbed,
199  const Eigen::Isometry3d& source,
200  const Eigen::Isometry3d& source_perturbed)
201 {
202  Eigen::Isometry3d pose_err = target.inverse() * source;
203  std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
204  Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);
205 
206  Eigen::Isometry3d perturbed_pose_err = target_perturbed.inverse() * source_perturbed;
207  std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
208  calcRotationalErrorDecomposed(perturbed_pose_err.rotation());
209 
210  // They should always pointing in the same direction
211 #ifndef NDEBUG
212  if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
213  throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
214 #endif
215 
216  // Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference
217  Eigen::VectorXd perturbed_err;
218  if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2)
219  perturbed_err = concat(perturbed_pose_err.translation(),
220  perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - (2 * M_PI)));
221  else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2)
222  perturbed_err = concat(perturbed_pose_err.translation(),
223  perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + (2 * M_PI)));
224  else
225  perturbed_err = concat(perturbed_pose_err.translation(),
226  perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);
227 
228  return (perturbed_err - err);
229 }
230 
231 Eigen::Vector4d computeRandomColor()
232 {
233  Eigen::Vector4d c;
234  c.setZero();
235  c[3] = 1;
236  while (almostEqualRelativeAndAbs(c[0], c[1]) || almostEqualRelativeAndAbs(c[2], c[1]) ||
237  almostEqualRelativeAndAbs(c[2], c[0]))
238  {
239  c[0] = (rand() % 100) / 100.0;
240  c[1] = (rand() % 100) / 100.0;
241  c[2] = (rand() % 100) / 100.0;
242  }
243  return c;
244 }
245 
246 void printNestedException(const std::exception& e, int level) // NOLINT(misc-no-recursion)
247 {
248  std::cerr << std::string(static_cast<unsigned>(2 * level), ' ') << "exception: " << e.what() << "\n";
249  try
250  {
251  my_rethrow_if_nested(e);
252  }
253  catch (const std::exception& e)
254  {
255  printNestedException(e, level + 1);
256  }
257  catch (...) // NOLINT
258  {
259  }
260 }
261 
262 std::string getTempPath()
263 {
264  return std::filesystem::temp_directory_path().string() + std::string(1, std::filesystem::path::preferred_separator);
265 }
266 
267 bool isNumeric(const std::string& s)
268 {
269  if (s.empty())
270  return false;
271 
272  std::stringstream ss;
273  ss.imbue(std::locale::classic());
274 
275  ss << s;
276 
277  double out{ 0 };
278  ss >> out;
279 
280  return !ss.fail() && ss.eof();
281 }
282 
283 bool isNumeric(const std::vector<std::string>& sv)
284 {
285  return std::all_of(sv.cbegin(), sv.cend(), [](const std::string& s) { return isNumeric(s); });
286 }
287 
288 Eigen::VectorXd generateRandomNumber(const Eigen::Ref<const Eigen::MatrixX2d>& limits)
289 {
290  Eigen::VectorXd joint_values;
291  joint_values.resize(limits.rows());
292  for (long i = 0; i < limits.rows(); ++i)
293  {
294  std::uniform_real_distribution<double> sample(limits(i, 0), limits(i, 1));
295  joint_values(i) = sample(mersenne);
296  }
297  return joint_values;
298 }
299 
300 void ltrim(std::string& s) { s.erase(0, s.find_first_not_of(" \n\r\t\f\v")); }
301 
302 void rtrim(std::string& s) { s.erase(s.find_last_not_of(" \n\r\t\f\v") + 1); }
303 
304 void trim(std::string& s)
305 {
306  ltrim(s);
307  rtrim(s);
308 }
309 
310 std::string getTimestampString()
311 {
312  std::ostringstream oss;
313  auto t = std::time(nullptr);
314  auto tm = *std::localtime(&t);
315  oss << std::put_time(&tm, "%d-%m-%Y-%H-%M-%S");
316  return oss.str();
317 }
318 
319 void reorder(Eigen::Ref<Eigen::VectorXd> v, std::vector<Eigen::Index> order)
320 {
321  assert(v.rows() == static_cast<Eigen::Index>(order.size()));
322  // for all elements to put in place
323  for (std::size_t i = 0; i < order.size() - 1; ++i)
324  {
325  if (order[i] == static_cast<Eigen::Index>(i))
326  continue;
327 
328  size_t j{ 0 };
329  for (j = i + 1; j < order.size(); ++j)
330  {
331  if (order[j] == static_cast<Eigen::Index>(i))
332  break;
333  }
334  std::swap(v[static_cast<Eigen::Index>(i)], v[order[i]]);
335  std::swap(order[i], order[j]);
336  }
337 }
338 
339 int QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value)
340 {
341  if (xml_element->Value() == nullptr)
342  return tinyxml2::XML_NO_ATTRIBUTE;
343 
344  value = std::string(xml_element->Value());
345  trim(value);
346  return tinyxml2::XML_SUCCESS;
347 }
348 
349 int QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text)
350 {
351  if (xml_element->GetText() == nullptr)
352  return tinyxml2::XML_NO_ATTRIBUTE;
353 
354  text = std::string(xml_element->GetText());
355  trim(text);
356  return tinyxml2::XML_SUCCESS;
357 }
358 
359 int QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value)
360 {
361  if (xml_attribute->Value() == nullptr)
362  return tinyxml2::XML_WRONG_ATTRIBUTE_TYPE;
363 
364  value = std::string(xml_attribute->Value());
365  trim(value);
366  return tinyxml2::XML_SUCCESS;
367 }
368 
369 int QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value)
370 {
371  const tinyxml2::XMLAttribute* attribute = xml_element->FindAttribute(name);
372  if (attribute == nullptr)
373  return tinyxml2::XML_NO_ATTRIBUTE;
374 
375  return QueryStringValue(attribute, value);
376 }
377 
378 std::string StringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string default_value)
379 {
380  std::string str = std::move(default_value);
381  QueryStringAttribute(xml_element, name, str);
382  return str;
383 }
384 
385 int QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value)
386 {
387  int status = QueryStringAttribute(xml_element, name, value);
388 
389  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
390  {
391  CONSOLE_BRIDGE_logError("Invalid %s attribute '%s'", xml_element->Name(), name);
392  }
393  else if (status == tinyxml2::XML_NO_ATTRIBUTE)
394  {
395  CONSOLE_BRIDGE_logError("Missing %s required attribute '%s'", xml_element->Name(), name);
396  }
397  else if (status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE)
398  {
399  CONSOLE_BRIDGE_logError("Invalid %s attribute type '%s'", xml_element->Name(), name);
400  }
401 
402  return status;
403 }
404 
405 int QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, double& value)
406 {
407  int status = xml_element->QueryDoubleAttribute(name, &value);
408 
409  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
410  {
411  CONSOLE_BRIDGE_logError("Invalid %s attribute '%s'", xml_element->Name(), name);
412  }
413  else if (status == tinyxml2::XML_NO_ATTRIBUTE)
414  {
415  CONSOLE_BRIDGE_logError("Missing %s required attribute '%s'", xml_element->Name(), name);
416  }
417  else if (status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE)
418  {
419  CONSOLE_BRIDGE_logError("Invalid %s attribute type '%s'", xml_element->Name(), name);
420  }
421 
422  return status;
423 }
424 
425 int QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value)
426 {
427  int status = xml_element->QueryIntAttribute(name, &value);
428 
429  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
430  {
431  CONSOLE_BRIDGE_logError("Invalid %s attribute '%s'", xml_element->Name(), name);
432  }
433  else if (status == tinyxml2::XML_NO_ATTRIBUTE)
434  {
435  CONSOLE_BRIDGE_logError("Missing %s required attribute '%s'", xml_element->Name(), name);
436  }
437  else if (status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE)
438  {
439  CONSOLE_BRIDGE_logError("Invalid %s attribute type '%s'", xml_element->Name(), name);
440  }
441 
442  return status;
443 }
444 
445 bool almostEqualRelativeAndAbs(double a, double b, double max_diff, double max_rel_diff)
446 {
447  double diff = std::fabs(a - b);
448  if (diff <= max_diff)
449  return true;
450 
451  a = std::fabs(a);
452  b = std::fabs(b);
453  double largest = (b > a) ? b : a;
454 
455  return (diff <= largest * max_rel_diff);
456 }
457 
458 bool almostEqualRelativeAndAbs(const Eigen::Ref<const Eigen::VectorXd>& v1,
459  const Eigen::Ref<const Eigen::VectorXd>& v2,
460  double max_diff,
461  double max_rel_diff)
462 {
463  const auto eigen_max_diff = Eigen::VectorXd::Constant(v1.size(), max_diff);
464  const auto eigen_max_rel_diff = Eigen::VectorXd::Constant(v1.size(), max_rel_diff);
465  // NOLINTNEXTLINE(clang-analyzer-core.uninitialized.UndefReturn)
466  return almostEqualRelativeAndAbs(v1, v2, eigen_max_diff, eigen_max_rel_diff);
467 }
468 
469 bool almostEqualRelativeAndAbs(const Eigen::Ref<const Eigen::VectorXd>& v1,
470  const Eigen::Ref<const Eigen::VectorXd>& v2,
471  const Eigen::Ref<const Eigen::VectorXd>& max_diff,
472  const Eigen::Ref<const Eigen::VectorXd>& max_rel_diff)
473 {
474  if (v1.size() == 0 && v2.size() == 0)
475  return true;
476  if (v1.size() != v2.size() || v1.size() != max_diff.size() || v1.size() != max_rel_diff.size())
477  return false;
478 
479  Eigen::ArrayWrapper<const Eigen::Ref<const Eigen::VectorXd>> a1 = v1.array();
480  Eigen::ArrayWrapper<const Eigen::Ref<const Eigen::VectorXd>> a2 = v2.array();
481  Eigen::ArrayWrapper<const Eigen::Ref<const Eigen::VectorXd>> md = max_diff.array();
482  Eigen::ArrayWrapper<const Eigen::Ref<const Eigen::VectorXd>> mrd = max_rel_diff.array();
483 
484  auto diff_abs = (a1 - a2).abs();
485  double diff = diff_abs.maxCoeff();
486  if ((diff <= md).all())
487  return true;
488 
489  return (diff_abs <= (mrd * a1.abs().max(a2.abs()))).all();
490 }
491 
492 std::vector<std::string> getAllowedCollisions(const std::vector<std::string>& link_names,
493  const AllowedCollisionEntries& acm_entries,
494  bool remove_duplicates)
495 {
496  std::vector<std::string> results;
497  results.reserve(acm_entries.size());
498 
499  for (const auto& entry : acm_entries)
500  {
501  const std::string link_1 = entry.first.first;
502  const std::string link_2 = entry.first.second;
503 
504  // If the first entry is one of the links we were looking for
505  if (std::find(link_names.begin(), link_names.end(), link_1) != link_names.end())
506  // If it hasn't already been added or remove_duplicates is disabled
507  if (!remove_duplicates || (std::find(results.begin(), results.end(), link_2) == results.end()))
508  results.push_back(link_2);
509 
510  if (std::find(link_names.begin(), link_names.end(), link_2) != link_names.end())
511  if (!remove_duplicates || (std::find(results.begin(), results.end(), link_1) == results.end()))
512  results.push_back(link_1);
513  }
514  return results;
515 }
516 
517 } // namespace tesseract_common
tesseract_common::getTempPath
std::string getTempPath()
Get the host temp directory path.
Definition: utils.cpp:262
tesseract_common::QueryStringAttributeRequired
int 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:385
tesseract_common
Definition: allowed_collision_matrix.h:19
tesseract_common::generateRandomNumber
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:288
tesseract_common::concat
Eigen::VectorXd concat(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Concatenate two vector.
Definition: utils.cpp:119
tesseract_common::AllowedCollisionEntries
std::unordered_map< tesseract_common::LinkNamesPair, std::string, tesseract_common::PairHash > AllowedCollisionEntries
Definition: allowed_collision_matrix.h:24
tesseract_common::toNumeric< long >
template bool toNumeric< long >(const std::string &, long &)
tesseract_common::toNumeric< float >
template bool toNumeric< float >(const std::string &, float &)
tesseract_common::trim
void trim(std::string &s)
Trim left and right of string.
Definition: utils.cpp:304
tesseract_common::jacobianChangeRefPoint
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:111
macros.h
Common Tesseract Macros.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
tesseract_common::twistChangeBase
void twistChangeBase(Eigen::Ref< Eigen::VectorXd > twist, const Eigen::Isometry3d &change_base)
Change the base coordinate system of the Twist.
Definition: utils.cpp:98
tesseract_common::QueryStringValue
int QueryStringValue(const tinyxml2::XMLElement *xml_element, std::string &value)
Query a string value from xml element.
Definition: utils.cpp:339
tesseract_common::toNumeric< int >
template bool toNumeric< int >(const std::string &, int &)
tesseract_common::almostEqualRelativeAndAbs
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:445
tesseract_common::calcRotationalError
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:151
tesseract_common::twistChangeRefPoint
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:91
tesseract_common::reorder
void reorder(Eigen::Ref< Eigen::VectorXd > v, std::vector< Eigen::Index > order)
Reorder Eigen::VectorXd implace given index list.
Definition: utils.cpp:319
utils.h
Common Tesseract Utility Functions.
tesseract_common::QueryDoubleAttributeRequired
int 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:405
tesseract_common::mersenne
static std::mt19937 mersenne
Random number generator.
Definition: utils.h:55
tesseract_common::printNestedException
void printNestedException(const std::exception &e, int level=0)
Print a nested exception.
Definition: utils.cpp:246
tesseract_common::getTimestampString
std::string getTimestampString()
Get Timestamp string.
Definition: utils.cpp:310
tesseract_common::QueryStringAttribute
int QueryStringAttribute(const tinyxml2::XMLElement *xml_element, const char *name, std::string &value)
Query a string attribute from an xml element.
Definition: utils.cpp:369
tesseract_common::fileToString
std::string fileToString(const std::filesystem::path &filepath)
Read in the contents of the file into a string.
Definition: utils.cpp:75
tesseract_common::QueryStringText
int QueryStringText(const tinyxml2::XMLElement *xml_element, std::string &text)
Query a string Text from xml element.
Definition: utils.cpp:349
tesseract_common::calcTransformError
Eigen::VectorXd calcTransformError(const Eigen::Isometry3d &t1, const Eigen::Isometry3d &t2)
Calculate error between two transforms expressed in t1 coordinate system.
Definition: utils.cpp:157
tesseract_common::calcRotationalErrorDecomposed
std::pair< Eigen::Vector3d, double > calcRotationalErrorDecomposed(const Eigen::Ref< const Eigen::Matrix3d > &R)
Definition: utils.cpp:127
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: macros.h:72
tesseract_common::ltrim
void ltrim(std::string &s)
Left trim string.
Definition: utils.cpp:300
tesseract_common::getAllowedCollisions
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:492
tesseract_common::isNumeric
bool isNumeric(const std::string &s)
Determine if a string is a number.
Definition: utils.cpp:267
tesseract_common::jacobianChangeBase
void jacobianChangeBase(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base)
Change the base coordinate system of the jacobian.
Definition: utils.cpp:104
tesseract_common::rtrim
void rtrim(std::string &s)
Right trim string.
Definition: utils.cpp:302
tesseract_common::QueryIntAttributeRequired
int 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:425
tesseract_common::toNumeric< double >
template bool toNumeric< double >(const std::string &, double &)
tesseract_common::StringAttribute
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:378
tesseract_common::my_rethrow_if_nested
std::enable_if_t<!std::is_polymorphic< E >::value > my_rethrow_if_nested(const E &)
Definition: utils.cpp:63
tesseract_common::calcJacobianTransformErrorDiff
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d &target, const Eigen::Isometry3d &source, const Eigen::Isometry3d &source_perturbed)
Calculate jacobian transform error difference expressed in the target frame coordinate system.
Definition: utils.cpp:163
tesseract_common::computeRandomColor
Eigen::Vector4d computeRandomColor()
This computes a random color RGBA [0, 1] with alpha set to 1.
Definition: utils.cpp:231

tesseract_common
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:01:40