tesseract_common_unit.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 #include <type_traits>
5 #include <boost/archive/xml_oarchive.hpp>
6 #include <boost/archive/xml_iarchive.hpp>
7 #include <boost/serialization/shared_ptr.hpp>
8 #include <tinyxml2.h>
9 #include <sstream>
10 TESSERACT_COMMON_IGNORE_WARNINGS_POP
11 
12 #include <tesseract_common/utils.h>
13 #include <tesseract_common/sfinae_utils.h>
14 #include <tesseract_common/resource_locator.h>
15 #include <tesseract_common/manipulator_info.h>
16 #include <tesseract_common/joint_state.h>
17 #include <tesseract_common/types.h>
18 #include <tesseract_common/any_poly.h>
19 #include <tesseract_common/kinematic_limits.h>
20 #include <tesseract_common/yaml_utils.h>
21 #include <tesseract_common/yaml_extenstions.h>
22 #include <tesseract_common/collision_margin_data.h>
23 #include <tesseract_common/stopwatch.h>
24 #include <tesseract_common/timer.h>
25 #include <tesseract_common/profile.h>
26 #include <tesseract_common/profile_dictionary.h>
27 
29 class TestResourceLocator : public tesseract_common::ResourceLocator
30 {
31 public:
32  using Ptr = std::shared_ptr<TestResourceLocator>;
33  using ConstPtr = std::shared_ptr<const TestResourceLocator>;
34 
35  ~TestResourceLocator() override = default;
36 
37  tesseract_common::Resource::Ptr locateResource(const std::string& url) const override final
38  {
39  std::string mod_url = url;
40  if (url.find("package://tesseract_common") == 0)
41  {
42  mod_url.erase(0, strlen("package://tesseract_common"));
43  size_t pos = mod_url.find('/');
44  if (pos == std::string::npos)
45  return nullptr;
46 
47  mod_url.erase(0, pos);
48 
49  std::filesystem::path file_path(__FILE__);
50  std::string package_path = file_path.parent_path().parent_path().string();
51 
52  if (package_path.empty())
53  return nullptr;
54 
55  mod_url = package_path + mod_url;
56  }
57 
58  if (!std::filesystem::path(mod_url).is_absolute())
59  return nullptr;
60 
61  return std::make_shared<tesseract_common::SimpleLocatedResource>(
62  url, mod_url, std::make_shared<TestResourceLocator>(*this));
63  }
64 };
65 
66 class TestProfile : public tesseract_common::Profile
67 {
68 public:
69  TestProfile() = default;
70  ~TestProfile() override = default;
71  TestProfile(std::size_t key) : Profile(key) {}
72  TestProfile(const TestProfile&) = default;
73  TestProfile& operator=(const TestProfile&) = default;
74  TestProfile(TestProfile&&) = default;
75  TestProfile& operator=(TestProfile&&) = default;
76 
77  bool operator==(const TestProfile& rhs) const { return (key_ == rhs.key_); }
78  bool operator!=(const TestProfile& rhs) const { return !operator==(rhs); }
79 };
80 
81 TEST(TesseractCommonUnit, ProfileDictionary) // NOLINT
82 {
83  tesseract_common::ProfileDictionary profiles;
84 
85  const std::string ns{ "test_ns" };
86  const std::string profile_name{ "test_profile" };
87  const std::string profile_name_2{ "test_profile_2" };
88  const std::size_t profile_key{ 100 };
89  auto profile = std::make_shared<const TestProfile>(profile_key);
90  auto profile2 = std::make_shared<const TestProfile>(2 * profile_key);
91  profiles.addProfile(ns, profile_name, profile);
92  profiles.addProfile(ns, profile_name_2, profile);
93  profiles.addProfile(ns, profile_name, profile2);
94  profiles.addProfile(ns, profile_name_2, profile2);
95 
96  // Constructors
97  auto data = profiles.getAllProfileEntries();
98  tesseract_common::ProfileDictionary profiles_copy(profiles);
99  EXPECT_EQ(data, profiles_copy.getAllProfileEntries());
100  tesseract_common::ProfileDictionary profiles_copy_assign;
101  profiles_copy_assign = profiles;
102  EXPECT_EQ(data, profiles_copy_assign.getAllProfileEntries());
103  tesseract_common::ProfileDictionary profiles_move(std::move(profiles_copy));
104  EXPECT_EQ(data, profiles_move.getAllProfileEntries());
105  tesseract_common::ProfileDictionary profiles_move_assign;
106  profiles_move_assign = std::move(profiles_copy_assign);
107  EXPECT_EQ(data, profiles_move_assign.getAllProfileEntries());
108 
109  EXPECT_FALSE(profiles.hasProfile(profile_key, ns, "does_not_exist"));
110  EXPECT_FALSE(profiles.hasProfile(profile_key, "does_not_exist", profile_name));
111  EXPECT_FALSE(profiles.hasProfile(0, ns, profile_name));
112  EXPECT_TRUE(profiles.hasProfile(profile_key, ns, profile_name));
113  EXPECT_TRUE(profiles.hasProfile(profile_key, ns, profile_name_2));
114  EXPECT_TRUE(profiles.hasProfile(2 * profile_key, ns, profile_name));
115  EXPECT_TRUE(profiles.hasProfile(2 * profile_key, ns, profile_name_2));
116  EXPECT_TRUE(profile == profiles.getProfile(profile_key, ns, profile_name));
117  EXPECT_TRUE(profile == profiles.getProfile(profile_key, ns, profile_name_2));
118  EXPECT_TRUE(profile2 == profiles.getProfile(2 * profile_key, ns, profile_name));
119  EXPECT_TRUE(profile2 == profiles.getProfile(2 * profile_key, ns, profile_name_2));
120  EXPECT_TRUE(profiles.hasProfileEntry(profile_key, ns));
121  EXPECT_TRUE(profiles.hasProfileEntry(2 * profile_key, ns));
122  EXPECT_FALSE(profiles.hasProfileEntry(profile_key, "does_not_exist"));
123  EXPECT_FALSE(profiles.hasProfileEntry(0, ns));
124  EXPECT_EQ(profiles.getProfileEntry(profile_key, ns).size(), 2);
125  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 2);
126 
127  // Remove profile wich does not exist
128  profiles.removeProfile(profile_key, ns, "does_not_exist");
129  EXPECT_EQ(profiles.getProfileEntry(profile_key, ns).size(), 2);
130  profiles.removeProfile(profile_key, "does_not_exist", profile_name);
131  EXPECT_EQ(profiles.getProfileEntry(profile_key, ns).size(), 2);
132  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 2);
133 
134  // Remove profile which does exist
135  profiles.removeProfile(profile_key, ns, profile_name);
136  EXPECT_EQ(profiles.getProfileEntry(profile_key, ns).size(), 1);
137  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 2);
138  profiles.removeProfile(profile_key, ns, profile_name_2);
139  EXPECT_FALSE(profiles.hasProfileEntry(profile_key, ns));
140  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 2);
141 
142  profiles.removeProfile(2 * profile_key, ns, profile_name);
143  EXPECT_FALSE(profiles.hasProfileEntry(profile_key, ns));
144  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 1);
145  profiles.removeProfile(2 * profile_key, ns, profile_name_2);
146  EXPECT_FALSE(profiles.hasProfileEntry(profile_key, ns));
147  EXPECT_FALSE(profiles.hasProfileEntry(2 * profile_key, ns));
148  EXPECT_TRUE(profiles.getAllProfileEntries().empty());
149 
150  // Test clear
151  profiles.addProfile(ns, std::vector<std::string>{ profile_name, profile_name_2 }, profile);
152  profiles.addProfile(ns, std::vector<std::string>{ profile_name, profile_name_2 }, profile2);
153  EXPECT_EQ(profiles.getProfileEntry(profile_key, ns).size(), 2);
154  EXPECT_EQ(profiles.getProfileEntry(2 * profile_key, ns).size(), 2);
155  EXPECT_FALSE(profiles.getAllProfileEntries().empty());
156  profiles.clear();
157  EXPECT_TRUE(profiles.getAllProfileEntries().empty());
158 
159  profiles.addProfile(ns, std::vector<std::string>{ profile_name }, profile);
160  profiles.addProfile(ns, std::vector<std::string>{ profile_name_2 }, profile);
161  profiles.addProfile(ns, std::vector<std::string>{ profile_name }, profile2);
162  profiles.addProfile(ns, std::vector<std::string>{ profile_name_2 }, profile2);
163 
164  EXPECT_TRUE(profiles.hasProfileEntry(profile_key, ns));
165  profiles.removeProfileEntry(profile_key, "does_not_exist");
166  EXPECT_TRUE(profiles.hasProfileEntry(profile_key, ns));
167  profiles.removeProfileEntry(profile_key, ns);
168  EXPECT_FALSE(profiles.hasProfileEntry(profile_key, ns));
169 
170  EXPECT_TRUE(profiles.hasProfileEntry(2 * profile_key, ns));
171  profiles.removeProfileEntry(2 * profile_key, "does_not_exist");
172  EXPECT_TRUE(profiles.hasProfileEntry(2 * profile_key, ns));
173  profiles.removeProfileEntry(2 * profile_key, ns);
174  EXPECT_FALSE(profiles.hasProfileEntry(2 * profile_key, ns));
175 
176  EXPECT_TRUE(profiles.getAllProfileEntries().empty());
177 
178  // Test failures
179  EXPECT_ANY_THROW(profiles.addProfile(ns, std::vector<std::string>{ "" }, profile)); // NOLINT
180  EXPECT_TRUE(profiles.getAllProfileEntries().empty());
181 
182  profiles.addProfile(ns, std::vector<std::string>{ profile_name }, profile);
183  profiles.addProfile(ns, std::vector<std::string>{ profile_name_2 }, profile);
184 
185  EXPECT_ANY_THROW(profiles.addProfile("", profile_name, profile)); // NOLINT
186  EXPECT_ANY_THROW(profiles.addProfile(ns, "", profile)); // NOLINT
187  EXPECT_ANY_THROW(profiles.addProfile(ns, profile_name, nullptr)); // NOLINT
188  EXPECT_ANY_THROW(profiles.addProfile("", std::vector<std::string>{ profile_name }, profile)); // NOLINT
189  EXPECT_ANY_THROW(profiles.addProfile(ns, std::vector<std::string>(), profile)); // NOLINT
190  EXPECT_ANY_THROW(profiles.addProfile(ns, std::vector<std::string>{ "" }, profile)); // NOLINT
191  EXPECT_ANY_THROW(profiles.addProfile(ns, std::vector<std::string>{ "" }, profile2)); // NOLINT
192  EXPECT_ANY_THROW(profiles.addProfile(ns, std::vector<std::string>{ profile_name }, nullptr)); // NOLINT
193  EXPECT_ANY_THROW(profiles.getProfileEntry(profile_key, "does_not_exist")); // NOLINT
194  EXPECT_ANY_THROW(profiles.getProfileEntry(0, ns)); // NOLINT
195 }
196 
197 TEST(TesseractCommonUnit, isNumeric) // NOLINT
198 {
199  std::vector<std::string> true_test = { "1", "1.5", "-1", "-1.5", "1e-5", "1e5",
200  "-1e-5", "-1e5", "1.0e-5", "1.0e5", "-1.0e-5", "-1.0e5" };
201 
202  EXPECT_TRUE(tesseract_common::isNumeric(true_test));
203  for (const auto& s : true_test)
204  {
205  EXPECT_TRUE(tesseract_common::isNumeric(s));
206  }
207 
208  std::vector<std::string> false_test = { "a", "test sdfs", "1 2", "1.0 2.0", "+", "-", "=" };
209  EXPECT_FALSE(tesseract_common::isNumeric(false_test));
210  for (const auto& s : false_test)
211  {
212  EXPECT_FALSE(tesseract_common::isNumeric(s));
213  }
214 
215  std::string empty_string;
216  EXPECT_FALSE(tesseract_common::isNumeric(empty_string));
217 }
218 
219 TEST(TesseractCommonUnit, toNumeric) // NOLINT
220 {
221  std::vector<std::string> true_test = { "1", "1.5", "-1", "-1.5", "1e-5", "1e5",
222  "-1e-5", "-1e5", "1.0e-5", "1.0e5", "-1.0e-5", "-1.0e5" };
223 
224  std::vector<double> true_test_value = { 1, 1.5, -1, -1.5, 1e-5, 1e5, -1e-5, -1e5, 1.0e-5, 1.0e5, -1.0e-5, -1.0e5 };
225 
226  EXPECT_TRUE(tesseract_common::isNumeric(true_test));
227  for (size_t i = 0; i < true_test.size(); ++i)
228  {
229  double value = 0;
230  EXPECT_TRUE(tesseract_common::toNumeric<double>(true_test[i], value));
231  EXPECT_NEAR(value, true_test_value[i], 1e-8);
232  }
233 
234  std::vector<std::string> false_test = { "a", "test sdfs", "1 2", "1.0 2.0", "+", "-", "=" };
235  EXPECT_FALSE(tesseract_common::isNumeric(false_test));
236  for (const auto& s : false_test)
237  {
238  double value = 0;
239  EXPECT_FALSE(tesseract_common::toNumeric(s, value));
240  EXPECT_NEAR(value, 0, 1e-8);
241  }
242 
243  std::string empty_string;
244  double value = 0;
245  EXPECT_FALSE(tesseract_common::toNumeric(empty_string, value));
246 }
247 
248 TEST(TesseractCommonUnit, generateRandomNumber) // NOLINT
249 {
250  Eigen::MatrixX2d limits(4, 2);
251  limits(0, 0) = -5;
252  limits(0, 1) = 5;
253  limits(1, 0) = 0;
254  limits(1, 1) = 10;
255  limits(2, 0) = 5;
256  limits(2, 1) = 15;
257  limits(3, 0) = -15;
258  limits(3, 1) = -5;
259 
260  Eigen::VectorXd random_numbers = tesseract_common::generateRandomNumber(limits);
261  EXPECT_EQ(limits.rows(), random_numbers.rows());
262  for (long i = 0; i < limits.rows(); ++i)
263  {
264  EXPECT_LE(random_numbers(i), limits(i, 1));
265  EXPECT_GE(random_numbers(i), limits(i, 0));
266  }
267 
268  Eigen::MatrixX2d empty_limits;
269  Eigen::VectorXd random_numbers2 = tesseract_common::generateRandomNumber(empty_limits);
270  EXPECT_EQ(empty_limits.rows(), random_numbers2.rows());
271 
272  Eigen::MatrixX2d equal_limits(4, 2);
273  equal_limits(0, 0) = 5;
274  equal_limits(0, 1) = 5;
275  equal_limits(1, 0) = 5;
276  equal_limits(1, 1) = 5;
277  equal_limits(2, 0) = 5;
278  equal_limits(2, 1) = 5;
279  equal_limits(3, 0) = 5;
280  equal_limits(3, 1) = 5;
281  Eigen::VectorXd random_numbers3 = tesseract_common::generateRandomNumber(equal_limits);
282  EXPECT_EQ(equal_limits.rows(), random_numbers3.rows());
283  for (long i = 0; i < equal_limits.rows(); ++i)
284  {
285  EXPECT_NEAR(random_numbers3(i), 5, 1e-5);
286  }
287 
288  Eigen::MatrixX2d wrong_limits(4, 2);
289  wrong_limits(0, 0) = 5;
290  wrong_limits(0, 1) = -5;
291  wrong_limits(1, 0) = 5;
292  wrong_limits(1, 1) = -5;
293  wrong_limits(2, 0) = 5;
294  wrong_limits(2, 1) = -5;
295  wrong_limits(3, 0) = 5;
296  wrong_limits(3, 1) = -5;
297  Eigen::VectorXd random_numbers4 = tesseract_common::generateRandomNumber(wrong_limits);
298  EXPECT_EQ(wrong_limits.rows(), random_numbers4.rows());
299  for (long i = 0; i < limits.rows(); ++i)
300  {
301  EXPECT_GE(random_numbers4(i), wrong_limits(i, 1));
302  EXPECT_LE(random_numbers4(i), wrong_limits(i, 0));
303  }
304 }
305 
306 TEST(TesseractCommonUnit, trim) // NOLINT
307 {
308  std::string check1 = " trim";
309  std::string check2 = "trim ";
310  std::string check3 = " trim ";
311  std::string check_trimmed = "trim";
312 
313  std::string s = check1;
314  tesseract_common::rtrim(s);
315  EXPECT_EQ(s, check1);
316  tesseract_common::ltrim(s);
317  EXPECT_EQ(s, check_trimmed);
318 
319  s = check2;
320  tesseract_common::ltrim(s);
321  EXPECT_EQ(s, check2);
322  tesseract_common::rtrim(s);
323  EXPECT_EQ(s, check_trimmed);
324 
325  s = check1;
326  tesseract_common::trim(s);
327  EXPECT_EQ(s, check_trimmed);
328 
329  s = check2;
330  tesseract_common::trim(s);
331  EXPECT_EQ(s, check_trimmed);
332 
333  s = check3;
334  tesseract_common::trim(s);
335  EXPECT_EQ(s, check_trimmed);
336 }
337 
338 struct TestHasMemberFunction
339 {
340  bool update() const { return true; } // NOLINT
341  int add(int a) const { return a + 1; } // NOLINT
342 };
343 
344 struct TestHasMemberWithArgFunction
345 {
346  bool update(std::shared_ptr<TestHasMemberWithArgFunction>& p) { return (p == nullptr); } // NOLINT
347  double add(double a, double b) const { return a + b; } // NOLINT
348 };
349 
350 struct TestMissingMemberFunction
351 {
352  bool missingUpdate() const { return false; } // NOLINT
353  double add(int a) const { return a + 1; } // NOLINT
354 };
355 
356 CREATE_MEMBER_CHECK(update);
357 CREATE_MEMBER_FUNC_INVOCABLE_CHECK(update, std::shared_ptr<T>&);
358 CREATE_MEMBER_FUNC_INVOCABLE_CHECK(add, double, double);
359 CREATE_MEMBER_FUNC_RETURN_TYPE_CHECK(add, int, int);
360 CREATE_MEMBER_FUNC_SIGNATURE_CHECK(add, double, double, double);
361 
362 TEST(TesseractCommonUnit, sfinaeHasMemberFunction) // NOLINT
363 {
364  bool t_true = has_member_update<TestHasMemberFunction>::value;
365  bool t_false = has_member_update<TestMissingMemberFunction>::value;
366  EXPECT_TRUE(t_true);
367  EXPECT_FALSE(t_false);
368 }
369 
370 TEST(TesseractCommonUnit, sfinaeHasMemberFunctionInvocable) // NOLINT
371 {
372  bool i_update_true = has_member_func_invocable_update<TestHasMemberWithArgFunction>::value;
373  bool i_add_true = has_member_func_invocable_add<TestHasMemberWithArgFunction>::value;
374  bool i_update_false = has_member_func_invocable_update<TestHasMemberFunction>::value;
375  bool i_add_false = has_member_func_invocable_add<TestHasMemberFunction>::value;
376  EXPECT_TRUE(i_update_true);
377  EXPECT_TRUE(i_add_true);
378  EXPECT_FALSE(i_update_false);
379  EXPECT_FALSE(i_add_false);
380 }
381 
382 TEST(TesseractCommonUnit, sfinaeHasMemberFunctionWithReturnType) // NOLINT
383 {
384  bool i_add_true = has_member_func_return_type_add<TestHasMemberFunction>::value;
385  bool t_add_false = has_member_func_return_type_add<TestMissingMemberFunction>::value;
386  EXPECT_TRUE(i_add_true);
387  EXPECT_FALSE(t_add_false);
388 }
389 
390 TEST(TesseractCommonUnit, sfinaeHasMemberFunctionSignature) // NOLINT
391 {
392  bool i_add_true = has_member_func_signature_add<TestHasMemberWithArgFunction>::value;
393  bool t_add_false = has_member_func_signature_add<TestMissingMemberFunction>::value;
394  EXPECT_TRUE(i_add_true);
395  EXPECT_FALSE(t_add_false);
396 }
397 
398 TEST(TesseractCommonUnit, bytesResource) // NOLINT
399 {
400  std::vector<uint8_t> data;
401  data.reserve(8);
402  for (uint8_t i = 0; i < 8; i++)
403  data.push_back(i);
404 
405  std::shared_ptr<tesseract_common::BytesResource> bytes_resource =
406  std::make_shared<tesseract_common::BytesResource>("package://test_package/data.bin", data);
407  EXPECT_EQ(bytes_resource->getUrl(), "package://test_package/data.bin");
408  EXPECT_EQ(bytes_resource->isFile(), false);
409  EXPECT_EQ(bytes_resource->getFilePath(), "");
410  EXPECT_EQ(bytes_resource->locateResource("test"), nullptr);
411  auto data2 = bytes_resource->getResourceContents();
412  ASSERT_EQ(data.size(), data2.size());
413  for (size_t i = 0; i < data.size(); i++)
414  {
415  EXPECT_EQ(data[i], data2[i]);
416  }
417  auto data2_stream = bytes_resource->getResourceContentStream();
418  for (unsigned char& i : data)
419  {
420  char data2_val{ 0 };
421  data2_stream->read(&data2_val, 1);
422  EXPECT_EQ(i, *reinterpret_cast<uint8_t*>(&data2_val)); // NOLINT
423  }
424 
425  std::shared_ptr<tesseract_common::BytesResource> bytes_resource2 =
426  std::make_shared<tesseract_common::BytesResource>("package://test_package/data.bin", data.data(), data.size());
427  EXPECT_EQ(bytes_resource2->getUrl(), "package://test_package/data.bin");
428  EXPECT_EQ(bytes_resource->getResourceContents().size(), data.size());
429 }
430 
431 TEST(TesseractCommonUnit, fileToString) // NOLINT
432 {
433  tesseract_common::ResourceLocator::Ptr locator = std::make_shared<TestResourceLocator>();
434  tesseract_common::Resource::Ptr resource = locator->locateResource("package://tesseract_common/package.xml");
435  std::string data = tesseract_common::fileToString(std::filesystem::path(resource->getFilePath()));
436  EXPECT_FALSE(data.empty());
437 }
438 
439 TEST(TesseractCommonUnit, stopwatch) // NOLINT
440 {
441  tesseract_common::Stopwatch stopwatch;
442  stopwatch.start();
443  sleep(1);
444  auto elapsed_ms = stopwatch.elapsedMilliseconds();
445  auto elapsed_s = stopwatch.elapsedSeconds();
446  EXPECT_GT(elapsed_ms, 999);
447  EXPECT_GT(elapsed_s, 0.999);
448  sleep(1);
449  stopwatch.stop();
450  elapsed_ms = stopwatch.elapsedMilliseconds();
451  elapsed_s = stopwatch.elapsedSeconds();
452  EXPECT_GT(elapsed_ms, 1999);
453  EXPECT_GT(elapsed_s, 1.999);
454 }
455 
456 TEST(TesseractCommonUnit, timer) // NOLINT
457 {
458  int counter{ 0 };
459  auto callback = [&counter]() { ++counter; };
460  std::chrono::steady_clock::duration interval(std::chrono::milliseconds(1));
461  tesseract_common::Timer timer;
462  timer.start(callback, interval);
463  sleep(1);
464  timer.stop();
465  EXPECT_GT(counter, 900);
466 }
467 
468 TEST(TesseractCommonUnit, ManipulatorInfo) // NOLINT
469 {
470  // Empty tcp
471  tesseract_common::ManipulatorInfo manip_info;
472  EXPECT_TRUE(manip_info.empty());
473  EXPECT_TRUE(manip_info.tcp_frame.empty());
474  EXPECT_TRUE(manip_info.manipulator.empty());
475  EXPECT_TRUE(manip_info.manipulator_ik_solver.empty());
476  EXPECT_TRUE(manip_info.working_frame.empty());
477 
478  tesseract_common::ManipulatorInfo manip_info_override("manipulator", "world", "tool0");
479  manip_info_override.tcp_offset = Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.0, 0.0, 0.25);
480  manip_info_override.manipulator_ik_solver = "OPWInvKin";
481  manip_info_override.working_frame = "base_link";
482 
483  manip_info = manip_info.getCombined(manip_info_override);
484  EXPECT_FALSE(manip_info.empty());
485  EXPECT_TRUE(manip_info.tcp_frame == manip_info_override.tcp_frame);
486  EXPECT_EQ(manip_info.manipulator, manip_info_override.manipulator);
487  EXPECT_EQ(manip_info.manipulator_ik_solver, manip_info_override.manipulator_ik_solver);
488  EXPECT_EQ(manip_info.working_frame, manip_info_override.working_frame);
489 
490  // Test empty method
491  {
492  tesseract_common::ManipulatorInfo manip_info("manip", "world", "");
493  EXPECT_TRUE(manip_info.empty());
494  }
495 
496  {
497  tesseract_common::ManipulatorInfo manip_info("manip", "", "tool0");
498  EXPECT_TRUE(manip_info.empty());
499  }
500 
501  {
502  tesseract_common::ManipulatorInfo manip_info("", "world", "tool0");
503  EXPECT_TRUE(manip_info.empty());
504  }
505 
506  {
507  tesseract_common::ManipulatorInfo manip_info("", "", "");
508  manip_info.manipulator_ik_solver = "manip";
509  EXPECT_TRUE(manip_info.empty());
510  }
511 }
512 
513 TEST(TesseractCommonUnit, JointStateTest) // NOLINT
514 {
515  std::vector<std::string> joint_names{ "joint_1", "joint_2", "joint_3" };
516  Eigen::VectorXd positons = Eigen::VectorXd::Constant(3, 5);
517  tesseract_common::JointState joint_state(joint_names, positons);
518  EXPECT_TRUE(joint_state.joint_names == joint_names);
519  EXPECT_TRUE(joint_state.position.isApprox(positons, 1e-5));
520 }
521 
522 TEST(TesseractCommonUnit, anyUnit) // NOLINT
523 {
524  tesseract_common::AnyPoly any_null;
525  EXPECT_TRUE(any_null.getType() == std::type_index(typeid(nullptr)));
526  EXPECT_TRUE(any_null.isNull());
527 
528  tesseract_common::AnyPoly any_type;
529  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(nullptr)));
530  EXPECT_TRUE(any_type.isNull());
531 
532  tesseract_common::AnyPoly any_double(1.5);
533  EXPECT_TRUE(any_double.getType() == std::type_index(typeid(double)));
534  EXPECT_FALSE(any_double.isNull());
535 
536  EXPECT_TRUE(any_null == any_type);
537 
538  tesseract_common::JointState joint_state;
539  joint_state.joint_names = { "joint_1", "joint_2", "joint_3" };
540  joint_state.position = Eigen::VectorXd::Constant(3, 5);
541  joint_state.velocity = Eigen::VectorXd::Constant(3, 6);
542  joint_state.acceleration = Eigen::VectorXd::Constant(3, 7);
543  joint_state.effort = Eigen::VectorXd::Constant(3, 8);
544  joint_state.time = 100;
545 
546  any_type = joint_state;
547  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(tesseract_common::JointState)));
548  EXPECT_FALSE(any_type.isNull());
549  EXPECT_TRUE(any_type.as<tesseract_common::JointState>() == joint_state);
550 
551  // Expect False
552  EXPECT_FALSE(any_type == any_null);
553  EXPECT_FALSE(any_type == any_double);
554 
555  tesseract_common::AnyInterface& interface = any_type.get();
556  EXPECT_TRUE(interface.getType() == std::type_index(typeid(tesseract_common::JointState)));
557 
558  const tesseract_common::AnyInterface& interface_const = std::as_const(any_type).get();
559  EXPECT_TRUE(interface_const.getType() == std::type_index(typeid(tesseract_common::JointState)));
560 
561  // Construct with interface
562  tesseract_common::AnyPoly any_interface_copy(interface_const);
563  EXPECT_TRUE(any_interface_copy.getType() == std::type_index(typeid(tesseract_common::JointState)));
564  EXPECT_FALSE(any_interface_copy.isNull());
565  EXPECT_TRUE(any_interface_copy.as<tesseract_common::JointState>() == joint_state);
566 
567  // Check clone
568  tesseract_common::AnyPoly any_copy = any_type;
569  EXPECT_TRUE(any_copy == any_type);
570 
571  tesseract_common::AnyPoly any_copy2;
572  any_copy2 = any_type;
573  EXPECT_TRUE(any_copy2 == any_type);
574 
575  // Check to make sure it is not making a copy during cast
576  auto& any_type_ref1 = any_type.as<tesseract_common::JointState>();
577  auto& any_type_ref2 = any_type.as<tesseract_common::JointState>();
578  auto& any_copy_ref = any_copy.as<tesseract_common::JointState>();
579  EXPECT_TRUE(&any_type_ref1 == &any_type_ref2);
580  EXPECT_TRUE(&any_type_ref1 != &any_copy_ref);
581  EXPECT_TRUE(&any_type_ref2 != &any_copy_ref);
582 
583  const auto& any_type_const_ref1 = any_type.as<tesseract_common::JointState>();
584  const auto& any_type_const_ref2 = any_type.as<tesseract_common::JointState>();
585  EXPECT_TRUE(&any_type_const_ref1 == &any_type_const_ref2);
586 
587  {
588  std::ofstream os(tesseract_common::getTempPath() + "any_type_boost.xml");
589  boost::archive::xml_oarchive oa(os);
590  oa << BOOST_SERIALIZATION_NVP(any_type);
591  }
592 
593  tesseract_common::AnyPoly nany_type;
594  {
595  std::ifstream ifs(tesseract_common::getTempPath() + "any_type_boost.xml");
596  assert(ifs.good());
597  boost::archive::xml_iarchive ia(ifs);
598 
599  // restore the schedule from the archive
600  ia >> BOOST_SERIALIZATION_NVP(nany_type);
601  }
602 
603  EXPECT_TRUE(nany_type.getType() == std::type_index(typeid(tesseract_common::JointState)));
604  EXPECT_TRUE(nany_type.as<tesseract_common::JointState>() == joint_state);
605 
606  // Test bad cast
607  EXPECT_ANY_THROW(nany_type.as<tesseract_common::Toolpath>()); // NOLINT
608 }
609 
610 template <typename T>
611 void runAnyPolyIntegralTest(T value, const std::string& type_str)
612 {
613  tesseract_common::AnyPoly any_type{ value };
614  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(T)));
615  EXPECT_TRUE(any_type.as<T>() == value);
616 
617  // Check clone
618  tesseract_common::AnyPoly any_copy = any_type;
619  EXPECT_TRUE(any_copy == any_type);
620  EXPECT_TRUE(any_copy.as<T>() == value);
621 
622  const std::string filepath{ tesseract_common::getTempPath() + "any_" + type_str + "_type_boost.xml" };
623  {
624  std::ofstream os(filepath);
625  boost::archive::xml_oarchive oa(os);
626  oa << BOOST_SERIALIZATION_NVP(any_type);
627  }
628 
629  tesseract_common::AnyPoly nany_type;
630  {
631  std::ifstream ifs(filepath);
632  assert(ifs.good());
633  boost::archive::xml_iarchive ia(ifs);
634 
635  // restore the schedule from the archive
636  ia >> BOOST_SERIALIZATION_NVP(nany_type);
637  }
638 
639  EXPECT_TRUE(nany_type.getType() == std::type_index(typeid(T)));
640  EXPECT_TRUE(nany_type.as<T>() == value);
641 
642  // Test bad cast
643  EXPECT_ANY_THROW(nany_type.as<tesseract_common::Toolpath>()); // NOLINT
644 }
645 
646 TEST(TesseractCommonUnit, anyIntegralTypesUnit) // NOLINT
647 {
648  runAnyPolyIntegralTest<bool>(true, "bool");
649  runAnyPolyIntegralTest<int>(-10, "int");
650  runAnyPolyIntegralTest<unsigned>(5, "unsigned");
651  runAnyPolyIntegralTest<double>(1.2, "double");
652  runAnyPolyIntegralTest<float>(-0.2F, "float");
653  runAnyPolyIntegralTest<std::string>("this", "string");
654  runAnyPolyIntegralTest<std::size_t>(10, "std_size_t");
655 }
656 
657 template <typename T>
658 void runAnyPolyUnorderedMapIntegralTest(T value, const std::string& type_str)
659 {
660  std::unordered_map<std::string, T> data;
661  data["test"] = std::move(value);
662  tesseract_common::AnyPoly any_type{ data };
663  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(std::unordered_map<std::string, T>)));
664  bool check = any_type.as<std::unordered_map<std::string, T>>() == data;
665  EXPECT_TRUE(check);
666 
667  // Check clone
668  tesseract_common::AnyPoly any_copy = any_type;
669  EXPECT_TRUE(any_copy == any_type);
670  check = any_copy.as<std::unordered_map<std::string, T>>() == data;
671  EXPECT_TRUE(check);
672 
673  const std::string filepath{ tesseract_common::getTempPath() + "any_unordered_map_string_" + type_str +
674  "_type_boost.xml" };
675  {
676  std::ofstream os(filepath);
677  boost::archive::xml_oarchive oa(os);
678  oa << BOOST_SERIALIZATION_NVP(any_type);
679  }
680 
681  tesseract_common::AnyPoly nany_type;
682  {
683  std::ifstream ifs(filepath);
684  assert(ifs.good());
685  boost::archive::xml_iarchive ia(ifs);
686 
687  // restore the schedule from the archive
688  ia >> BOOST_SERIALIZATION_NVP(nany_type);
689  }
690 
691  EXPECT_TRUE(nany_type.getType() == std::type_index(typeid(std::unordered_map<std::string, T>)));
692  check = nany_type.as<std::unordered_map<std::string COMMA T>>() == data;
693  EXPECT_TRUE(check);
694 
695  // Test bad cast
696  EXPECT_ANY_THROW(nany_type.as<tesseract_common::Toolpath>()); // NOLINT
697 }
698 
699 TEST(TesseractCommonUnit, anyUnorderedMapIntegralTypesUnit) // NOLINT
700 {
701  runAnyPolyUnorderedMapIntegralTest<bool>(true, "bool");
702  runAnyPolyUnorderedMapIntegralTest<int>(-10, "int");
703  runAnyPolyUnorderedMapIntegralTest<unsigned>(5, "unsigned");
704  runAnyPolyUnorderedMapIntegralTest<double>(1.2, "double");
705  runAnyPolyUnorderedMapIntegralTest<float>(-0.2F, "float");
706  runAnyPolyUnorderedMapIntegralTest<std::string>("this", "string");
707  runAnyPolyUnorderedMapIntegralTest<std::size_t>(10, "std_size_t");
708 }
709 
710 TEST(TesseractCommonUnit, anySharedPtrUnit) // NOLINT
711 {
712  tesseract_common::AnyPoly any_type;
713  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(nullptr)));
714 
715  tesseract_common::JointState joint_state;
716  joint_state.joint_names = { "joint_1", "joint_2", "joint_3" };
717  joint_state.position = Eigen::VectorXd::Constant(3, 5);
718  joint_state.velocity = Eigen::VectorXd::Constant(3, 6);
719  joint_state.acceleration = Eigen::VectorXd::Constant(3, 7);
720  joint_state.effort = Eigen::VectorXd::Constant(3, 8);
721  joint_state.time = 100;
722 
723  auto joint_state_ptr = std::make_shared<tesseract_common::JointState>(joint_state);
724  any_type = joint_state_ptr;
725  EXPECT_TRUE(any_type.getType() == std::type_index(typeid(std::shared_ptr<tesseract_common::JointState>)));
726  EXPECT_TRUE(*any_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state);
727  EXPECT_TRUE(any_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state_ptr);
728 
729  // Check clone
730  tesseract_common::AnyPoly any_copy = any_type;
731  EXPECT_TRUE(any_copy == any_type);
732 
733  // Check to make sure it is not making a copy during cast
734  auto& any_type_ref1 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
735  auto& any_type_ref2 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
736  auto& any_copy_ref = any_copy.as<std::shared_ptr<tesseract_common::JointState>>();
737  EXPECT_TRUE(&any_type_ref1 == &any_type_ref2);
738  EXPECT_TRUE(&any_type_ref1 != &any_copy_ref);
739  EXPECT_TRUE(&any_type_ref2 != &any_copy_ref);
740 
741  const auto& any_type_const_ref1 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
742  const auto& any_type_const_ref2 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
743  EXPECT_TRUE(&any_type_const_ref1 == &any_type_const_ref2);
744 
745  {
746  std::ofstream os(tesseract_common::getTempPath() + "any_shared_ptr_type_boost.xml");
747  boost::archive::xml_oarchive oa(os);
748  oa << BOOST_SERIALIZATION_NVP(any_type);
749  }
750 
751  tesseract_common::AnyPoly nany_type;
752  {
753  std::ifstream ifs(tesseract_common::getTempPath() + "any_shared_ptr_type_boost.xml");
754  assert(ifs.good());
755  boost::archive::xml_iarchive ia(ifs);
756 
757  // restore the schedule from the archive
758  ia >> BOOST_SERIALIZATION_NVP(nany_type);
759  }
760 
761  EXPECT_TRUE(nany_type.getType() == std::type_index(typeid(std::shared_ptr<tesseract_common::JointState>)));
762  EXPECT_TRUE(*nany_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state);
763 
764  // Test bad cast
765  EXPECT_ANY_THROW(nany_type.as<tesseract_common::JointState>()); // NOLINT
766 }
767 
768 TEST(TesseractCommonUnit, boundsUnit) // NOLINT
769 {
770  Eigen::VectorXd v = Eigen::VectorXd::Ones(6);
771  v = v.array() + std::numeric_limits<float>::epsilon();
772  Eigen::MatrixX2d limits(6, 2); // NOLINT(clang-analyzer-core.uninitialized.UndefReturn)
773  limits.col(0) = -Eigen::VectorXd::Ones(6);
774  limits.col(1) = Eigen::VectorXd::Ones(6);
775 
776  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
777  EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
778  EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
779  tesseract_common::enforceLimits<double>(v, limits);
780  EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
781 
782  v = -Eigen::VectorXd::Ones(6);
783  v = v.array() - std::numeric_limits<float>::epsilon();
784 
785  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
786  EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
787  EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
788  tesseract_common::enforceLimits<double>(v, limits);
789  EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
790 
791  // Check that clamp is done correctly on both sides
792  v = Eigen::VectorXd::Constant(6, -2);
793  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
794  EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
795  tesseract_common::enforceLimits<double>(v, limits);
796  ASSERT_EQ((v - limits.col(0)).norm(), 0);
797 
798  v = Eigen::VectorXd::Constant(6, 2);
799  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
800  EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
801  tesseract_common::enforceLimits<double>(v, limits);
802  ASSERT_EQ((v - limits.col(1)).norm(), 0);
803 
804  v = Eigen::VectorXd::Ones(6);
805  v = v.array() - std::numeric_limits<float>::epsilon();
806  EXPECT_TRUE(tesseract_common::isWithinLimits<double>(v, limits));
807 
808  v = Eigen::VectorXd::Ones(6);
809  v(3) = v(3) + std::numeric_limits<float>::epsilon();
810  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
811 
812  v = -Eigen::VectorXd::Ones(6);
813  v(3) = v(3) - std::numeric_limits<float>::epsilon();
814  EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
815 }
816 
817 TEST(TesseractCommonUnit, isIdenticalUnit) // NOLINT
818 {
819  std::vector<std::string> v1{ "a", "b", "c" };
820  std::vector<std::string> v2{ "a", "b", "c" };
821  EXPECT_TRUE(tesseract_common::isIdentical(v1, v2, false));
822  EXPECT_TRUE(tesseract_common::isIdentical(v1, v2, true));
823 
824  v2 = { "c", "b", "a" };
825  EXPECT_TRUE(tesseract_common::isIdentical(v1, v2, false));
826  EXPECT_FALSE(tesseract_common::isIdentical(v1, v2, true));
827 
828  v2 = { "a", "b", "d" };
829  EXPECT_FALSE(tesseract_common::isIdentical(v1, v2, false));
830  EXPECT_FALSE(tesseract_common::isIdentical(v1, v2, true));
831 }
832 
833 TEST(TesseractCommonUnit, isIdenticalMapUnit) // NOLINT
834 {
835  std::map<std::string, int> v1;
836  v1["1"] = 1;
837  v1["2"] = 2;
838  std::map<std::string, int> v2;
839  bool equal = tesseract_common::isIdenticalMap<std::map<std::string, int>, int>(v1, v2);
840  EXPECT_FALSE(equal);
841 
842  v2["2"] = 2;
843  equal = tesseract_common::isIdenticalMap<std::map<std::string, int>, int>(v1, v2);
844  EXPECT_FALSE(equal);
845 
846  v2 = v1;
847  equal = tesseract_common::isIdenticalMap<std::map<std::string, int>, int>(v1, v2);
848  EXPECT_TRUE(equal);
849 
850  v1.clear();
851  equal = tesseract_common::isIdenticalMap<std::map<std::string, int>, int>(v1, v2);
852  EXPECT_FALSE(equal);
853 }
854 
855 TEST(TesseractCommonUnit, isIdenticalSetUnit) // NOLINT
856 {
857  std::set<int> v1;
858  std::set<int> v2;
859  bool equal = tesseract_common::isIdenticalSet<int>(v1, v2);
860  EXPECT_TRUE(equal);
861 
862  v1.insert(1);
863  equal = tesseract_common::isIdenticalSet<int>(v1, v2);
864  EXPECT_FALSE(equal);
865 
866  v2.insert(1);
867  v2.insert(2);
868  equal = tesseract_common::isIdenticalSet<int>(v1, v2);
869  EXPECT_FALSE(equal);
870 
871  v1.insert(2);
872  equal = tesseract_common::isIdenticalSet<int>(v1, v2);
873  EXPECT_TRUE(equal);
874 }
875 
876 TEST(TesseractCommonUnit, isIdenticalArrayUnit) // NOLINT
877 {
878  {
879  std::array<int, 4> v1 = { 1, 2, 3, 4 };
880  std::array<int, 4> v2 = { 1, 2, 3, 4 };
881  bool equal = tesseract_common::isIdenticalArray<int, 4>(v1, v2);
882  EXPECT_TRUE(equal);
883  }
884  {
885  std::array<int, 4> v1 = { 1, 2, 3, 4 };
886  std::array<int, 4> v2 = { -1, 2, 3, 4 };
887  bool equal = tesseract_common::isIdenticalArray<int, 4>(v1, v2);
888  EXPECT_FALSE(equal);
889  }
890  {
891  // Clang-tidy catches initialized arrays anyway, but check it just in case the caller isn't running clang-tidy
892  std::array<int, 4> v1 = { 1, 2, 3, 6 };
893  std::array<int, 4> v2; // NOLINT
894  bool equal = tesseract_common::isIdenticalArray<int, 4>(v1, v2);
895  EXPECT_FALSE(equal);
896  }
897 }
898 
899 TEST(TesseractCommonUnit, pointersEqual) // NOLINT
900 {
901  {
902  auto p1 = std::make_shared<int>(1);
903  auto p2 = std::make_shared<int>(2);
904  bool equal = tesseract_common::pointersEqual(p1, p2);
905  EXPECT_FALSE(equal);
906  }
907  {
908  auto p1 = std::make_shared<int>(1);
909  auto p2 = nullptr;
910  bool equal = tesseract_common::pointersEqual<int>(p1, p2);
911  EXPECT_FALSE(equal);
912  }
913  {
914  auto p1 = nullptr;
915  auto p2 = std::make_shared<int>(2);
916  bool equal = tesseract_common::pointersEqual<int>(p1, p2);
917  EXPECT_FALSE(equal);
918  }
919  {
920  auto p1 = nullptr;
921  auto p2 = nullptr;
922  bool equal = tesseract_common::pointersEqual<int>(p1, p2);
923  EXPECT_TRUE(equal);
924  }
925  {
926  auto p1 = std::make_shared<int>(1);
927  auto p2 = std::make_shared<int>(1);
928  bool equal = tesseract_common::pointersEqual<int>(p1, p2);
929  EXPECT_TRUE(equal);
930  }
931 }
932 
933 TEST(TesseractCommonUnit, pointersComparison) // NOLINT
934 {
935  // True if p1 < p2
936  {
937  auto p1 = std::make_shared<int>(1);
938  auto p2 = std::make_shared<int>(2);
939  bool equal = tesseract_common::pointersComparison<int>(p1, p2);
940  EXPECT_TRUE(equal);
941  }
942  {
943  auto p1 = std::make_shared<int>(1);
944  auto p2 = nullptr;
945  bool equal = tesseract_common::pointersComparison<int>(p1, p2);
946  EXPECT_FALSE(equal);
947  }
948  {
949  auto p1 = nullptr;
950  auto p2 = std::make_shared<int>(2);
951  bool equal = tesseract_common::pointersComparison<int>(p1, p2);
952  EXPECT_TRUE(equal);
953  }
954  {
955  auto p1 = nullptr;
956  auto p2 = nullptr;
957  bool equal = tesseract_common::pointersComparison<int>(p1, p2);
958  EXPECT_FALSE(equal);
959  }
960  {
961  auto p1 = std::make_shared<int>(1);
962  auto p2 = std::make_shared<int>(1);
963  bool equal = tesseract_common::pointersComparison<int>(p1, p2);
964  EXPECT_FALSE(equal);
965  }
966 }
967 
968 TEST(TesseractCommonUnit, getTimestampStringUnit) // NOLINT
969 {
970  std::string s1 = tesseract_common::getTimestampString();
971  EXPECT_FALSE(s1.empty());
972 }
973 
974 TEST(TesseractCommonUnit, reorder) // NOLINT
975 {
976  std::vector<std::vector<Eigen::Index>> checks;
977  checks.push_back({ 5, 4, 3, 2, 1, 0 });
978  checks.push_back({ 0, 1, 2, 3, 4, 5 });
979  checks.push_back({ 3, 2, 4, 1, 5, 0 });
980  Eigen::VectorXd v = Eigen::VectorXd::Random(6);
981 
982  for (const auto& check : checks)
983  {
984  Eigen::VectorXd v_copy = v;
985  tesseract_common::reorder(v_copy, check);
986 
987  for (std::size_t i = 0; i < check.size(); ++i)
988  {
989  EXPECT_NEAR(v_copy(static_cast<Eigen::Index>(i)), v(check[i]), 1e-8);
990  }
991  }
992 }
993 
994 TEST(TesseractCommonUnit, getTempPathUnit) // NOLINT
995 {
996  std::string s1 = tesseract_common::getTempPath();
997  EXPECT_FALSE(s1.empty());
998  EXPECT_TRUE(std::filesystem::exists(s1));
999 }
1000 
1001 TEST(TesseractCommonUnit, QueryStringValueUnit) // NOLINT
1002 {
1003  {
1004  std::string str = R"(<box>Test</box>)";
1005  tinyxml2::XMLDocument xml_doc;
1006  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1007 
1008  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1009  EXPECT_TRUE(element != nullptr);
1010 
1011  std::string string_value;
1012  int status = tesseract_common::QueryStringValue(element, string_value);
1013  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1014  EXPECT_TRUE(string_value == "box");
1015  }
1016 }
1017 
1018 TEST(TesseractCommonUnit, QueryStringTextUnit) // NOLINT
1019 {
1020  {
1021  std::string str = R"(<box>Test</box>)";
1022  tinyxml2::XMLDocument xml_doc;
1023  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1024 
1025  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1026  EXPECT_TRUE(element != nullptr);
1027 
1028  std::string string_value;
1029  int status = tesseract_common::QueryStringText(element, string_value);
1030  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1031  EXPECT_TRUE(string_value == "Test");
1032  }
1033 
1034  {
1035  std::string str = R"(<box></box>)";
1036  tinyxml2::XMLDocument xml_doc;
1037  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1038 
1039  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1040  EXPECT_TRUE(element != nullptr);
1041 
1042  std::string string_value;
1043  int status = tesseract_common::QueryStringText(element, string_value);
1044  EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE);
1045  }
1046 }
1047 
1048 TEST(TesseractCommonUnit, QueryStringAttributeUnit) // NOLINT
1049 {
1050  {
1051  std::string str = R"(<box name="test" />)";
1052  tinyxml2::XMLDocument xml_doc;
1053  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1054 
1055  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1056  EXPECT_TRUE(element != nullptr);
1057 
1058  std::string string_value;
1059  int status = tesseract_common::QueryStringAttribute(element, "name", string_value);
1060  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1061  EXPECT_TRUE(string_value == "test");
1062  }
1063 
1064  {
1065  std::string str = R"(<box missing="test" />)";
1066  tinyxml2::XMLDocument xml_doc;
1067  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1068 
1069  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1070  EXPECT_TRUE(element != nullptr);
1071 
1072  std::string string_value;
1073  int status = tesseract_common::QueryStringAttribute(element, "name", string_value);
1074  EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE);
1075  }
1076 }
1077 
1078 TEST(TesseractCommonUnit, StringAttributeUnit) // NOLINT
1079 {
1080  {
1081  std::string str = R"(<box name="test" />)";
1082  tinyxml2::XMLDocument xml_doc;
1083  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1084 
1085  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1086  EXPECT_TRUE(element != nullptr);
1087 
1088  std::string string_value = tesseract_common::StringAttribute(element, "name", "default");
1089  EXPECT_TRUE(string_value == "test");
1090  }
1091 
1092  {
1093  std::string str = R"(<box name="test" />)";
1094  tinyxml2::XMLDocument xml_doc;
1095  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1096 
1097  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1098  EXPECT_TRUE(element != nullptr);
1099 
1100  std::string string_value = tesseract_common::StringAttribute(element, "missing", "default");
1101  EXPECT_TRUE(string_value == "default");
1102  }
1103 }
1104 
1105 TEST(TesseractCommonUnit, QueryStringAttributeRequiredUnit) // NOLINT
1106 {
1107  {
1108  std::string str = R"(<box name="test" />)";
1109  tinyxml2::XMLDocument xml_doc;
1110  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1111 
1112  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1113  EXPECT_TRUE(element != nullptr);
1114 
1115  std::string string_value;
1116  int status = tesseract_common::QueryStringAttributeRequired(element, "name", string_value);
1117  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1118  EXPECT_TRUE(string_value == "test");
1119  }
1120 
1121  {
1122  std::string str = R"(<box name="test" />)";
1123  tinyxml2::XMLDocument xml_doc;
1124  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1125 
1126  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1127  EXPECT_TRUE(element != nullptr);
1128 
1129  std::string string_value;
1130  int status = tesseract_common::QueryStringAttributeRequired(element, "missing", string_value);
1131  EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE);
1132  }
1133 }
1134 
1135 TEST(TesseractCommonUnit, QueryDoubleAttributeRequiredUnit) // NOLINT
1136 {
1137  {
1138  std::string str = R"(<box name="1.5" />)";
1139  tinyxml2::XMLDocument xml_doc;
1140  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1141 
1142  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1143  EXPECT_TRUE(element != nullptr);
1144 
1145  double double_value{ 0 };
1146  int status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value);
1147  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1148  EXPECT_NEAR(double_value, 1.5, 1e-6);
1149  }
1150 
1151  {
1152  std::string str = R"(<box name="1.5" />)";
1153  tinyxml2::XMLDocument xml_doc;
1154  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1155 
1156  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1157  EXPECT_TRUE(element != nullptr);
1158 
1159  double double_value{ 0 };
1160  int status = tesseract_common::QueryDoubleAttributeRequired(element, "missing", double_value);
1161  EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE);
1162  }
1163 
1164  {
1165  std::string str = R"(<box name="abc" />)";
1166  tinyxml2::XMLDocument xml_doc;
1167  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1168 
1169  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1170  EXPECT_TRUE(element != nullptr);
1171 
1172  double double_value{ 0 };
1173  int status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value);
1174  EXPECT_TRUE(status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE);
1175  }
1176 }
1177 
1178 TEST(TesseractCommonUnit, QueryIntAttributeRequiredUnit) // NOLINT
1179 {
1180  {
1181  std::string str = R"(<box name="1" />)";
1182  tinyxml2::XMLDocument xml_doc;
1183  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1184 
1185  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1186  EXPECT_TRUE(element != nullptr);
1187 
1188  int int_value{ 0 };
1189  int status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value);
1190  EXPECT_TRUE(status == tinyxml2::XML_SUCCESS);
1191  EXPECT_NEAR(int_value, 1, 1e-6);
1192  }
1193 
1194  {
1195  std::string str = R"(<box name="1.5" />)";
1196  tinyxml2::XMLDocument xml_doc;
1197  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1198 
1199  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1200  EXPECT_TRUE(element != nullptr);
1201 
1202  int int_value{ 0 };
1203  int status = tesseract_common::QueryIntAttributeRequired(element, "missing", int_value);
1204  EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE);
1205  }
1206 
1207  {
1208  std::string str = R"(<box name="abc" />)";
1209  tinyxml2::XMLDocument xml_doc;
1210  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1211 
1212  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("box");
1213  EXPECT_TRUE(element != nullptr);
1214 
1215  int int_value{ 0 };
1216  int status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value);
1217  EXPECT_TRUE(status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE);
1218  }
1219 }
1220 
1221 // sample function that catches an exception and wraps it in a nested exception
1222 void runThrowNestedException()
1223 {
1224  try
1225  {
1226  throw std::runtime_error("failed");
1227  }
1228  catch (...)
1229  {
1230  std::throw_with_nested(std::runtime_error("runThrowNestedException() failed"));
1231  }
1232 }
1233 
1234 TEST(TesseractCommonUnit, printNestedExceptionUnit) // NOLINT
1235 {
1236  try
1237  {
1238  runThrowNestedException();
1239  }
1240  catch (const std::exception& e)
1241  {
1242  tesseract_common::printNestedException(e);
1243  }
1244 }
1245 
1246 TEST(TesseractCommonUnit, checkForUnknownKeys) // NOLINT
1247 {
1248  std::set<std::string> expected_keys{ "search_paths", "search_libraries" };
1249 
1250  std::string yaml_string = R"(kinematic_plugins:
1251  search_paths:
1252  - /usr/local/lib
1253  search_libraries:
1254  - tesseract_kinematics_kdl_factories)";
1255 
1256  YAML::Node config = tesseract_common::fromYAMLString(yaml_string);
1257  EXPECT_NO_THROW(tesseract_common::checkForUnknownKeys(config["kinematic_plugins"], expected_keys)); // NOLINT
1258 
1259  // Not a map
1260  EXPECT_ANY_THROW(
1261  tesseract_common::checkForUnknownKeys(config["kinematic_plugins"]["search_paths"], expected_keys)); // NOLINT
1262 
1263  expected_keys = { "search_paths" };
1264  EXPECT_ANY_THROW(tesseract_common::checkForUnknownKeys(config["kinematic_plugins"], expected_keys)); // NOLINT
1265 }
1266 
1267 TEST(TesseractCommonUnit, almostEqualRelativeAndAbsUnit) // NOLINT
1268 {
1269  double a = 1e-5;
1270  double b = 0;
1271  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(a, b));
1272 
1273  a = 1e-7;
1274  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(a, b));
1275 
1276  a = 100000000000000.01;
1277  b = 100000000000000;
1278  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(a, b));
1279 
1280  a = 100000000000000.1;
1281  b = 100000000000000;
1282  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(a, b));
1283 
1284  Eigen::VectorXd v1 = Eigen::VectorXd::Constant(3, 1e-5);
1285  Eigen::VectorXd v2 = Eigen::VectorXd::Constant(3, 0);
1286  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(v1, v2));
1287 
1288  v1 = Eigen::VectorXd::Constant(3, 1e-7);
1289  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(v1, v2));
1290 
1291  v1 = Eigen::VectorXd::Constant(3, 100000000000000.01);
1292  v2 = Eigen::VectorXd::Constant(3, 100000000000000);
1293  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(v1, v2));
1294 
1295  v1 = Eigen::VectorXd::Constant(3, 100000000000000.1);
1296  v2 = Eigen::VectorXd::Constant(3, 100000000000000);
1297  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(v1, v2));
1298 
1299  v2 = Eigen::VectorXd::Constant(1, 100000000000000);
1300  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(v1, v2));
1301 
1302  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(Eigen::VectorXd(), Eigen::VectorXd()));
1303 }
1304 
1305 TEST(TesseractCommonUnit, kinematicsPluginInfoUnit) // NOLINT
1306 {
1307  tesseract_common::KinematicsPluginInfo kpi;
1308  EXPECT_TRUE(kpi.empty());
1309 
1310  tesseract_common::KinematicsPluginInfo kpi_insert;
1311  kpi_insert.search_paths.insert("/usr/local/lib");
1312  kpi_insert.search_libraries.insert("tesseract_collision");
1313 
1314  {
1315  tesseract_common::PluginInfo pi;
1316  pi.class_name = "KDLFwdKin";
1317  kpi_insert.fwd_plugin_infos["manipulator"].plugins = { std::make_pair("KDLFwdKin", pi) };
1318  kpi_insert.fwd_plugin_infos["manipulator"].default_plugin = "KDLFwdKin";
1319  }
1320 
1321  {
1322  tesseract_common::PluginInfo pi;
1323  pi.class_name = "KDLInvKin";
1324  kpi_insert.inv_plugin_infos["manipulator"].plugins = { std::make_pair("KDLInvKin", pi) };
1325  kpi_insert.inv_plugin_infos["manipulator"].default_plugin = "KDLInvKin";
1326  }
1327 
1328  EXPECT_FALSE(kpi_insert.empty());
1329 
1330  EXPECT_NE(kpi, kpi_insert);
1331  kpi.insert(kpi_insert);
1332  EXPECT_FALSE(kpi.empty());
1333  EXPECT_EQ(kpi, kpi_insert);
1334 
1335  kpi.clear();
1336  EXPECT_TRUE(kpi.empty());
1337 }
1338 
1339 TEST(TesseractCommonUnit, ContactManagersPluginInfoUnit) // NOLINT
1340 {
1341  tesseract_common::ContactManagersPluginInfo cmpi;
1342  EXPECT_TRUE(cmpi.empty());
1343 
1344  tesseract_common::ContactManagersPluginInfo cmpi_insert;
1345  cmpi_insert.search_paths.insert("/usr/local/lib");
1346  cmpi_insert.search_libraries.insert("tesseract_collision");
1347 
1348  {
1349  tesseract_common::PluginInfo pi;
1350  pi.class_name = "DiscretePluginFactory";
1351  cmpi_insert.discrete_plugin_infos.plugins = { std::make_pair("DiscretePlugin", pi) };
1352  cmpi_insert.discrete_plugin_infos.default_plugin = "DiscretePlugin";
1353  }
1354 
1355  {
1356  tesseract_common::PluginInfo pi;
1357  pi.class_name = "ContinuousPluginFactory";
1358  cmpi_insert.continuous_plugin_infos.plugins = { std::make_pair("ContinuousPlugin", pi) };
1359  cmpi_insert.continuous_plugin_infos.default_plugin = "ContinuousPlugin";
1360  }
1361 
1362  EXPECT_FALSE(cmpi_insert.empty());
1363 
1364  EXPECT_NE(cmpi, cmpi_insert);
1365  cmpi.insert(cmpi_insert);
1366  EXPECT_FALSE(cmpi.empty());
1367  EXPECT_EQ(cmpi, cmpi_insert);
1368 
1369  cmpi.clear();
1370  EXPECT_TRUE(cmpi.empty());
1371 }
1372 
1373 TEST(TesseractCommonUnit, TaskComposerPluginInfoUnit) // NOLINT
1374 {
1375  tesseract_common::TaskComposerPluginInfo tcpi;
1376  EXPECT_TRUE(tcpi.empty());
1377 
1378  tesseract_common::TaskComposerPluginInfo tcpi_insert;
1379  tcpi_insert.search_paths.insert("/usr/local/lib");
1380  tcpi_insert.search_libraries.insert("tesseract_collision");
1381 
1382  {
1383  tesseract_common::PluginInfo pi;
1384  pi.class_name = "TaskComposerExecutorPluginFactory";
1385  tcpi_insert.executor_plugin_infos.plugins = { std::make_pair("TaskComposerExecutorPlugin", pi) };
1386  tcpi_insert.executor_plugin_infos.default_plugin = "TaskComposerExecutorPlugin";
1387  }
1388 
1389  {
1390  tesseract_common::PluginInfo pi;
1391  pi.class_name = "TaskComposerNodePluginFactory";
1392  tcpi_insert.task_plugin_infos.plugins = { std::make_pair("TaskComposerNodePlugin", pi) };
1393  tcpi_insert.task_plugin_infos.default_plugin = "TaskComposerNodePlugin";
1394  }
1395 
1396  EXPECT_FALSE(tcpi_insert.empty());
1397 
1398  EXPECT_NE(tcpi, tcpi_insert);
1399  tcpi.insert(tcpi_insert);
1400  EXPECT_FALSE(tcpi.empty());
1401  EXPECT_EQ(tcpi, tcpi_insert);
1402 
1403  tcpi.clear();
1404  EXPECT_TRUE(tcpi.empty());
1405 }
1406 
1407 TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT
1408 {
1409  std::string yaml_string = R"(kinematic_plugins:
1410  search_paths:
1411  - /usr/local/lib
1412  search_libraries:
1413  - tesseract_kinematics_kdl_factories
1414  fwd_kin_plugins:
1415  iiwa_manipulator:
1416  default: KDLFwdKinChain
1417  plugins:
1418  KDLFwdKinChain:
1419  class: KDLFwdKinChainFactory
1420  config:
1421  base_link: base_link
1422  tip_link: tool0
1423  inv_kin_plugins:
1424  iiwa_manipulator:
1425  default: KDLInvKinChainLMA
1426  plugins:
1427  KDLInvKinChainLMA:
1428  class: KDLInvKinChainLMAFactory
1429  config:
1430  base_link: base_link
1431  tip_link: tool0
1432  KDLInvKinChainNR:
1433  class: KDLInvKinChainNRFactory
1434  config:
1435  base_link: base_link
1436  tip_link: tool0)";
1437 
1438  { // Success
1439  tesseract_common::GeneralResourceLocator locator;
1440  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1441  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1442  auto cmpi = config.as<tesseract_common::KinematicsPluginInfo>();
1443 
1444  const YAML::Node& plugin_info = plugin_config["kinematic_plugins"];
1445  const YAML::Node& search_paths = plugin_info["search_paths"];
1446  const YAML::Node& search_libraries = plugin_info["search_libraries"];
1447  const YAML::Node& fwd_kin_default_plugin = plugin_info["fwd_kin_plugins"]["iiwa_manipulator"]["default"];
1448  const YAML::Node& fwd_kin_plugins = plugin_info["fwd_kin_plugins"]["iiwa_manipulator"]["plugins"];
1449  const YAML::Node& inv_kin_default_plugin = plugin_info["inv_kin_plugins"]["iiwa_manipulator"]["default"];
1450  const YAML::Node& inv_kin_plugins = plugin_info["inv_kin_plugins"]["iiwa_manipulator"]["plugins"];
1451 
1452  {
1453  std::set<std::string> sp = cmpi.search_paths;
1454  EXPECT_EQ(sp.size(), 1);
1455 
1456  for (auto it = search_paths.begin(); it != search_paths.end(); ++it)
1457  {
1458  EXPECT_TRUE(sp.find(it->as<std::string>()) != sp.end());
1459  }
1460  }
1461 
1462  {
1463  std::set<std::string> sl = cmpi.search_libraries;
1464  EXPECT_EQ(sl.size(), 1);
1465 
1466  for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it)
1467  {
1468  EXPECT_TRUE(sl.find(it->as<std::string>()) != sl.end());
1469  }
1470  }
1471 
1472  EXPECT_EQ(fwd_kin_default_plugin.as<std::string>(), cmpi.fwd_plugin_infos["iiwa_manipulator"].default_plugin);
1473  EXPECT_EQ(fwd_kin_plugins.size(), cmpi.fwd_plugin_infos["iiwa_manipulator"].plugins.size());
1474 
1475  EXPECT_EQ(inv_kin_default_plugin.as<std::string>(), cmpi.inv_plugin_infos["iiwa_manipulator"].default_plugin);
1476  EXPECT_EQ(inv_kin_plugins.size(), cmpi.inv_plugin_infos["iiwa_manipulator"].plugins.size());
1477  }
1478 
1479  { // search_paths failure
1480  std::string yaml_string = R"(kinematic_plugins:
1481  search_paths:
1482  failure: issue
1483  search_libraries:
1484  - tesseract_kinematics_kdl_factories
1485  fwd_kin_plugins:
1486  iiwa_manipulator:
1487  default: KDLFwdKinChain
1488  plugins:
1489  KDLFwdKinChain:
1490  class: KDLFwdKinChainFactory
1491  config:
1492  base_link: base_link
1493  tip_link: tool0
1494  inv_kin_plugins:
1495  iiwa_manipulator:
1496  default: KDLInvKinChainLMA
1497  plugins:
1498  KDLInvKinChainLMA:
1499  class: KDLInvKinChainLMAFactory
1500  config:
1501  base_link: base_link
1502  tip_link: tool0
1503  KDLInvKinChainNR:
1504  class: KDLInvKinChainNRFactory
1505  config:
1506  base_link: base_link
1507  tip_link: tool0)";
1508 
1509  tesseract_common::GeneralResourceLocator locator;
1510  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1511  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1512  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1513  }
1514 
1515  { // search_libraries failure
1516  std::string yaml_string = R"(kinematic_plugins:
1517  search_paths:
1518  - /usr/local/lib
1519  search_libraries:
1520  failure: issue
1521  fwd_kin_plugins:
1522  iiwa_manipulator:
1523  default: KDLFwdKinChain
1524  plugins:
1525  KDLFwdKinChain:
1526  class: KDLFwdKinChainFactory
1527  config:
1528  base_link: base_link
1529  tip_link: tool0
1530  inv_kin_plugins:
1531  iiwa_manipulator:
1532  default: KDLInvKinChainLMA
1533  plugins:
1534  KDLInvKinChainLMA:
1535  class: KDLInvKinChainLMAFactory
1536  config:
1537  base_link: base_link
1538  tip_link: tool0
1539  KDLInvKinChainNR:
1540  class: KDLInvKinChainNRFactory
1541  config:
1542  base_link: base_link
1543  tip_link: tool0)";
1544 
1545  tesseract_common::GeneralResourceLocator locator;
1546  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1547  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1548  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1549  }
1550 
1551  { // missing fwd plugins failure
1552  std::string yaml_string = R"(kinematic_plugins:
1553  search_paths:
1554  - /usr/local/lib
1555  search_libraries:
1556  - tesseract_kinematics_kdl_factories
1557  fwd_kin_plugins:
1558  iiwa_manipulator:
1559  default: KDLFwdKinChain
1560  inv_kin_plugins:
1561  iiwa_manipulator:
1562  default: KDLInvKinChainLMA
1563  plugins:
1564  KDLInvKinChainLMA:
1565  class: KDLInvKinChainLMAFactory
1566  config:
1567  base_link: base_link
1568  tip_link: tool0
1569  KDLInvKinChainNR:
1570  class: KDLInvKinChainNRFactory
1571  config:
1572  base_link: base_link
1573  tip_link: tool0)";
1574 
1575  tesseract_common::GeneralResourceLocator locator;
1576  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1577  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1578  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1579  }
1580 
1581  { // fwd plugins is not map failure
1582 
1583  std::string yaml_string = R"(kinematic_plugins:
1584  search_paths:
1585  - /usr/local/lib
1586  search_libraries:
1587  - tesseract_kinematics_kdl_factories
1588  fwd_kin_plugins:
1589  iiwa_manipulator:
1590  - tesseract_collision_bullet_factories
1591  - tesseract_collision_fcl_factories
1592  inv_kin_plugins:
1593  iiwa_manipulator:
1594  default: KDLInvKinChainLMA
1595  plugins:
1596  KDLInvKinChainLMA:
1597  class: KDLInvKinChainLMAFactory
1598  config:
1599  base_link: base_link
1600  tip_link: tool0
1601  KDLInvKinChainNR:
1602  class: KDLInvKinChainNRFactory
1603  config:
1604  base_link: base_link
1605  tip_link: tool0)";
1606 
1607  tesseract_common::GeneralResourceLocator locator;
1608  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1609  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1610  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1611  }
1612 
1613  { // missing inv plugins failure
1614  std::string yaml_string = R"(kinematic_plugins:
1615  search_paths:
1616  - /usr/local/lib
1617  search_libraries:
1618  - tesseract_kinematics_kdl_factories
1619  fwd_kin_plugins:
1620  iiwa_manipulator:
1621  default: KDLFwdKinChain
1622  plugins:
1623  KDLFwdKinChain:
1624  class: KDLFwdKinChainFactory
1625  config:
1626  base_link: base_link
1627  tip_link: tool0
1628  inv_kin_plugins:
1629  iiwa_manipulator:
1630  default: KDLInvKinChainLMA)";
1631 
1632  tesseract_common::GeneralResourceLocator locator;
1633  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1634  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1635  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1636  }
1637 
1638  { // inv plugins is not map failure
1639  std::string yaml_string = R"(kinematic_plugins:
1640  search_paths:
1641  - /usr/local/lib
1642  search_libraries:
1643  - tesseract_kinematics_kdl_factories
1644  fwd_kin_plugins:
1645  iiwa_manipulator:
1646  default: KDLFwdKinChain
1647  plugins:
1648  KDLFwdKinChain:
1649  class: KDLFwdKinChainFactory
1650  config:
1651  base_link: base_link
1652  tip_link: tool0
1653  inv_kin_plugins:
1654  iiwa_manipulator:
1655  - tesseract_collision_bullet_factories
1656  - tesseract_collision_fcl_factories)";
1657 
1658  tesseract_common::GeneralResourceLocator locator;
1659  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1660  YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY];
1661  EXPECT_ANY_THROW(config.as<tesseract_common::KinematicsPluginInfo>()); // NOLINT
1662  }
1663 }
1664 
1665 TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT
1666 {
1667  std::string yaml_string = R"(contact_manager_plugins:
1668  search_paths:
1669  - /usr/local/lib
1670  search_libraries:
1671  - tesseract_collision_bullet_factories
1672  - tesseract_collision_fcl_factories
1673  discrete_plugins:
1674  default: BulletDiscreteBVHManager
1675  plugins:
1676  BulletDiscreteBVHManager:
1677  class: BulletDiscreteBVHManagerFactory
1678  BulletDiscreteSimpleManager:
1679  class: BulletDiscreteSimpleManagerFactory
1680  FCLDiscreteBVHManager:
1681  class: FCLDiscreteBVHManagerFactory
1682  continuous_plugins:
1683  default: BulletCastBVHManager
1684  plugins:
1685  BulletCastBVHManager:
1686  class: BulletCastBVHManagerFactory
1687  BulletCastSimpleManager:
1688  class: BulletCastSimpleManagerFactory)";
1689 
1690  { // Success
1691  tesseract_common::GeneralResourceLocator locator;
1692  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1693  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1694  auto cmpi = config.as<tesseract_common::ContactManagersPluginInfo>();
1695 
1696  const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"];
1697  const YAML::Node& search_paths = plugin_info["search_paths"];
1698  const YAML::Node& search_libraries = plugin_info["search_libraries"];
1699  const YAML::Node& discrete_default_plugin = plugin_info["discrete_plugins"]["default"];
1700  const YAML::Node& discrete_plugins = plugin_info["discrete_plugins"]["plugins"];
1701  const YAML::Node& continuous_default_plugin = plugin_info["continuous_plugins"]["default"];
1702  const YAML::Node& continuous_plugins = plugin_info["continuous_plugins"]["plugins"];
1703 
1704  {
1705  std::set<std::string> sp = cmpi.search_paths;
1706  EXPECT_EQ(sp.size(), 1);
1707 
1708  for (auto it = search_paths.begin(); it != search_paths.end(); ++it)
1709  {
1710  EXPECT_TRUE(sp.find(it->as<std::string>()) != sp.end());
1711  }
1712  }
1713 
1714  {
1715  std::set<std::string> sl = cmpi.search_libraries;
1716  EXPECT_EQ(sl.size(), 2);
1717 
1718  for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it)
1719  {
1720  EXPECT_TRUE(sl.find(it->as<std::string>()) != sl.end());
1721  }
1722  }
1723 
1724  EXPECT_EQ(discrete_default_plugin.as<std::string>(), cmpi.discrete_plugin_infos.default_plugin);
1725  EXPECT_EQ(discrete_plugins.size(), cmpi.discrete_plugin_infos.plugins.size());
1726 
1727  EXPECT_EQ(continuous_default_plugin.as<std::string>(), cmpi.continuous_plugin_infos.default_plugin);
1728  EXPECT_EQ(continuous_plugins.size(), cmpi.continuous_plugin_infos.plugins.size());
1729  }
1730 
1731  { // search_paths failure
1732  std::string yaml_string = R"(contact_manager_plugins:
1733  search_paths:
1734  failure: issue
1735  search_libraries:
1736  - tesseract_collision_bullet_factories
1737  - tesseract_collision_fcl_factories
1738  discrete_plugins:
1739  default: BulletDiscreteBVHManager
1740  plugins:
1741  BulletDiscreteBVHManager:
1742  class: BulletDiscreteBVHManagerFactory
1743  BulletDiscreteSimpleManager:
1744  class: BulletDiscreteSimpleManagerFactory
1745  FCLDiscreteBVHManager:
1746  class: FCLDiscreteBVHManagerFactory
1747  continuous_plugins:
1748  default: BulletCastBVHManager
1749  plugins:
1750  BulletCastBVHManager:
1751  class: BulletCastBVHManagerFactory
1752  BulletCastSimpleManager:
1753  class: BulletCastSimpleManagerFactory)";
1754  tesseract_common::GeneralResourceLocator locator;
1755  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1756  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1757  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1758  }
1759 
1760  { // search_libraries failure
1761  std::string yaml_string = R"(contact_manager_plugins:
1762  search_paths:
1763  - /usr/local/lib
1764  search_libraries:
1765  failure: issue
1766  discrete_plugins:
1767  default: BulletDiscreteBVHManager
1768  plugins:
1769  BulletDiscreteBVHManager:
1770  class: BulletDiscreteBVHManagerFactory
1771  BulletDiscreteSimpleManager:
1772  class: BulletDiscreteSimpleManagerFactory
1773  FCLDiscreteBVHManager:
1774  class: FCLDiscreteBVHManagerFactory
1775  continuous_plugins:
1776  default: BulletCastBVHManager
1777  plugins:
1778  BulletCastBVHManager:
1779  class: BulletCastBVHManagerFactory
1780  BulletCastSimpleManager:
1781  class: BulletCastSimpleManagerFactory)";
1782  tesseract_common::GeneralResourceLocator locator;
1783  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1784  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1785  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1786  }
1787 
1788  { // missing discrete plugins failure
1789  std::string yaml_string = R"(contact_manager_plugins:
1790  search_paths:
1791  - /usr/local/lib
1792  search_libraries:
1793  - tesseract_collision_bullet_factories
1794  - tesseract_collision_fcl_factories
1795  discrete_plugins:
1796  default: BulletDiscreteBVHManager
1797  continuous_plugins:
1798  default: BulletCastBVHManager
1799  plugins:
1800  BulletCastBVHManager:
1801  class: BulletCastBVHManagerFactory
1802  BulletCastSimpleManager:
1803  class: BulletCastSimpleManagerFactory)";
1804  tesseract_common::GeneralResourceLocator locator;
1805  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1806  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1807  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1808  }
1809 
1810  { // discrete plugins is not map failure
1811  std::string yaml_string = R"(contact_manager_plugins:
1812  search_paths:
1813  - /usr/local/lib
1814  search_libraries:
1815  - tesseract_collision_bullet_factories
1816  - tesseract_collision_fcl_factories
1817  discrete_plugins:
1818  - tesseract_collision_bullet_factories
1819  - tesseract_collision_fcl_factories
1820  continuous_plugins:
1821  default: BulletCastBVHManager
1822  plugins:
1823  BulletCastBVHManager:
1824  class: BulletCastBVHManagerFactory
1825  BulletCastSimpleManager:
1826  class: BulletCastSimpleManagerFactory)";
1827  tesseract_common::GeneralResourceLocator locator;
1828  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1829  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1830  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1831  }
1832 
1833  { // missing continuous plugins failure
1834  std::string yaml_string = R"(contact_manager_plugins:
1835  search_paths:
1836  - /usr/local/lib
1837  search_libraries:
1838  - tesseract_collision_bullet_factories
1839  - tesseract_collision_fcl_factories
1840  discrete_plugins:
1841  default: BulletDiscreteBVHManager
1842  plugins:
1843  BulletDiscreteBVHManager:
1844  class: BulletDiscreteBVHManagerFactory
1845  BulletDiscreteSimpleManager:
1846  class: BulletDiscreteSimpleManagerFactory
1847  FCLDiscreteBVHManager:
1848  class: FCLDiscreteBVHManagerFactory
1849  continuous_plugins:
1850  default: BulletCastBVHManager)";
1851  tesseract_common::GeneralResourceLocator locator;
1852  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1853  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1854  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1855  }
1856 
1857  { // continuous plugins is not map failure
1858  std::string yaml_string = R"(contact_manager_plugins:
1859  search_paths:
1860  - /usr/local/lib
1861  search_libraries:
1862  - tesseract_collision_bullet_factories
1863  - tesseract_collision_fcl_factories
1864  discrete_plugins:
1865  default: BulletDiscreteBVHManager
1866  plugins:
1867  BulletDiscreteBVHManager:
1868  class: BulletDiscreteBVHManagerFactory
1869  BulletDiscreteSimpleManager:
1870  class: BulletDiscreteSimpleManagerFactory
1871  FCLDiscreteBVHManager:
1872  class: FCLDiscreteBVHManagerFactory
1873  continuous_plugins:
1874  - tesseract_collision_bullet_factories
1875  - tesseract_collision_fcl_factories)";
1876  tesseract_common::GeneralResourceLocator locator;
1877  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1878  YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY];
1879  EXPECT_ANY_THROW(config.as<tesseract_common::ContactManagersPluginInfo>()); // NOLINT
1880  }
1881 }
1882 
1883 TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT
1884 {
1885  std::string yaml_string = R"(task_composer_plugins:
1886  search_paths:
1887  - /usr/local/lib
1888  search_libraries:
1889  - tesseract_task_composer_executor_factories
1890  - tesseract_task_composer_node_factories
1891  executors:
1892  default: TaskflowTaskComposerExecutor
1893  plugins:
1894  TaskflowTaskComposerExecutor:
1895  class: TaskflowTaskComposerExecutorFactory
1896  config:
1897  threads: 5
1898  TaskflowTaskComposerExecutor2:
1899  class: TaskflowTaskComposerExecutorFactory
1900  config:
1901  threads: 10
1902  TaskflowTaskComposerExecutor3:
1903  class: TaskflowTaskComposerExecutorFactory
1904  config:
1905  threads: 15
1906  tasks:
1907  default: CartesianMotionPipeline
1908  plugins:
1909  CartesianMotionPipeline:
1910  class: CartesianMotionPipelineFactory
1911  config:
1912  input_key: "input"
1913  output_key: "output"
1914  FreespaceMotionPipeline:
1915  class: FreespaceMotionPipelineFactory
1916  config:
1917  input_key: "input"
1918  output_key: "output")";
1919 
1920  { // Success
1921  tesseract_common::GeneralResourceLocator locator;
1922  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1923  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
1924  auto tcpi = config.as<tesseract_common::TaskComposerPluginInfo>();
1925 
1926  const YAML::Node& plugin_info = plugin_config["task_composer_plugins"];
1927  const YAML::Node& search_paths = plugin_info["search_paths"];
1928  const YAML::Node& search_libraries = plugin_info["search_libraries"];
1929  const YAML::Node& executor_default_plugin = plugin_info["executors"]["default"];
1930  const YAML::Node& executor_plugins = plugin_info["executors"]["plugins"];
1931  const YAML::Node& task_default_plugin = plugin_info["tasks"]["default"];
1932  const YAML::Node& task_plugins = plugin_info["tasks"]["plugins"];
1933 
1934  {
1935  std::set<std::string> sp = tcpi.search_paths;
1936  EXPECT_EQ(sp.size(), 1);
1937 
1938  for (auto it = search_paths.begin(); it != search_paths.end(); ++it)
1939  {
1940  EXPECT_TRUE(sp.find(it->as<std::string>()) != sp.end());
1941  }
1942  }
1943 
1944  {
1945  std::set<std::string> sl = tcpi.search_libraries;
1946  EXPECT_EQ(sl.size(), 2);
1947 
1948  for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it)
1949  {
1950  EXPECT_TRUE(sl.find(it->as<std::string>()) != sl.end());
1951  }
1952  }
1953 
1954  EXPECT_EQ(executor_default_plugin.as<std::string>(), tcpi.executor_plugin_infos.default_plugin);
1955  EXPECT_EQ(executor_plugins.size(), tcpi.executor_plugin_infos.plugins.size());
1956 
1957  EXPECT_EQ(task_default_plugin.as<std::string>(), tcpi.task_plugin_infos.default_plugin);
1958  EXPECT_EQ(task_plugins.size(), tcpi.task_plugin_infos.plugins.size());
1959  }
1960 
1961  { // search_paths failure
1962  std::string yaml_string = R"(task_composer_plugins:
1963  search_paths:
1964  failure: issue
1965  search_libraries:
1966  - tesseract_task_composer_executor_factories
1967  - tesseract_task_composer_node_factories
1968  executors:
1969  default: TaskflowTaskComposerExecutor
1970  plugins:
1971  TaskflowTaskComposerExecutor:
1972  class: TaskflowTaskComposerExecutorFactory
1973  config:
1974  threads: 5
1975  TaskflowTaskComposerExecutor2:
1976  class: TaskflowTaskComposerExecutorFactory
1977  config:
1978  threads: 10
1979  TaskflowTaskComposerExecutor3:
1980  class: TaskflowTaskComposerExecutorFactory
1981  config:
1982  threads: 15
1983  tasks:
1984  default: CartesianMotionPipeline
1985  plugins:
1986  CartesianMotionPipeline:
1987  class: CartesianMotionPipelineFactory
1988  config:
1989  input_key: "input"
1990  output_key: "output"
1991  FreespaceMotionPipeline:
1992  class: FreespaceMotionPipelineFactory
1993  config:
1994  input_key: "input"
1995  output_key: "output")";
1996  tesseract_common::GeneralResourceLocator locator;
1997  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
1998  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
1999  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2000  }
2001 
2002  { // search_libraries failure
2003  std::string yaml_string = R"(task_composer_plugins:
2004  search_paths:
2005  - /usr/local/lib
2006  search_libraries:
2007  failure: issue
2008  executors:
2009  default: TaskflowTaskComposerExecutor
2010  plugins:
2011  TaskflowTaskComposerExecutor:
2012  class: TaskflowTaskComposerExecutorFactory
2013  config:
2014  threads: 5
2015  TaskflowTaskComposerExecutor2:
2016  class: TaskflowTaskComposerExecutorFactory
2017  config:
2018  threads: 10
2019  TaskflowTaskComposerExecutor3:
2020  class: TaskflowTaskComposerExecutorFactory
2021  config:
2022  threads: 15
2023  tasks:
2024  default: CartesianMotionPipeline
2025  plugins:
2026  CartesianMotionPipeline:
2027  class: CartesianMotionPipelineFactory
2028  config:
2029  input_key: "input"
2030  output_key: "output"
2031  FreespaceMotionPipeline:
2032  class: FreespaceMotionPipelineFactory
2033  config:
2034  input_key: "input"
2035  output_key: "output")";
2036  tesseract_common::GeneralResourceLocator locator;
2037  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
2038  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
2039  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2040  }
2041 
2042  { // missing executor plugins failure
2043  std::string yaml_string = R"(task_composer_plugins:
2044  search_paths:
2045  - /usr/local/lib
2046  search_libraries:
2047  - tesseract_task_composer_executor_factories
2048  - tesseract_task_composer_node_factories
2049  executors:
2050  default: TaskflowTaskComposerExecutor
2051  tasks:
2052  default: CartesianMotionPipeline
2053  plugins:
2054  CartesianMotionPipeline:
2055  class: CartesianMotionPipelineFactory
2056  config:
2057  input_key: "input"
2058  output_key: "output"
2059  FreespaceMotionPipeline:
2060  class: FreespaceMotionPipelineFactory
2061  config:
2062  input_key: "input"
2063  output_key: "output")";
2064  tesseract_common::GeneralResourceLocator locator;
2065  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
2066  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
2067  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2068  }
2069 
2070  { // executor plugins is not map failure
2071  std::string yaml_string = R"(task_composer_plugins:
2072  search_paths:
2073  - /usr/local/lib
2074  search_libraries:
2075  - tesseract_task_composer_executor_factories
2076  - tesseract_task_composer_node_factories
2077  executors:
2078  - TaskflowTaskComposerExecutor:
2079  class: TaskflowTaskComposerExecutorFactory
2080  config:
2081  threads: 5
2082  - TaskflowTaskComposerExecutor2:
2083  class: TaskflowTaskComposerExecutorFactory
2084  config:
2085  threads: 10
2086  - TaskflowTaskComposerExecutor3:
2087  class: TaskflowTaskComposerExecutorFactory
2088  config:
2089  threads: 15
2090  tasks:
2091  default: CartesianMotionPipeline
2092  plugins:
2093  CartesianMotionPipeline:
2094  class: CartesianMotionPipelineFactory
2095  config:
2096  input_key: "input"
2097  output_key: "output"
2098  FreespaceMotionPipeline:
2099  class: FreespaceMotionPipelineFactory
2100  config:
2101  input_key: "input"
2102  output_key: "output")";
2103  tesseract_common::GeneralResourceLocator locator;
2104  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
2105  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
2106  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2107  }
2108 
2109  { // missing node plugins failure
2110  std::string yaml_string = R"(task_composer_plugins:
2111  search_paths:
2112  - /usr/local/lib
2113  search_libraries:
2114  - tesseract_task_composer_executor_factories
2115  - tesseract_task_composer_node_factories
2116  executors:
2117  default: TaskflowTaskComposerExecutor
2118  plugins:
2119  TaskflowTaskComposerExecutor:
2120  class: TaskflowTaskComposerExecutorFactory
2121  config:
2122  threads: 5
2123  TaskflowTaskComposerExecutor2:
2124  class: TaskflowTaskComposerExecutorFactory
2125  config:
2126  threads: 10
2127  TaskflowTaskComposerExecutor3:
2128  class: TaskflowTaskComposerExecutorFactory
2129  config:
2130  threads: 15
2131  tasks:
2132  default: CartesianMotionPipeline)";
2133  tesseract_common::GeneralResourceLocator locator;
2134  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
2135  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
2136  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2137  }
2138 
2139  { // continuous plugins is not map failure
2140  std::string yaml_string = R"(task_composer_plugins:
2141  search_paths:
2142  - /usr/local/lib
2143  search_libraries:
2144  - tesseract_task_composer_executor_factories
2145  - tesseract_task_composer_node_factories
2146  executors:
2147  default: TaskflowTaskComposerExecutor
2148  plugins:
2149  TaskflowTaskComposerExecutor:
2150  class: TaskflowTaskComposerExecutorFactory
2151  config:
2152  threads: 5
2153  TaskflowTaskComposerExecutor2:
2154  class: TaskflowTaskComposerExecutorFactory
2155  config:
2156  threads: 10
2157  TaskflowTaskComposerExecutor3:
2158  class: TaskflowTaskComposerExecutorFactory
2159  config:
2160  threads: 15
2161  tasks:
2162  - CartesianMotionPipeline:
2163  class: CartesianMotionPipelineFactory
2164  config:
2165  input_key: "input"
2166  output_key: "output"
2167  - FreespaceMotionPipeline:
2168  class: FreespaceMotionPipelineFactory
2169  config:
2170  input_key: "input"
2171  output_key: "output")";
2172  tesseract_common::GeneralResourceLocator locator;
2173  YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator);
2174  YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY];
2175  EXPECT_ANY_THROW(config.as<tesseract_common::TaskComposerPluginInfo>()); // NOLINT
2176  }
2177 }
2178 
2179 TEST(TesseractCommonUnit, TransformMapYamlUnit) // NOLINT
2180 {
2181  std::string yaml_string =
2182  R"(joints:
2183  joint_1:
2184  position:
2185  x: 1
2186  y: 2
2187  z: 3
2188  orientation:
2189  x: 0
2190  y: 0
2191  z: 0
2192  w: 1
2193  joint_2:
2194  position:
2195  x: 4
2196  y: 5
2197  z: 6
2198  orientation:
2199  x: 0
2200  y: 0
2201  z: 0
2202  w: 1)";
2203 
2204  { // valid string
2205  tesseract_common::GeneralResourceLocator locator;
2206  YAML::Node node = tesseract_common::loadYamlString(yaml_string, locator);
2207  auto trans_map = node["joints"].as<tesseract_common::TransformMap>();
2208  EXPECT_EQ(trans_map.size(), 2);
2209  EXPECT_FALSE(trans_map.empty());
2210  EXPECT_TRUE(trans_map.find("joint_1") != trans_map.end());
2211  EXPECT_TRUE(trans_map.find("joint_2") != trans_map.end());
2212  }
2213 
2214  std::string bad_yaml_string =
2215  R"(joints:
2216  - joint_1:
2217  position:
2218  x: 1
2219  y: 2
2220  z: 3
2221  orientation:
2222  x: 0
2223  y: 0
2224  z: 0
2225  w: 1
2226  - joint_2:
2227  position:
2228  x: 4
2229  y: 5
2230  z: 6
2231  orientation:
2232  x: 0
2233  y: 0
2234  z: 0
2235  w: 1)";
2236  { // invalid string
2237  tesseract_common::GeneralResourceLocator locator;
2238  YAML::Node node = tesseract_common::loadYamlString(bad_yaml_string, locator);
2239  EXPECT_ANY_THROW(node["joints"].as<tesseract_common::TransformMap>()); // NOLINT
2240  }
2241 }
2242 
2243 TEST(TesseractCommonUnit, CalibrationInfoYamlUnit) // NOLINT
2244 {
2245  std::string yaml_string =
2246  R"(calibration:
2247  joints:
2248  joint_1:
2249  position:
2250  x: 1
2251  y: 2
2252  z: 3
2253  orientation:
2254  x: 0
2255  y: 0
2256  z: 0
2257  w: 1
2258  joint_2:
2259  position:
2260  x: 4
2261  y: 5
2262  z: 6
2263  orientation:
2264  x: 0
2265  y: 0
2266  z: 0
2267  w: 1)";
2268 
2269  tesseract_common::GeneralResourceLocator locator;
2270  YAML::Node node = tesseract_common::loadYamlString(yaml_string, locator);
2271  auto cal_info = node[tesseract_common::CalibrationInfo::CONFIG_KEY].as<tesseract_common::CalibrationInfo>();
2272  EXPECT_FALSE(cal_info.empty());
2273  EXPECT_TRUE(cal_info.joints.find("joint_1") != cal_info.joints.end());
2274  EXPECT_TRUE(cal_info.joints.find("joint_2") != cal_info.joints.end());
2275 
2276  tesseract_common::CalibrationInfo cal_insert;
2277  EXPECT_TRUE(cal_insert.empty());
2278  cal_insert.insert(cal_info);
2279  EXPECT_FALSE(cal_insert.empty());
2280  EXPECT_TRUE(cal_insert.joints.find("joint_1") != cal_insert.joints.end());
2281  EXPECT_TRUE(cal_insert.joints.find("joint_2") != cal_insert.joints.end());
2282 
2283  cal_info.clear();
2284  EXPECT_TRUE(cal_info.empty());
2285 }
2286 
2287 TEST(TesseractCommonUnit, linkNamesPairUnit) // NOLINT
2288 {
2289  {
2290  tesseract_common::LinkNamesPair p1 = tesseract_common::makeOrderedLinkPair("link_1", "link_2");
2291  tesseract_common::LinkNamesPair p2 = tesseract_common::makeOrderedLinkPair("link_2", "link_1");
2292 
2293  EXPECT_EQ(p1.first, p2.first);
2294  EXPECT_EQ(p1.second, p2.second);
2295 
2296  tesseract_common::PairHash hash;
2297  EXPECT_EQ(hash(p1), hash(p2));
2298  }
2299 
2300  {
2301  tesseract_common::LinkNamesPair p1;
2302  tesseract_common::makeOrderedLinkPair(p1, "link_1", "link_2");
2303  tesseract_common::LinkNamesPair p2;
2304  tesseract_common::makeOrderedLinkPair(p2, "link_2", "link_1");
2305 
2306  EXPECT_EQ(p1.first, p2.first);
2307  EXPECT_EQ(p1.second, p2.second);
2308 
2309  tesseract_common::PairHash hash;
2310  EXPECT_EQ(hash(p1), hash(p2));
2311  }
2312 
2313  {
2314  tesseract_common::LinkNamesPair p1 = tesseract_common::makeOrderedLinkPair("link_1", "link_2");
2315  tesseract_common::LinkNamesPair p2 = tesseract_common::makeOrderedLinkPair("link_2", "link_1");
2316 
2317  tesseract_common::LinkNamesPair mp1;
2318  tesseract_common::makeOrderedLinkPair(mp1, "link_1", "link_2");
2319  tesseract_common::LinkNamesPair mp2;
2320  tesseract_common::makeOrderedLinkPair(mp2, "link_2", "link_1");
2321 
2322  EXPECT_EQ(p1.first, mp1.first);
2323  EXPECT_EQ(p1.second, mp1.second);
2324  EXPECT_EQ(p2.first, mp2.first);
2325  EXPECT_EQ(p2.second, mp2.second);
2326  }
2327 }
2328 
2330 TEST(TesseractCommonUnit, calcRotationalError) // NOLINT
2331 {
2332  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
2333  Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI - 0.0001, Eigen::Vector3d::UnitZ());
2334  Eigen::Vector3d rot_err = tesseract_common::calcRotationalError(pi_rot.rotation());
2335  EXPECT_NEAR(rot_err.norm(), M_PI - 0.0001, 1e-6);
2336  EXPECT_TRUE(rot_err.normalized().isApprox(Eigen::Vector3d::UnitZ(), 1e-6));
2337 
2338  pi_rot = identity * Eigen::AngleAxisd(-M_PI + 0.0001, Eigen::Vector3d::UnitZ());
2339  rot_err = tesseract_common::calcRotationalError(pi_rot.rotation());
2340  EXPECT_NEAR(rot_err.norm(), M_PI - 0.0001, 1e-6);
2341  EXPECT_TRUE(rot_err.normalized().isApprox(-Eigen::Vector3d::UnitZ(), 1e-6));
2342 
2343  // Test greater than PI
2344  pi_rot = identity * Eigen::AngleAxisd(3 * M_PI_2, Eigen::Vector3d::UnitZ());
2345  rot_err = tesseract_common::calcRotationalError(pi_rot.rotation());
2346  EXPECT_NEAR(rot_err.norm(), M_PI_2, 1e-6);
2347  EXPECT_TRUE(rot_err.normalized().isApprox(-Eigen::Vector3d::UnitZ(), 1e-6));
2348 
2349  // Test less than than -PI
2350  pi_rot = identity * Eigen::AngleAxisd(-3 * M_PI_2, Eigen::Vector3d::UnitZ());
2351  rot_err = tesseract_common::calcRotationalError(pi_rot.rotation());
2352  EXPECT_NEAR(rot_err.norm(), M_PI_2, 1e-6);
2353  EXPECT_TRUE(rot_err.normalized().isApprox(Eigen::Vector3d::UnitZ(), 1e-6));
2354 
2355  // Test for angle between [0, PI]
2356  Eigen::Isometry3d pi_rot_plus = identity * Eigen::AngleAxisd(M_PI_2 + 0.001, Eigen::Vector3d::UnitZ());
2357  Eigen::Isometry3d pi_rot_minus = identity * Eigen::AngleAxisd(M_PI_2 - 0.001, Eigen::Vector3d::UnitZ());
2358  Eigen::Vector3d pi_rot_delta = tesseract_common::calcRotationalError(pi_rot_plus.rotation()) -
2359  tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2360  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2361 
2362  // Test for angle between [-PI, 0]
2363  pi_rot_plus = identity * Eigen::AngleAxisd(-M_PI_2 + 0.001, Eigen::Vector3d::UnitZ());
2364  pi_rot_minus = identity * Eigen::AngleAxisd(-M_PI_2 - 0.001, Eigen::Vector3d::UnitZ());
2365  pi_rot_delta = tesseract_common::calcRotationalError(pi_rot_plus.rotation()) -
2366  tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2367  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2368 
2369  // Test for angle at 0
2370  pi_rot_plus = identity * Eigen::AngleAxisd(0.001, Eigen::Vector3d::UnitZ());
2371  pi_rot_minus = identity * Eigen::AngleAxisd(-0.001, Eigen::Vector3d::UnitZ());
2372  pi_rot_delta = tesseract_common::calcRotationalError(pi_rot_plus.rotation()) -
2373  tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2374  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2375 
2376  // Test for angle at PI
2377  pi_rot_plus = identity * Eigen::AngleAxisd(M_PI + 0.001, Eigen::Vector3d::UnitZ());
2378  pi_rot_minus = identity * Eigen::AngleAxisd(M_PI - 0.001, Eigen::Vector3d::UnitZ());
2379  pi_rot_delta = tesseract_common::calcRotationalError(pi_rot_plus.rotation()) -
2380  tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2381  EXPECT_TRUE(pi_rot_delta.norm() > M_PI); // This is because calcRotationalError breaks down at PI or -PI
2382 
2383  // Test for angle at -PI
2384  pi_rot_plus = identity * Eigen::AngleAxisd(-M_PI + 0.001, Eigen::Vector3d::UnitZ());
2385  pi_rot_minus = identity * Eigen::AngleAxisd(-M_PI - 0.001, Eigen::Vector3d::UnitZ());
2386  pi_rot_delta = tesseract_common::calcRotationalError(pi_rot_plus.rotation()) -
2387  tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2388  EXPECT_TRUE(pi_rot_delta.norm() > M_PI); // This is because calcRotationalError breaks down at PI or -PI
2389 
2390  // Test random axis
2391  for (int i = 0; i < 100; i++)
2392  {
2393  Eigen::Vector3d axis = Eigen::Vector3d::Random().normalized();
2394 
2395  // Avoid M_PI angle because this breaks down
2396  Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(100, -M_PI + 0.005, M_PI - 0.005);
2397  for (Eigen::Index j = 0; j < angles.rows(); j++)
2398  {
2399  pi_rot_plus = identity * Eigen::AngleAxisd(angles(j) + 0.001, axis);
2400  pi_rot_minus = identity * Eigen::AngleAxisd(angles(j) - 0.001, axis);
2401  Eigen::Vector3d e1 = tesseract_common::calcRotationalError(pi_rot_plus.rotation());
2402  Eigen::Vector3d e2 = tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2403  EXPECT_FALSE((e1.norm() < -M_PI));
2404  EXPECT_FALSE((e1.norm() > M_PI));
2405  EXPECT_FALSE((e2.norm() < -M_PI));
2406  EXPECT_FALSE((e2.norm() > M_PI));
2407  pi_rot_delta = e1 - e2;
2408  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2409  }
2410 
2411  // Avoid M_PI angle because this breaks down
2412  angles = Eigen::VectorXd::LinSpaced(100, M_PI + 0.005, 2 * M_PI);
2413  for (Eigen::Index j = 0; j < angles.rows(); j++)
2414  {
2415  pi_rot_plus = identity * Eigen::AngleAxisd(angles(j) + 0.001, axis);
2416  pi_rot_minus = identity * Eigen::AngleAxisd(angles(j) - 0.001, axis);
2417  Eigen::Vector3d e1 = tesseract_common::calcRotationalError(pi_rot_plus.rotation());
2418  Eigen::Vector3d e2 = tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2419  EXPECT_FALSE((e1.norm() < -M_PI));
2420  EXPECT_FALSE((e1.norm() > M_PI));
2421  EXPECT_FALSE((e2.norm() < -M_PI));
2422  EXPECT_FALSE((e2.norm() > M_PI));
2423  pi_rot_delta = e1 - e2;
2424  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2425  }
2426 
2427  // Avoid M_PI angle because this breaks down
2428  angles = Eigen::VectorXd::LinSpaced(100, -M_PI - 0.005, -2 * M_PI);
2429  for (Eigen::Index j = 0; j < angles.rows(); j++)
2430  {
2431  pi_rot_plus = identity * Eigen::AngleAxisd(angles(j) + 0.001, axis);
2432  pi_rot_minus = identity * Eigen::AngleAxisd(angles(j) - 0.001, axis);
2433  Eigen::Vector3d e1 = tesseract_common::calcRotationalError(pi_rot_plus.rotation());
2434  Eigen::Vector3d e2 = tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2435  EXPECT_FALSE((e1.norm() < -M_PI));
2436  EXPECT_FALSE((e1.norm() > M_PI));
2437  EXPECT_FALSE((e2.norm() < -M_PI));
2438  EXPECT_FALSE((e2.norm() > M_PI));
2439  pi_rot_delta = e1 - e2;
2440  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
2441  }
2442 
2443  // These should fail
2444  {
2445  pi_rot_plus = identity * Eigen::AngleAxisd(M_PI + 0.001, axis);
2446  pi_rot_minus = identity * Eigen::AngleAxisd(M_PI - 0.001, axis);
2447  Eigen::Vector3d e1 = tesseract_common::calcRotationalError(pi_rot_plus.rotation());
2448  Eigen::Vector3d e2 = tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2449  EXPECT_FALSE((e1.norm() < -M_PI));
2450  EXPECT_FALSE((e1.norm() > M_PI));
2451  EXPECT_FALSE((e2.norm() < -M_PI));
2452  EXPECT_FALSE((e2.norm() > M_PI));
2453  pi_rot_delta = e1 - e2;
2454  EXPECT_TRUE(pi_rot_delta.norm() > M_PI); // This is because calcRotationalError breaks down at PI or -PI
2455  }
2456  {
2457  pi_rot_plus = identity * Eigen::AngleAxisd(-M_PI + 0.001, axis);
2458  pi_rot_minus = identity * Eigen::AngleAxisd(-M_PI - 0.001, axis);
2459  Eigen::Vector3d e1 = tesseract_common::calcRotationalError(pi_rot_plus.rotation());
2460  Eigen::Vector3d e2 = tesseract_common::calcRotationalError(pi_rot_minus.rotation());
2461  EXPECT_FALSE((e1.norm() < -M_PI));
2462  EXPECT_FALSE((e1.norm() > M_PI));
2463  EXPECT_FALSE((e2.norm() < -M_PI));
2464  EXPECT_FALSE((e2.norm() > M_PI));
2465  pi_rot_delta = e1 - e2;
2466  EXPECT_TRUE(pi_rot_delta.norm() > M_PI); // This is because calcRotationalError breaks down at PI or -PI
2467  }
2468  }
2469 }
2470 
2472 TEST(TesseractCommonUnit, calcTransformError) // NOLINT
2473 {
2474  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
2475 
2476  { // X-Axis
2477  Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX());
2478  Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot);
2479  EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
2480  EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(M_PI_2, 0, 0)));
2481  }
2482 
2483  { // Y-Axis
2484  Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY());
2485  Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot);
2486  EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
2487  EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, M_PI_2, 0)));
2488  }
2489 
2490  { // Z-Axis
2491  Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
2492  Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot);
2493  EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
2494  EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, 0, M_PI_2)));
2495  }
2496 
2497  { // Translation
2498  Eigen::Isometry3d pi_rot = identity * Eigen::Translation3d(1, 2, 3);
2499  Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot);
2500  EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d(1, 2, 3)));
2501  EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d::Zero()));
2502  }
2503 }
2504 
2505 void runCalcJacobianTransformErrorDiffTest(double anlge)
2506 {
2507  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
2508  const double eps = 0.001;
2509  { // X-Axis positive
2510  Eigen::Isometry3d target_tf{ identity };
2511  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2512  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2513  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX());
2514  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2515  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2516  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(eps, 0, 0)));
2517  }
2518 
2519  { // X-Axis negative
2520  Eigen::Isometry3d target_tf{ identity };
2521  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2522  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2523  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2524  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2525  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2526  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-eps, 0, 0)));
2527  }
2528 
2529  { // X-Axis positive and negative
2530  Eigen::Isometry3d target_tf{ identity };
2531  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2532  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX());
2533  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2534  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2535  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2536  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
2537  }
2538 
2539  { // X-Axis translation
2540  Eigen::Isometry3d target_tf{ identity };
2541  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2542  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2543  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2544  source_tf_perturbed.translation() += Eigen::Vector3d(eps, -eps, eps);
2545  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2546  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps)));
2547  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-eps, 0, 0)));
2548  }
2549 
2550  { // Y-Axis positive
2551  Eigen::Isometry3d target_tf{ identity };
2552  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2553  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2554  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY());
2555  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2556  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2557  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, eps, 0)));
2558  }
2559 
2560  { // Y-Axis negative
2561  Eigen::Isometry3d target_tf{ identity };
2562  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2563  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2564  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2565  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2566  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2567  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -eps, 0)));
2568  }
2569 
2570  { // Y-Axis positive and negative
2571  Eigen::Isometry3d target_tf{ identity };
2572  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2573  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY());
2574  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2575  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2576  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2577  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
2578  }
2579 
2580  { // Y-Axis translation
2581  Eigen::Isometry3d target_tf{ identity };
2582  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2583  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2584  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2585  source_tf_perturbed.translation() += Eigen::Vector3d(-eps, eps, -eps);
2586  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2587  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps)));
2588  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -eps, 0)));
2589  }
2590 
2591  { // Z-Axis positive
2592  Eigen::Isometry3d target_tf{ identity };
2593  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2594  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2595  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ());
2596  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2597  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2598  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, eps)));
2599  }
2600 
2601  { // Z-Axis negative
2602  Eigen::Isometry3d target_tf{ identity };
2603  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2604  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2605  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2606  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2607  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2608  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -eps)));
2609  }
2610 
2611  { // Z-Axis positive and negative
2612  Eigen::Isometry3d target_tf{ identity };
2613  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2614  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ());
2615  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2616  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2617  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
2618  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
2619  }
2620 
2621  { // Z-Axis translation
2622  Eigen::Isometry3d target_tf{ identity };
2623  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2624  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2625  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2626  source_tf_perturbed.translation() += Eigen::Vector3d(-eps, -eps, eps);
2627  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, source_tf_perturbed);
2628  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, -eps, eps)));
2629  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -eps)));
2630  }
2631 }
2632 
2633 void runCalcJacobianTransformErrorDiffDynamicTargetTest(double angle)
2634 {
2635  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
2636  const double eps = 0.001;
2637  { // X-Axis positive
2638  Eigen::Isometry3d target_tf{ identity };
2639  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2640  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX()) * target_tf;
2641  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2642  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX());
2643  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2644  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2645  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2646  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(2 * eps, 0, 0)));
2647  }
2648 
2649  { // X-Axis negative
2650  Eigen::Isometry3d target_tf{ identity };
2651  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2652  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
2653  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2654  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
2655  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2656  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2657  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2658  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
2659  }
2660 
2661  { // X-Axis positive and negative
2662  Eigen::Isometry3d target_tf{ identity };
2663  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2664  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
2665  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX());
2666  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
2667  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2668  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2669  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2670  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-3 * eps, 0, 0)));
2671  }
2672 
2673  { // X-Axis translation
2674  Eigen::Isometry3d target_tf{ identity };
2675  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2676  Eigen::Isometry3d target_tf_perturbed =
2677  Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf * Eigen::Translation3d(eps, -eps, eps);
2678  ;
2679  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2680  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
2681  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2682  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2683  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps)));
2684  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
2685  }
2686 
2687  { // Y-Axis positive
2688  Eigen::Isometry3d target_tf{ identity };
2689  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2690  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY()) * target_tf;
2691  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2692  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY());
2693  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2694  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2695  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2696  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 2 * eps, 0)));
2697  }
2698 
2699  { // Y-Axis negative
2700  Eigen::Isometry3d target_tf{ identity };
2701  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2702  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
2703  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2704  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
2705  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2706  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2707  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2708  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
2709  }
2710 
2711  { // Y-Axis positive and negative
2712  Eigen::Isometry3d target_tf{ identity };
2713  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2714  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
2715  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY());
2716  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
2717  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2718  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2719  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2720  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -3 * eps, 0)));
2721  }
2722 
2723  { // Y-Axis translation
2724  Eigen::Isometry3d target_tf{ identity };
2725  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2726  Eigen::Isometry3d target_tf_perturbed =
2727  Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf * Eigen::Translation3d(-eps, eps, -eps);
2728  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2729  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
2730  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2731  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2732  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps)));
2733  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
2734  }
2735 
2736  { // Z-Axis positive
2737  Eigen::Isometry3d target_tf{ identity };
2738  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2739  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ()) * target_tf;
2740  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2741  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ());
2742  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2743  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2744  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2745  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, 2 * eps)));
2746  }
2747 
2748  { // Z-Axis negative
2749  Eigen::Isometry3d target_tf{ identity };
2750  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2751  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
2752  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2753  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
2754  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2755  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2756  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2757  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
2758  }
2759 
2760  { // Z-Axis positive and negative
2761  Eigen::Isometry3d target_tf{ identity };
2762  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2763  Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
2764  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ());
2765  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
2766  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2767  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2768  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
2769  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -3 * eps)));
2770  }
2771 
2772  { // Z-Axis translation
2773  Eigen::Isometry3d target_tf{ identity };
2774  target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2775  Eigen::Isometry3d target_tf_perturbed =
2776  Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf * Eigen::Translation3d(-eps, -eps, eps);
2777  Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2778  Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
2779  Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
2780  target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2781  EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, eps, -eps)));
2782  EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
2783  }
2784 }
2785 
2787 TEST(TesseractCommonUnit, calcJacobianTransformErrorDiff) // NOLINT
2788 {
2789  runCalcJacobianTransformErrorDiffTest(0);
2790  runCalcJacobianTransformErrorDiffTest(M_PI_2);
2791  runCalcJacobianTransformErrorDiffTest(M_PI);
2792  runCalcJacobianTransformErrorDiffTest(3 * M_PI_2);
2793  runCalcJacobianTransformErrorDiffTest(2 * M_PI);
2794 
2795  runCalcJacobianTransformErrorDiffDynamicTargetTest(0);
2796  runCalcJacobianTransformErrorDiffDynamicTargetTest(M_PI_2);
2797  runCalcJacobianTransformErrorDiffDynamicTargetTest(M_PI);
2798  runCalcJacobianTransformErrorDiffDynamicTargetTest(3 * M_PI_2);
2799  runCalcJacobianTransformErrorDiffDynamicTargetTest(2 * M_PI);
2800 }
2801 
2803 TEST(TesseractCommonUnit, computeRandomColor) // NOLINT
2804 {
2805  Eigen::Vector4d color = tesseract_common::computeRandomColor();
2806  EXPECT_FALSE(color(0) < 0);
2807  EXPECT_FALSE(color(1) < 0);
2808  EXPECT_FALSE(color(2) < 0);
2809  EXPECT_FALSE(color(3) < 0);
2810  EXPECT_FALSE(color(0) > 1);
2811  EXPECT_FALSE(color(1) > 1);
2812  EXPECT_FALSE(color(2) > 1);
2813  EXPECT_FALSE(color(3) > 1);
2814 }
2815 
2817 TEST(TesseractCommonUnit, concat) // NOLINT
2818 {
2819  Eigen::Vector3d a(1, 2, 3);
2820  Eigen::Vector3d b(4, 5, 6);
2821 
2822  Eigen::VectorXd c = tesseract_common::concat(a, b);
2823  EXPECT_EQ(c.rows(), a.rows() + b.rows());
2824  EXPECT_TRUE(c.head(3).isApprox(a));
2825  EXPECT_TRUE(c.tail(3).isApprox(b));
2826 }
2827 
2828 TEST(TesseractCommonUnit, TestAllowedCollisionMatrix) // NOLINT
2829 {
2830  tesseract_common::AllowedCollisionMatrix acm;
2831  acm.reserveAllowedCollisionMatrix(2);
2832 
2833  acm.addAllowedCollision("link1", "link2", "test");
2834  // collision between link1 and link2 should be allowed
2835  EXPECT_TRUE(acm.isCollisionAllowed("link1", "link2"));
2836  // but now between link2 and link3
2837  EXPECT_FALSE(acm.isCollisionAllowed("link2", "link3"));
2838 
2839  tesseract_common::AllowedCollisionMatrix acm_copy(acm);
2840  EXPECT_TRUE(acm_copy == acm);
2841  // collision between link1 and link2 should be allowed
2842  EXPECT_TRUE(acm_copy.isCollisionAllowed("link1", "link2"));
2843  // but now between link2 and link3
2844  EXPECT_FALSE(acm_copy.isCollisionAllowed("link2", "link3"));
2845 
2846  tesseract_common::AllowedCollisionMatrix acm_move(std::move(acm_copy));
2847  EXPECT_TRUE(acm_move == acm);
2848  // collision between link1 and link2 should be allowed
2849  EXPECT_TRUE(acm_move.isCollisionAllowed("link1", "link2"));
2850  // but now between link2 and link3
2851  EXPECT_FALSE(acm_move.isCollisionAllowed("link2", "link3"));
2852 
2853  acm.removeAllowedCollision("link1", "link2");
2854  // now collision link1 and link2 is not allowed anymore
2855  EXPECT_FALSE(acm.isCollisionAllowed("link1", "link2"));
2856 
2857  acm.addAllowedCollision("link3", "link3", "test");
2858  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 1);
2859  acm.clearAllowedCollisions();
2860  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 0);
2861 
2862  tesseract_common::AllowedCollisionMatrix acm2;
2863  acm.addAllowedCollision("link1", "link2", "test");
2864  acm2.addAllowedCollision("link1", "link2", "test");
2865  acm2.addAllowedCollision("link1", "link3", "test");
2866  acm.insertAllowedCollisionMatrix(acm2);
2867 
2868  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 2);
2869  EXPECT_TRUE(acm.isCollisionAllowed("link1", "link2"));
2870  EXPECT_TRUE(acm.isCollisionAllowed("link1", "link3"));
2871  EXPECT_FALSE(acm.isCollisionAllowed("link2", "link3"));
2872  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 2);
2873 
2874  acm.addAllowedCollision("link2", "link3", "test");
2875  acm.removeAllowedCollision("link1");
2876  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 1);
2877  EXPECT_FALSE(acm.isCollisionAllowed("link1", "link2"));
2878  EXPECT_FALSE(acm.isCollisionAllowed("link1", "link3"));
2879  EXPECT_TRUE(acm.isCollisionAllowed("link2", "link3"));
2880  EXPECT_EQ(acm.getAllAllowedCollisions().size(), 1);
2881 
2882  // ostream
2883  std::stringstream ss;
2884  EXPECT_NO_THROW(ss << acm); // NOLINT
2885 }
2886 
2887 TEST(TesseractCommonUnit, TestAllowedCollisionEntriesCompare) // NOLINT
2888 {
2889  tesseract_common::AllowedCollisionMatrix acm1;
2890  acm1.addAllowedCollision("link1", "link2", "test");
2891 
2892  tesseract_common::AllowedCollisionMatrix acm2;
2893  acm2.addAllowedCollision("link1", "link2", "test");
2894 
2895  EXPECT_TRUE(acm1.getAllAllowedCollisions() == acm2.getAllAllowedCollisions());
2896 
2897  acm2.addAllowedCollision("link1", "link3", "test");
2898  EXPECT_FALSE(acm1.getAllAllowedCollisions() == acm2.getAllAllowedCollisions());
2899 
2900  acm2.clearAllowedCollisions();
2901  acm2.addAllowedCollision("link1", "link3", "test");
2902  EXPECT_FALSE(acm1.getAllAllowedCollisions() == acm2.getAllAllowedCollisions());
2903 
2904  acm2.clearAllowedCollisions();
2905  acm2.addAllowedCollision("link1", "link2", "do_not_match");
2906  EXPECT_FALSE(acm1.getAllAllowedCollisions() == acm2.getAllAllowedCollisions());
2907 }
2908 
2909 TEST(TesseractCommonUnit, CollisionMarginDataUnit) // NOLINT
2910 {
2911  double tol = std::numeric_limits<double>::epsilon();
2912 
2913  { // Test Default Constructor
2914  tesseract_common::CollisionMarginData data;
2915  EXPECT_NEAR(data.getDefaultCollisionMargin(), 0, tol);
2916  EXPECT_NEAR(data.getMaxCollisionMargin(), 0, tol);
2917  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), 0, tol);
2918  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2919 
2920  double increment = 0.01;
2921  data.incrementMargins(increment);
2922  EXPECT_NEAR(data.getDefaultCollisionMargin(), increment, tol);
2923  EXPECT_NEAR(data.getMaxCollisionMargin(), increment, tol);
2924  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), increment, tol);
2925  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2926 
2927  increment = -0.01;
2928  data.incrementMargins(increment);
2929  data.incrementMargins(increment);
2930  EXPECT_NEAR(data.getDefaultCollisionMargin(), increment, tol);
2931  EXPECT_NEAR(data.getMaxCollisionMargin(), increment, tol);
2932  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), increment, tol);
2933  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2934  }
2935 
2936  { // Test construction with non zero default distance
2937  double default_margin = 0.0254;
2938  tesseract_common::CollisionMarginData data(default_margin);
2939  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
2940  EXPECT_NEAR(data.getMaxCollisionMargin(), default_margin, tol);
2941  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), default_margin, tol);
2942  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2943 
2944  double scale{ -1 };
2945  data.scaleMargins(scale);
2946  EXPECT_NEAR(data.getDefaultCollisionMargin(), scale * default_margin, tol);
2947  EXPECT_NEAR(data.getMaxCollisionMargin(), scale * default_margin, tol);
2948  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), scale * default_margin, tol);
2949  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2950 
2951  data.scaleMargins(scale);
2952  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
2953  EXPECT_NEAR(data.getMaxCollisionMargin(), default_margin, tol);
2954  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), default_margin, tol);
2955  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2956  }
2957 
2958  { // Test changing default margin
2959  double default_margin = 0.0254;
2960  tesseract_common::CollisionMarginData data;
2961  data.setDefaultCollisionMargin(default_margin);
2962  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
2963  EXPECT_NEAR(data.getMaxCollisionMargin(), default_margin, tol);
2964  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), default_margin, tol);
2965  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 0);
2966  }
2967 
2968  { // Test construction with default margin and pair margins
2969  double default_margin = 0.0;
2970  double pair_margin = 0.5;
2971  tesseract_common::CollisionMarginPairData pair_margins;
2972  pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
2973  tesseract_common::CollisionMarginData data(pair_margins);
2974  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
2975  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
2976  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
2977  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
2978  }
2979 
2980  { // Test construction with non default margin and pair margins
2981  double default_margin = 0.0254;
2982  double pair_margin = 0.5;
2983  tesseract_common::CollisionMarginPairData pair_margins;
2984  pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
2985  tesseract_common::CollisionMarginData data(default_margin, pair_margins);
2986  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
2987  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
2988  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
2989  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
2990  }
2991 
2992  { // Test construction with non default margin and pair margins
2993  double default_margin = 0.0254;
2994  double pair_margin = 0.5;
2995  tesseract_common::PairsCollisionMarginData temp;
2996  temp[std::make_pair("link_1", "link_2")] = pair_margin;
2997 
2998  tesseract_common::CollisionMarginPairData pair_margins(temp);
2999  tesseract_common::CollisionMarginData data(default_margin, pair_margins);
3000  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3001  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3002  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3003  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3004  }
3005 
3006  { // Test construction with default margin and pair margins
3007  double default_margin = 0.0;
3008  double pair_margin = 0.5;
3009  tesseract_common::PairsCollisionMarginData temp;
3010  temp[std::make_pair("link_1", "link_2")] = pair_margin;
3011  tesseract_common::CollisionMarginPairData pair_margins(temp);
3012  tesseract_common::CollisionMarginData data(pair_margins);
3013  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3014  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3015  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3016  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3017  }
3018 
3019  { // Test adding pair margin larger than default
3020  double default_margin = 0.0254;
3021  double pair_margin = 0.5;
3022  tesseract_common::CollisionMarginData data(default_margin);
3023  data.setCollisionMargin("link_1", "link_2", pair_margin);
3024  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3025  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3026  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3027  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3028  }
3029 
3030  { // Test adding pair margin less than default
3031  double default_margin = 0.0254;
3032  double pair_margin = 0.01;
3033  tesseract_common::CollisionMarginData data(default_margin);
3034  data.setCollisionMargin("link_1", "link_2", pair_margin);
3035  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3036  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3037  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3038  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3039  }
3040 
3041  { // Test setting default larger than the current max margin
3042  double default_margin = 0.0254;
3043  double pair_margin = 0.5;
3044  tesseract_common::CollisionMarginData data(default_margin);
3045  data.setCollisionMargin("link_1", "link_2", pair_margin);
3046 
3047  default_margin = 2 * pair_margin;
3048  data.setDefaultCollisionMargin(default_margin);
3049  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3050  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3051  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3052  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3053  }
3054 
3055  { // Test setting pair_margin larger than default and then set it lower so the max should be the default
3056  double default_margin = 0.0254;
3057  double pair_margin = 0.5;
3058  tesseract_common::CollisionMarginData data(default_margin);
3059  data.setCollisionMargin("link_1", "link_2", pair_margin);
3060  data.setCollisionMargin("link_1", "link_2", default_margin);
3061  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3062  EXPECT_NEAR(data.getMaxCollisionMargin(), default_margin, tol);
3063  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), default_margin, tol);
3064  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3065  }
3066 
3067  { // Test setting default larger than pair the change to lower than pair and the max should be the pair
3068  double default_margin = 0.05;
3069  double pair_margin = 0.0254;
3070  tesseract_common::CollisionMarginData data(default_margin);
3071  data.setCollisionMargin("link_1", "link_2", pair_margin);
3072 
3073  default_margin = 0.0;
3074  data.setDefaultCollisionMargin(default_margin);
3075  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3076  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3077  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3078  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3079  }
3080 
3081  { // Test increment positive
3082  double default_margin = 0.0254;
3083  double pair_margin = 0.5;
3084  double increment = 0.01;
3085  tesseract_common::CollisionMarginData data(default_margin);
3086  data.setCollisionMargin("link_1", "link_2", pair_margin);
3087  data.incrementMargins(increment);
3088  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin + increment, tol);
3089  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin + increment, pair_margin + increment), tol);
3090  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin + increment, tol);
3091  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3092  }
3093 
3094  { // Test increment negative
3095  double default_margin = 0.0254;
3096  double pair_margin = 0.5;
3097  double increment = -0.01;
3098  tesseract_common::CollisionMarginData data(default_margin);
3099  data.setCollisionMargin("link_1", "link_2", pair_margin);
3100  data.incrementMargins(increment);
3101  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin + increment, tol);
3102  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin + increment, pair_margin + increment), tol);
3103  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin + increment, tol);
3104  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3105  }
3106 
3107  { // Test scale > 1
3108  double default_margin = 0.0254;
3109  double pair_margin = 0.5;
3110  double scale = 1.5;
3111  tesseract_common::CollisionMarginData data(default_margin);
3112  data.setCollisionMargin("link_1", "link_2", pair_margin);
3113  data.scaleMargins(scale);
3114  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin * scale, tol);
3115  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin * scale, pair_margin * scale), tol);
3116  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin * scale, tol);
3117  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3118  }
3119 
3120  { // Test scale < 1
3121  double default_margin = 0.0254;
3122  double pair_margin = 0.5;
3123  double scale = 0.5;
3124  tesseract_common::CollisionMarginData data(default_margin);
3125  data.setCollisionMargin("link_1", "link_2", pair_margin);
3126  data.scaleMargins(scale);
3127  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin * scale, tol);
3128  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin * scale, pair_margin * scale), tol);
3129  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin * scale, tol);
3130  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3131  }
3132 
3133  { // Test Apply Override Default
3134  double default_margin = 0.0254;
3135  double pair_margin = 0.5;
3136  tesseract_common::CollisionMarginData data(default_margin);
3137  data.setCollisionMargin("link_1", "link_2", pair_margin);
3138 
3139  default_margin = default_margin * 3;
3140  data.setDefaultCollisionMargin(default_margin);
3141  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3142  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3143  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3144  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3145  }
3146 
3147  { // Test Apply Override Link Pair
3148  double default_margin = 0.0254;
3149  double pair_margin = 0.5;
3150  tesseract_common::CollisionMarginData data(default_margin);
3151  data.setCollisionMargin("link_1", "link_2", pair_margin);
3152 
3153  default_margin = default_margin * 3;
3154  pair_margin = pair_margin * 3;
3155  tesseract_common::CollisionMarginPairData override_pair_margins;
3156  override_pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
3157  data.setDefaultCollisionMargin(default_margin);
3158  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3159  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3160  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3161  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3162  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3163  }
3164 
3165  { // Test Apply Override Replace
3166  double default_margin = 0.0254;
3167  double pair_margin = 0.5;
3168  tesseract_common::CollisionMarginData data(default_margin);
3169  data.setCollisionMargin("link_1", "link_2", pair_margin);
3170 
3171  default_margin = default_margin * 3;
3172  pair_margin = pair_margin * 3;
3173  tesseract_common::CollisionMarginPairData override_pair_margins;
3174  override_pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
3175  data.setDefaultCollisionMargin(default_margin);
3176  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::REPLACE);
3177  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3178  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3179  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3180  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3181  }
3182 
3183  { // Test Apply Override None
3184  double default_margin = 0.0254;
3185  double pair_margin = 0.5;
3186  tesseract_common::CollisionMarginData data(default_margin);
3187  data.setCollisionMargin("link_1", "link_2", pair_margin);
3188 
3189  default_margin = default_margin * 3;
3190  tesseract_common::CollisionMarginPairData override_pair_margins;
3191  override_pair_margins.setCollisionMargin("link_1", "link_2", pair_margin * 3);
3192  data.setDefaultCollisionMargin(default_margin);
3193  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::NONE);
3194  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3195  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin), tol);
3196  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3197  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 1);
3198  }
3199 
3200  { // Test Apply Override Modify
3201  double default_margin = 0.0254;
3202  double pair_margin = 0.5;
3203  tesseract_common::CollisionMarginData data(default_margin);
3204  data.setCollisionMargin("link_1", "link_2", pair_margin);
3205 
3206  default_margin = default_margin * 3;
3207  tesseract_common::CollisionMarginPairData override_pair_margins;
3208  override_pair_margins.setCollisionMargin("link_1", "link_3", pair_margin * 3);
3209  data.setDefaultCollisionMargin(default_margin);
3210  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3211  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3212  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin * 3), tol);
3213  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3214  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_3"), pair_margin * 3, tol);
3215  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 2);
3216 
3217  // Test clearing the pair data
3218  auto pair_data_copy = data.getCollisionMarginPairData();
3219  EXPECT_EQ(pair_data_copy.getCollisionMargins().size(), 2);
3220  EXPECT_FALSE(pair_data_copy.empty());
3221  pair_data_copy.clear();
3222  EXPECT_EQ(pair_data_copy.getCollisionMargins().size(), 0);
3223  EXPECT_TRUE(pair_data_copy.empty());
3224  }
3225 
3226  { // Test Apply Override Modify with pair that already exists
3227  double default_margin = 0.0254;
3228  double pair_margin = 0.5;
3229  tesseract_common::CollisionMarginData data(default_margin);
3230  data.setCollisionMargin("link_1", "link_2", pair_margin);
3231 
3232  pair_margin = pair_margin * 3;
3233  default_margin = default_margin * 3;
3234  tesseract_common::CollisionMarginPairData override_pair_margins;
3235  override_pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
3236  override_pair_margins.setCollisionMargin("link_1", "link_3", pair_margin * 3);
3237  data.setDefaultCollisionMargin(default_margin);
3238  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3239  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3240  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin * 3), tol);
3241  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3242  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_3"), pair_margin * 3, tol);
3243  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 2);
3244  }
3245 
3246  { // Test Apply Override Modify Pair
3247  double default_margin = 0.0254;
3248  double pair_margin = 0.5;
3249  tesseract_common::CollisionMarginData data(default_margin);
3250  data.setCollisionMargin("link_1", "link_2", pair_margin);
3251 
3252  default_margin = default_margin * 3;
3253  tesseract_common::CollisionMarginPairData override_pair_margins;
3254  override_pair_margins.setCollisionMargin("link_1", "link_3", pair_margin * 3);
3255  data.setDefaultCollisionMargin(default_margin);
3256  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3257  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3258  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin * 3), tol);
3259  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3260  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_3"), pair_margin * 3, tol);
3261  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 2);
3262  }
3263 
3264  { // Test Apply Override Modify Pair that already exists
3265  double default_margin = 0.0254;
3266  double pair_margin = 0.5;
3267  tesseract_common::CollisionMarginData data(default_margin);
3268  data.setCollisionMargin("link_1", "link_2", pair_margin);
3269 
3270  pair_margin = pair_margin * 3;
3271  default_margin = default_margin * 3;
3272  tesseract_common::CollisionMarginPairData override_pair_margins;
3273  override_pair_margins.setCollisionMargin("link_1", "link_2", pair_margin);
3274  override_pair_margins.setCollisionMargin("link_1", "link_3", pair_margin * 3);
3275  data.setDefaultCollisionMargin(default_margin);
3276  data.apply(override_pair_margins, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3277  EXPECT_NEAR(data.getDefaultCollisionMargin(), default_margin, tol);
3278  EXPECT_NEAR(data.getMaxCollisionMargin(), std::max(default_margin, pair_margin * 3), tol);
3279  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_2"), pair_margin, tol);
3280  EXPECT_NEAR(data.getCollisionMargin("link_1", "link_3"), pair_margin * 3, tol);
3281  EXPECT_EQ(data.getCollisionMarginPairData().getCollisionMargins().size(), 2);
3282  }
3283 }
3284 
3285 TEST(TesseractCommonUnit, CollisionMarginDataCompare) // NOLINT
3286 {
3287  { // EQUAL Default
3288  tesseract_common::CollisionMarginData margin_data1;
3289  tesseract_common::CollisionMarginData margin_data2;
3290 
3291  EXPECT_TRUE(margin_data1 == margin_data2);
3292  }
3293 
3294  { // EQUAL with pair data
3295  tesseract_common::CollisionMarginData margin_data1;
3296  margin_data1.setCollisionMargin("link_1", "link_2", 1);
3297  tesseract_common::CollisionMarginData margin_data2;
3298  margin_data2.setCollisionMargin("link_1", "link_2", 1);
3299 
3300  EXPECT_TRUE(margin_data1 == margin_data2);
3301  }
3302 
3303  { // Not EQUAL Default
3304  tesseract_common::CollisionMarginData margin_data1(0.1);
3305  tesseract_common::CollisionMarginData margin_data2(0.2);
3306 
3307  EXPECT_FALSE(margin_data1 == margin_data2);
3308  }
3309 
3310  { // Not EQUAL with pair data
3311  tesseract_common::CollisionMarginData margin_data1;
3312  margin_data1.setCollisionMargin("link_1", "link_2", 1);
3313  tesseract_common::CollisionMarginData margin_data2;
3314  margin_data2.setCollisionMargin("link_1", "link_2", 1);
3315  margin_data2.setCollisionMargin("link_1", "link_3", 1);
3316 
3317  EXPECT_FALSE(margin_data1 == margin_data2);
3318  }
3319 
3320  { // Not EQUAL with pair data
3321  tesseract_common::CollisionMarginData margin_data1;
3322  margin_data1.setCollisionMargin("link_1", "link_2", 1);
3323  tesseract_common::CollisionMarginData margin_data2;
3324  margin_data2.setCollisionMargin("link_1", "link_2", 2);
3325 
3326  EXPECT_FALSE(margin_data1 == margin_data2);
3327  }
3328 
3329  { // Not EQUAL with pair data
3330  tesseract_common::CollisionMarginData margin_data1;
3331  margin_data1.setCollisionMargin("link_1", "link_2", 1);
3332  tesseract_common::CollisionMarginData margin_data2;
3333  margin_data2.setCollisionMargin("link_1", "link_3", 1);
3334 
3335  EXPECT_FALSE(margin_data1 == margin_data2);
3336  }
3337 }
3338 
3339 // Helper function to create temporary test files
3340 void createTestYamlWithIncludeDirectivesFile(const std::string& filePath, const std::string& content)
3341 {
3342  std::ofstream file(filePath);
3343  ASSERT_TRUE(file.is_open());
3344  file << content;
3345  file.close();
3346 }
3347 
3348 TEST(TesseractCommonUnit, YamlBasicIncludeTest) // NOLINT
3349 {
3350  std::string separator(1, std::filesystem::path::preferred_separator);
3351  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_1" + separator;
3352 
3353  // Create a temporary test directory
3354  std::filesystem::create_directory(test_dir);
3355 
3356  // Resource locator
3357  tesseract_common::GeneralResourceLocator locator;
3358 
3359  // Create test files
3360  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3361 key1: value1
3362 key2: !include included.yaml
3363 )");
3364  createTestYamlWithIncludeDirectivesFile(test_dir + "included.yaml", R"(
3365 included_key1: included_value1
3366 included_key2: included_value2
3367 )");
3368 
3369  // Load the main file
3370  YAML::Node root = loadYamlFile(test_dir + "main.yaml", locator);
3371  tesseract_common::writeYamlToFile(root, test_dir + "processed.yaml");
3372 
3373  // Validate the structure
3374  ASSERT_TRUE(root["key1"].IsScalar());
3375  ASSERT_EQ(root["key1"].as<std::string>(), "value1");
3376 
3377  ASSERT_TRUE(root["key2"].IsMap());
3378  ASSERT_EQ(root["key2"]["included_key1"].as<std::string>(), "included_value1");
3379  ASSERT_EQ(root["key2"]["included_key2"].as<std::string>(), "included_value2");
3380 
3381  // Clean up the test directory
3382  std::filesystem::remove_all(test_dir);
3383 }
3384 
3385 TEST(TesseractCommonUnit, YamlIncludeNestedIncludesTest) // NOLINT
3386 {
3387  std::string separator(1, std::filesystem::path::preferred_separator);
3388  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_2" + separator;
3389 
3390  // Create a temporary test directory
3391  std::filesystem::create_directory(test_dir);
3392 
3393  // Resource locator
3394  tesseract_common::GeneralResourceLocator locator;
3395 
3396  // Create test files
3397  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3398 key1: value1
3399 key2: !include included.yaml
3400 )");
3401  createTestYamlWithIncludeDirectivesFile(test_dir + "included.yaml", R"(
3402 nested_key1: !include nested.yaml
3403 )");
3404  createTestYamlWithIncludeDirectivesFile(test_dir + "nested.yaml", R"(
3405 deep_key1: deep_value1
3406 )");
3407 
3408  // Load the main file
3409  YAML::Node root = loadYamlFile(test_dir + "main.yaml", locator);
3410  tesseract_common::writeYamlToFile(root, test_dir + "processed.yaml");
3411 
3412  // Validate the structure
3413  ASSERT_TRUE(root["key2"].IsMap());
3414  ASSERT_TRUE(root["key2"]["nested_key1"].IsMap());
3415  ASSERT_EQ(root["key2"]["nested_key1"]["deep_key1"].as<std::string>(), "deep_value1");
3416 
3417  // Clean up the test directory
3418  std::filesystem::remove_all(test_dir);
3419 }
3420 
3421 TEST(TesseractCommonUnit, YamlIncludeSequenceIncludesTest) // NOLINT
3422 {
3423  std::string separator(1, std::filesystem::path::preferred_separator);
3424  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_3" + separator;
3425 
3426  // Create a temporary test directory
3427  std::filesystem::create_directory(test_dir);
3428 
3429  // Resource locator
3430  tesseract_common::GeneralResourceLocator locator;
3431 
3432  // Create test files
3433  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3434 key1:
3435  - item1
3436  - !include included.yaml
3437 )");
3438  createTestYamlWithIncludeDirectivesFile(test_dir + "included.yaml", R"(
3439 - included_item1
3440 - included_item2
3441 )");
3442 
3443  // Load the main file
3444  YAML::Node root = loadYamlFile(test_dir + "main.yaml", locator);
3445  tesseract_common::writeYamlToFile(root, test_dir + "processed.yaml");
3446 
3447  // Validate the structure
3448  ASSERT_TRUE(root["key1"].IsSequence());
3449  ASSERT_EQ(root["key1"].size(), 2);
3450  ASSERT_EQ(root["key1"][0].as<std::string>(), "item1");
3451  ASSERT_EQ(root["key1"][1][0].as<std::string>(), "included_item1");
3452  ASSERT_EQ(root["key1"][1][1].as<std::string>(), "included_item2");
3453 
3454  // Clean up the test directory
3455  std::filesystem::remove_all(test_dir);
3456 }
3457 
3458 TEST(TesseractCommonUnit, YamlIncludeSequenceIncludesMapTest) // NOLINT
3459 {
3460  std::string separator(1, std::filesystem::path::preferred_separator);
3461  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_4" + separator;
3462 
3463  // Create a temporary test directory
3464  std::filesystem::create_directory(test_dir);
3465 
3466  // Resource locator
3467  tesseract_common::GeneralResourceLocator locator;
3468 
3469  // Create test files
3470  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3471 key1:
3472  - item1
3473  - !include included.yaml
3474 )");
3475  createTestYamlWithIncludeDirectivesFile(test_dir + "included.yaml", R"(
3476 keyA: valueA
3477 keyB: valueB
3478 )");
3479 
3480  // Load the main file
3481  YAML::Node root = loadYamlFile(test_dir + "main.yaml", locator);
3482  tesseract_common::writeYamlToFile(root, test_dir + "processed.yaml");
3483 
3484  // Validate the structure
3485  ASSERT_TRUE(root["key1"].IsSequence());
3486  ASSERT_EQ(root["key1"].size(), 2);
3487  ASSERT_EQ(root["key1"][0].as<std::string>(), "item1");
3488 
3489  // Check the included map
3490  ASSERT_TRUE(root["key1"][1].IsMap());
3491  ASSERT_EQ(root["key1"][1]["keyA"].as<std::string>(), "valueA");
3492  ASSERT_EQ(root["key1"][1]["keyB"].as<std::string>(), "valueB");
3493 
3494  // Clean up the test directory
3495  std::filesystem::remove_all(test_dir);
3496 }
3497 
3498 TEST(TesseractCommonUnit, YamlIncludeMissingIncludeFileTest) // NOLINT
3499 {
3500  std::string separator(1, std::filesystem::path::preferred_separator);
3501  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_5" + separator;
3502 
3503  // Create a temporary test directory
3504  std::filesystem::create_directory(test_dir);
3505 
3506  // Resource locator
3507  tesseract_common::GeneralResourceLocator locator;
3508 
3509  // Create a test file
3510  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3511 key1: !include missing.yaml
3512 )");
3513 
3514  // Attempt to load the main file and expect an exception
3515  EXPECT_THROW(loadYamlFile(test_dir + "main.yaml", locator), std::runtime_error); // NOLINT
3516 
3517  // Clean up the test directory
3518  std::filesystem::remove_all(test_dir);
3519 }
3520 
3521 TEST(TesseractCommonUnit, YamlIncludeInvalidIncludeTagTest) // NOLINT
3522 {
3523  std::string separator(1, std::filesystem::path::preferred_separator);
3524  std::string test_dir = tesseract_common::getTempPath() + "test_yaml_6" + separator;
3525 
3526  // Create a temporary test directory
3527  std::filesystem::create_directory(test_dir);
3528 
3529  // Resource locator
3530  tesseract_common::GeneralResourceLocator locator;
3531 
3532  // Create a test file with an invalid !include tag
3533  createTestYamlWithIncludeDirectivesFile(test_dir + "main.yaml", R"(
3534 key1: !include
3535 )");
3536 
3537  // Attempt to load the main file and expect an exception
3538  EXPECT_THROW(loadYamlFile(test_dir + "main.yaml", locator), std::runtime_error); // NOLINT
3539 
3540  // Clean up the test directory
3541  std::filesystem::remove_all(test_dir);
3542 }
3543 
3544 TEST(TesseractCommonUnit, YamlCollisionMarginPairOverrideType) // NOLINT
3545 {
3546  {
3547  YAML::Node n = YAML::Load(R"(NONE)");
3548  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3549  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::NONE);
3550  }
3551 
3552  {
3553  YAML::Node n = YAML::Load(R"(MODIFY)");
3554  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3555  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3556  }
3557 
3558  {
3559  YAML::Node n = YAML::Load(R"(REPLACE)");
3560  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3561  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::REPLACE);
3562  }
3563 
3564  {
3565  YAML::Node n(tesseract_common::CollisionMarginPairOverrideType::NONE);
3566  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3567  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::NONE);
3568  }
3569 
3570  {
3571  YAML::Node n(tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3572  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3573  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::MODIFY);
3574  }
3575 
3576  {
3577  YAML::Node n(tesseract_common::CollisionMarginPairOverrideType::REPLACE);
3578  auto v = n.as<tesseract_common::CollisionMarginPairOverrideType>();
3579  EXPECT_EQ(v, tesseract_common::CollisionMarginPairOverrideType::REPLACE);
3580  }
3581 
3582  {
3583  YAML::Node n = YAML::Load(R"(DOES_NOT_EXIST)");
3584  EXPECT_ANY_THROW(n.as<tesseract_common::CollisionMarginPairOverrideType>()); // NOLINT
3585  }
3586 
3587  {
3588  YAML::Node n = YAML::Load(R"(["test", "test"])");
3589  EXPECT_ANY_THROW(n.as<tesseract_common::CollisionMarginPairOverrideType>()); // NOLINT
3590  }
3591 }
3592 
3593 TEST(TesseractCommonUnit, YamlPairsCollisionMarginData) // NOLINT
3594 {
3595  const std::string yaml_string = R"(
3596  ["link1","link2"]: 0.8
3597  ["base","tool0"]: 1.5
3598 )";
3599 
3600  tesseract_common::PairsCollisionMarginData data_original;
3601  data_original[std::make_pair("link1", "link2")] = 0.8;
3602  data_original[std::make_pair("base", "tool0")] = 1.5;
3603 
3604  {
3605  YAML::Node n(data_original);
3606  auto data = n.as<tesseract_common::PairsCollisionMarginData>();
3607  EXPECT_EQ(data.size(), 2U);
3608  EXPECT_DOUBLE_EQ(data.at({ "link1", "link2" }), data_original[std::make_pair("link1", "link2")]);
3609  EXPECT_DOUBLE_EQ(data.at({ "base", "tool0" }), data_original[std::make_pair("base", "tool0")]);
3610  }
3611 
3612  {
3613  YAML::Node n = YAML::Load(yaml_string);
3614  auto data = n.as<tesseract_common::PairsCollisionMarginData>();
3615  EXPECT_EQ(data.size(), 2U);
3616  EXPECT_DOUBLE_EQ(data.at({ "link1", "link2" }), data_original[std::make_pair("link1", "link2")]);
3617  EXPECT_DOUBLE_EQ(data.at({ "base", "tool0" }), data_original[std::make_pair("base", "tool0")]);
3618  }
3619 
3620  { // Failure: Is not map
3621  YAML::Node n = YAML::Load(R"(["test", "test"])");
3622  EXPECT_ANY_THROW(n.as<tesseract_common::PairsCollisionMarginData>()); // NOLINT
3623  }
3624 
3625  { // Failure: Invalid Key
3626  const std::string invalid_yaml = R"(
3627  ["link1","link2", "link2"]: 0.8
3628  ["base","tool0"]: 1.5
3629 )";
3630  YAML::Node n = YAML::Load(invalid_yaml);
3631  EXPECT_ANY_THROW(n.as<tesseract_common::PairsCollisionMarginData>()); // NOLINT
3632  }
3633 }
3634 
3635 TEST(TesseractCommonUnit, YamlCollisionMarginPairData) // NOLINT
3636 {
3637  const std::string yaml_string = R"(
3638  ["link1","link2"]: 0.8
3639  ["base","tool0"]: 1.5
3640 )";
3641 
3642  tesseract_common::CollisionMarginPairData data_original;
3643  {
3644  tesseract_common::PairsCollisionMarginData pair_data;
3645  pair_data[std::make_pair("link1", "link2")] = 0.8;
3646  pair_data[std::make_pair("base", "tool0")] = 1.5;
3647  data_original = tesseract_common::CollisionMarginPairData(pair_data);
3648  }
3649 
3650  {
3651  YAML::Node n = YAML::Load(yaml_string);
3652  auto data = n.as<tesseract_common::CollisionMarginPairData>();
3653  EXPECT_EQ(data.getCollisionMargins().size(), 2U);
3654  // NOLINTNEXTLINE
3655  EXPECT_DOUBLE_EQ(data.getCollisionMargin("link1", "link2").value(),
3656  data_original.getCollisionMargin("link1", "link2").value());
3657  // NOLINTNEXTLINE
3658  EXPECT_DOUBLE_EQ(data.getCollisionMargin("base", "tool0").value(),
3659  data_original.getCollisionMargin("base", "tool0").value());
3660  }
3661 
3662  {
3663  YAML::Node n(data_original);
3664  auto data = n.as<tesseract_common::CollisionMarginPairData>();
3665  EXPECT_EQ(data.getCollisionMargins().size(), 2U);
3666  // NOLINTNEXTLINE
3667  EXPECT_DOUBLE_EQ(data.getCollisionMargin("link1", "link2").value(),
3668  data_original.getCollisionMargin("link1", "link2").value());
3669  // NOLINTNEXTLINE
3670  EXPECT_DOUBLE_EQ(data.getCollisionMargin("base", "tool0").value(),
3671  data_original.getCollisionMargin("base", "tool0").value());
3672  }
3673 }
3674 
3675 TEST(TesseractCommonUnit, YamlAllowedCollisionEntries) // NOLINT
3676 {
3677  const std::string yaml_string = R"(
3678  ["link1","link2"]: "adjacent"
3679  ["base","tool0"]: "never"
3680 )";
3681 
3682  tesseract_common::AllowedCollisionEntries data_original;
3683  data_original[std::make_pair("link1", "link2")] = "adjacent";
3684  data_original[std::make_pair("base", "tool0")] = "never";
3685 
3686  {
3687  YAML::Node n(data_original);
3688  auto data = n.as<tesseract_common::AllowedCollisionEntries>();
3689  EXPECT_EQ(data.size(), 2U);
3690  EXPECT_EQ(data.at({ "link1", "link2" }), data_original[std::make_pair("link1", "link2")]);
3691  EXPECT_EQ(data.at({ "base", "tool0" }), data_original[std::make_pair("base", "tool0")]);
3692  }
3693 
3694  {
3695  YAML::Node n = YAML::Load(yaml_string);
3696  auto data = n.as<tesseract_common::AllowedCollisionEntries>();
3697  EXPECT_EQ(data.size(), 2U);
3698  EXPECT_EQ(data.at({ "link1", "link2" }), data_original[std::make_pair("link1", "link2")]);
3699  EXPECT_EQ(data.at({ "base", "tool0" }), data_original[std::make_pair("base", "tool0")]);
3700  }
3701 
3702  { // Failure: Is not map
3703  YAML::Node n = YAML::Load(R"(["test", "test"])");
3704  EXPECT_ANY_THROW(n.as<tesseract_common::AllowedCollisionEntries>()); // NOLINT
3705  }
3706 
3707  { // Failure: Invalid Key
3708  const std::string invalid_yaml = R"(
3709  ["link1","link2", "link2"]: 0.8
3710  ["base","tool0"]: 1.5
3711 )";
3712  YAML::Node n = YAML::Load(invalid_yaml);
3713  EXPECT_ANY_THROW(n.as<tesseract_common::AllowedCollisionEntries>()); // NOLINT
3714  }
3715 }
3716 
3717 TEST(TesseractCommonUnit, YamlAllowedCollisionMatrix) // NOLINT
3718 {
3719  const std::string yaml_string = R"(
3720  ["link1","link2"]: "adjacent"
3721  ["base","tool0"]: "never"
3722 )";
3723 
3724  tesseract_common::AllowedCollisionMatrix data_original;
3725  {
3726  tesseract_common::AllowedCollisionEntries pair_data;
3727  pair_data[std::make_pair("link1", "link2")] = "adjacent";
3728  pair_data[std::make_pair("base", "tool0")] = "never";
3729  data_original = tesseract_common::AllowedCollisionMatrix(pair_data);
3730  }
3731 
3732  {
3733  YAML::Node n(data_original);
3734  auto data = n.as<tesseract_common::AllowedCollisionMatrix>();
3735  EXPECT_EQ(data.getAllAllowedCollisions().size(), 2U);
3736  EXPECT_EQ(data.isCollisionAllowed("link1", "link2"), data_original.isCollisionAllowed("link1", "link2"));
3737  EXPECT_EQ(data.isCollisionAllowed("base", "tool0"), data_original.isCollisionAllowed("base", "tool0"));
3738  }
3739 
3740  {
3741  YAML::Node n = YAML::Load(yaml_string);
3742  auto data = n.as<tesseract_common::AllowedCollisionMatrix>();
3743  EXPECT_EQ(data.getAllAllowedCollisions().size(), 2U);
3744  EXPECT_EQ(data.isCollisionAllowed("link1", "link2"), data_original.isCollisionAllowed("link1", "link2"));
3745  EXPECT_EQ(data.isCollisionAllowed("base", "tool0"), data_original.isCollisionAllowed("base", "tool0"));
3746  }
3747 }
3748 
3749 TEST(TesseractCommonUnit, YamlStdUnorderedMapStringBool) // NOLINT
3750 {
3751  using DataType = std::unordered_map<std::string, bool>;
3752 
3753  const std::string yaml_string = R"(
3754  link1: true
3755  tool0: false
3756 )";
3757 
3758  DataType data_original;
3759  data_original["link1"] = true;
3760  data_original["tool0"] = false;
3761 
3762  {
3763  YAML::Node n(data_original);
3764  auto data = n.as<DataType>();
3765  EXPECT_EQ(data.size(), 2U);
3766  EXPECT_EQ(data["link1"], data_original["link1"]);
3767  EXPECT_EQ(data["tool0"], data_original["tool0"]);
3768  }
3769 
3770  {
3771  YAML::Node n = YAML::Load(yaml_string);
3772  auto data = n.as<DataType>();
3773  EXPECT_EQ(data.size(), 2U);
3774  EXPECT_EQ(data["link1"], data_original["link1"]);
3775  EXPECT_EQ(data["tool0"], data_original["tool0"]);
3776  }
3777 
3778  { // Failure: Is not map
3779  YAML::Node n = YAML::Load(R"(["test", "test"])");
3780  EXPECT_ANY_THROW(n.as<DataType>()); // NOLINT
3781  }
3782 }
3783 
3784 int main(int argc, char** argv)
3785 {
3786  testing::InitGoogleTest(&argc, argv);
3787 
3788  return RUN_ALL_TESTS();
3789 }
TestHasMemberWithArgFunction
Definition: tesseract_common_unit.cpp:344
tesseract_common::Timer::start
void start(const std::function< void()> &callback, std::chrono::steady_clock::duration interval)
Start the timer with a callback function and a std::chrono::duration interval.
Definition: timer.cpp:33
tesseract_common::CalibrationInfo::CONFIG_KEY
static const std::string CONFIG_KEY
Definition: calibration_info.h:66
tesseract_common::CollisionMarginData::setCollisionMargin
void setCollisionMargin(const std::string &obj1, const std::string &obj2, double collision_margin)
Set the margin for a given contact pair.
Definition: collision_margin_data.cpp:186
tesseract_common::ContactManagersPluginInfo::empty
bool empty() const
Check if structure is empty.
Definition: plugin_info.cpp:212
profile.h
This is a profile base class.
tesseract_common::CalibrationInfo::empty
bool empty() const
Check if structure is empty.
Definition: calibration_info.cpp:49
tesseract_common::Timer
A timer which calls a callback every interval on a separate thread.
Definition: timer.h:40
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
Get the largest collision margin.
Definition: collision_margin_data.cpp:202
TestResourceLocator::~TestResourceLocator
~TestResourceLocator() override=default
tesseract_common::AnyInterface::getType
virtual std::type_index getType() const
Get the type index of the object stored.
Definition: any_poly.cpp:41
tesseract_common::ProfileDictionary::getProfile
Profile::ConstPtr getProfile(std::size_t key, const std::string &ns, const std::string &profile_name) const
Get a profile by name.
Definition: profile_dictionary.cpp:233
tesseract_common::TaskComposerPluginInfo::empty
bool empty() const
Check if structure is empty.
Definition: plugin_info.cpp:267
tesseract_common::TaskComposerPluginInfo::CONFIG_KEY
static const std::string CONFIG_KEY
Definition: plugin_info.h:196
tesseract_common::writeYamlToFile
void writeYamlToFile(const YAML::Node &node, const std::string &file_path)
Writes a YAML::Node to a file.
Definition: yaml_utils.cpp:106
tesseract_common::CollisionMarginPairData::getCollisionMargin
std::optional< double > getCollisionMargin(const std::string &obj1, const std::string &obj2) const
Get the pairs collision margin data.
Definition: collision_margin_data.cpp:55
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
runAnyPolyUnorderedMapIntegralTest
void runAnyPolyUnorderedMapIntegralTest(T value, const std::string &type_str)
Definition: tesseract_common_unit.cpp:658
TestProfile::operator==
bool operator==(const TestProfile &rhs) const
Definition: tesseract_common_unit.cpp:77
tesseract_common::toNumeric
bool toNumeric(const std::string &s, FloatType &value)
Convert a string to a numeric value type.
Definition: utils.h:508
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::AllowedCollisionMatrix::isCollisionAllowed
virtual bool isCollisionAllowed(const std::string &link_name1, const std::string &link_name2) const
This checks if two links are allowed to be in collision.
Definition: allowed_collision_matrix.cpp:100
tesseract_common::concat
Eigen::VectorXd concat(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Concatenate two vector.
Definition: utils.cpp:119
tesseract_common::AllowedCollisionMatrix::getAllAllowedCollisions
const AllowedCollisionEntries & getAllAllowedCollisions() const
Get all of the entries in the allowed collision matrix.
Definition: allowed_collision_matrix.cpp:77
tesseract_common::isIdentical
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:227
tesseract_common::Toolpath
AlignedVector< VectorIsometry3d > Toolpath
Definition: eigen_types.h:33
tesseract_common::AllowedCollisionEntries
std::unordered_map< tesseract_common::LinkNamesPair, std::string, tesseract_common::PairHash > AllowedCollisionEntries
Definition: allowed_collision_matrix.h:24
types.h
Common Tesseract Types.
tesseract_common::ContactManagersPluginInfo::clear
void clear()
Clear the contents.
Definition: plugin_info.cpp:204
tesseract_common::pointersComparison
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:347
tesseract_common::CollisionMarginPairOverrideType::REPLACE
@ REPLACE
Replace the contact manager's CollisionMarginPairData.
tesseract_common::CollisionMarginData::getCollisionMarginPairData
const CollisionMarginPairData & getCollisionMarginPairData() const
Get Collision Margin Data for stored pairs.
Definition: collision_margin_data.cpp:200
tesseract_common::KinematicsPluginInfo::clear
void clear()
Clear the contents.
Definition: plugin_info.cpp:141
TestResourceLocator
Resource locator implementation using a provided function to locate file resources.
Definition: resource_locator_unit.cpp:30
tesseract_common::CalibrationInfo::joints
tesseract_common::TransformMap joints
The joint origin calibration information.
Definition: calibration_info.h:54
tesseract_common::TaskComposerPluginInfo::executor_plugin_infos
tesseract_common::PluginInfoContainer executor_plugin_infos
A map of name to task composer executor plugin information.
Definition: plugin_info.h:181
tesseract_common::Stopwatch::stop
void stop()
Stop the timer.
Definition: stopwatch.cpp:37
tesseract_common::JointState::effort
Eigen::VectorXd effort
The Effort at the waypoint (optional)
Definition: joint_state.h:68
tesseract_common::CollisionMarginPairOverrideType::MODIFY
@ MODIFY
Modify the contact managers pair margins.
any_poly.h
tesseract_common::AllowedCollisionMatrix::insertAllowedCollisionMatrix
void insertAllowedCollisionMatrix(const AllowedCollisionMatrix &acm)
Inserts an allowable collision matrix ignoring duplicate pairs.
Definition: allowed_collision_matrix.cpp:109
CREATE_MEMBER_FUNC_RETURN_TYPE_CHECK
CREATE_MEMBER_FUNC_RETURN_TYPE_CHECK(add, int, int)
tesseract_common::CollisionMarginPairData::getCollisionMargins
const PairsCollisionMarginData & getCollisionMargins() const
Get Collision Margin Data for stored pairs.
Definition: collision_margin_data.cpp:70
collision_margin_data.h
This is used to store collision margin information.
tesseract_common::trim
void trim(std::string &s)
Trim left and right of string.
Definition: utils.cpp:304
tesseract_common::TaskComposerPluginInfo::search_libraries
std::set< std::string > search_libraries
A list of library names without the prefix or suffix that contain plugins.
Definition: plugin_info.h:178
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: eigen_types.h:32
tesseract_common::AllowedCollisionMatrix::reserveAllowedCollisionMatrix
void reserveAllowedCollisionMatrix(std::size_t size)
Reserve space for the internal data storage.
Definition: allowed_collision_matrix.cpp:114
tesseract_common::ManipulatorInfo
Contains information about a robot manipulator.
Definition: manipulator_info.h:44
tesseract_common::pointersEqual
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:336
CREATE_MEMBER_FUNC_SIGNATURE_CHECK
CREATE_MEMBER_FUNC_SIGNATURE_CHECK(add, double, double, double)
tesseract_common::JointState::velocity
Eigen::VectorXd velocity
The velocity at the waypoint (optional)
Definition: joint_state.h:62
CREATE_MEMBER_CHECK
CREATE_MEMBER_CHECK(update)
tesseract_common::TaskComposerPluginInfo
The task composer plugin information structure.
Definition: plugin_info.h:172
sfinae_utils.h
macros.h
Common Tesseract Macros.
runCalcJacobianTransformErrorDiffDynamicTargetTest
void runCalcJacobianTransformErrorDiffDynamicTargetTest(double angle)
Definition: tesseract_common_unit.cpp:2572
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
tesseract_common::CollisionMarginData::getDefaultCollisionMargin
double getDefaultCollisionMargin() const
Get the default collision margin.
Definition: collision_margin_data.cpp:184
tesseract_common::ProfileDictionary::removeProfile
void removeProfile(std::size_t key, const std::string &ns, const std::string &profile_name)
Remove a profile.
Definition: profile_dictionary.cpp:241
tesseract_common::TaskComposerPluginInfo::insert
void insert(const TaskComposerPluginInfo &other)
Insert the content of an other TaskComposerPluginInfo.
Definition: plugin_info.cpp:241
TestResourceLocator::locateResource
tesseract_common::Resource::Ptr locateResource(const std::string &url) const override final
Locate a resource based on a URL.
Definition: tesseract_common_unit.cpp:37
tesseract_common::QueryStringValue
int QueryStringValue(const tinyxml2::XMLElement *xml_element, std::string &value)
Query a string value from xml element.
Definition: utils.cpp:339
TestProfile::operator!=
bool operator!=(const TestProfile &rhs) const
Definition: tesseract_common_unit.cpp:78
tesseract_common::ContactManagersPluginInfo::discrete_plugin_infos
tesseract_common::PluginInfoContainer discrete_plugin_infos
A map of name to discrete contact manager plugin information.
Definition: plugin_info.h:144
tesseract_common::CollisionMarginData
Stores information about how the margins allowed between collision objects.
Definition: collision_margin_data.h:159
tesseract_common::ProfileDictionary::removeProfileEntry
void removeProfileEntry(std::size_t key, const std::string &ns)
Remove a profile entry.
Definition: profile_dictionary.cpp:88
tesseract_common::LinkNamesPair
std::pair< std::string, std::string > LinkNamesPair
Definition: types.h:37
tesseract_common::CollisionMarginData::getCollisionMargin
double getCollisionMargin(const std::string &obj1, const std::string &obj2) const
Get the pairs collision margin data.
Definition: collision_margin_data.cpp:191
tesseract_common::checkForUnknownKeys
void checkForUnknownKeys(const YAML::Node &node, const std::set< std::string > &expected_keys)
Check node map for unknown keys.
Definition: yaml_utils.cpp:122
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::ProfileDictionary::getAllProfileEntries
std::unordered_map< std::string, std::unordered_map< std::size_t, std::unordered_map< std::string, Profile::ConstPtr > > > getAllProfileEntries() const
Get all profile entries.
Definition: profile_dictionary.cpp:118
tesseract_common::PairHash
Definition: types.h:39
tesseract_common::CollisionMarginPairData::setCollisionMargin
void setCollisionMargin(const std::string &obj1, const std::string &obj2, double margin)
Set the margin for a given contact pair.
Definition: collision_margin_data.cpp:48
tesseract_common::ContactManagersPluginInfo::continuous_plugin_infos
tesseract_common::PluginInfoContainer continuous_plugin_infos
A map of name to continuous contact manager plugin information.
Definition: plugin_info.h:147
runCalcJacobianTransformErrorDiffTest
void runCalcJacobianTransformErrorDiffTest(double anlge)
Definition: tesseract_common_unit.cpp:2444
TestProfile::TestProfile
TestProfile(std::size_t key)
Definition: tesseract_common_unit.cpp:71
tesseract_common::KinematicsPluginInfo::search_paths
std::set< std::string > search_paths
A list of paths to search for plugins.
Definition: plugin_info.h:101
tesseract_common::JointState
Definition: joint_state.h:49
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::ManipulatorInfo::tcp_offset
std::variant< std::string, Eigen::Isometry3d > tcp_offset
(Optional) Offset transform applied to the tcp_frame link to represent the manipulator TCP
Definition: manipulator_info.h:73
tesseract_common::PluginInfo::class_name
std::string class_name
The plugin class name.
Definition: plugin_info.h:54
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
manipulator_info.h
tesseract_common::TaskComposerPluginInfo::task_plugin_infos
tesseract_common::PluginInfoContainer task_plugin_infos
A map of name to task composer task plugin information.
Definition: plugin_info.h:184
tesseract_common::JointState::joint_names
std::vector< std::string > joint_names
The joint corresponding to the position vector.
Definition: joint_state.h:56
utils.h
Common Tesseract Utility Functions.
tesseract_common::KinematicsPluginInfo::insert
void insert(const KinematicsPluginInfo &other)
Insert the content of an other KinematicsPluginInfo.
Definition: plugin_info.cpp:117
tesseract_common::CollisionMarginData::scaleMargins
void scaleMargins(double scale)
Scale all margins by input value.
Definition: collision_margin_data.cpp:216
tesseract_common::AnyPoly
Definition: any_poly.h:113
TEST
TEST(TesseractCommonUnit, ProfileDictionary)
Definition: tesseract_common_unit.cpp:81
TestProfile::TestProfile
TestProfile()=default
timer.h
Simple timer class using chrono and thread.
tesseract_common::CollisionMarginPairOverrideType
CollisionMarginPairOverrideType
Identifies how the provided contact margin data should be applied.
Definition: collision_margin_data.h:51
tesseract_common::CalibrationInfo
The CalibrationInfo struct.
Definition: calibration_info.h:41
tesseract_common::KinematicsPluginInfo::inv_plugin_infos
std::map< std::string, tesseract_common::PluginInfoContainer > inv_plugin_infos
A map of group name to inverse kinematics plugin information.
Definition: plugin_info.h:110
tesseract_common::JointState::acceleration
Eigen::VectorXd acceleration
The Acceleration at the waypoint (optional)
Definition: joint_state.h:65
tesseract_common::AnyInterface
Definition: any_poly.h:43
TestProfile::operator=
TestProfile & operator=(const TestProfile &)=default
tesseract_common::AnyPoly::getType
std::type_index getType() const
Get the stored derived type.
Definition: any_poly.cpp:71
tesseract_common::ManipulatorInfo::getCombined
ManipulatorInfo getCombined(const ManipulatorInfo &manip_info_override) const
If the provided manipulator information member is not empty it will override this and return a new ma...
Definition: manipulator_info.cpp:50
main
int main(int argc, char **argv)
Definition: tesseract_common_unit.cpp:3723
tesseract_common::ContactManagersPluginInfo
The contact managers plugin information structure.
Definition: plugin_info.h:135
tesseract_common::PluginInfo
The Plugin Information struct.
Definition: plugin_info.h:51
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::ResourceLocator::ConstPtr
std::shared_ptr< const ResourceLocator > ConstPtr
Definition: resource_locator.h:57
joint_state.h
Tesseract Joint State.
yaml_utils.h
YAML Type conversions.
TestHasMemberWithArgFunction::add
double add(double a, double b) const
Definition: tesseract_common_unit.cpp:347
tesseract_common::printNestedException
void printNestedException(const std::exception &e, int level=0)
Print a nested exception.
Definition: utils.cpp:246
tesseract_common::ResourceLocator
Abstract class for resource loaders.
Definition: resource_locator.h:53
tesseract_common::ManipulatorInfo::tcp_frame
std::string tcp_frame
The coordinate frame within to the environment to use as the reference frame for the tool center poin...
Definition: manipulator_info.h:70
tesseract_common::loadYamlString
YAML::Node loadYamlString(const std::string &yaml_string, const ResourceLocator &locator)
Definition: yaml_utils.cpp:100
tesseract_common::TaskComposerPluginInfo::search_paths
std::set< std::string > search_paths
A list of paths to search for plugins.
Definition: plugin_info.h:175
tesseract_common::PluginInfoContainer::default_plugin
std::string default_plugin
Definition: plugin_info.h:83
tesseract_common::ManipulatorInfo::manipulator_ik_solver
std::string manipulator_ik_solver
(Optional) IK Solver to be used
Definition: manipulator_info.h:76
tesseract_common::getTimestampString
std::string getTimestampString()
Get Timestamp string.
Definition: utils.cpp:310
tesseract_common::CollisionMarginData::apply
void apply(const CollisionMarginPairData &pair_margin_data, CollisionMarginPairOverrideType override_type)
Apply the contents of the provide CollisionMarginPairData based on the override type.
Definition: collision_margin_data.cpp:222
tesseract_common::KinematicsPluginInfo
The kinematics plugin information structure.
Definition: plugin_info.h:98
tesseract_common::AnyPoly::as
T & as()
Definition: any_poly.h:181
createTestYamlWithIncludeDirectivesFile
void createTestYamlWithIncludeDirectivesFile(const std::string &filePath, const std::string &content)
Definition: tesseract_common_unit.cpp:3279
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::loadYamlFile
YAML::Node loadYamlFile(const std::string &file_path, const ResourceLocator &locator)
Loads a YAML file and processes !include directives recursively.
Definition: yaml_utils.cpp:93
tesseract_common::Profile
The Profile class.
Definition: profile.h:39
tesseract_common::enforceLimits< double >
template void enforceLimits< double >(Eigen::Ref< Eigen::Matrix< double, Eigen::Dynamic, 1 >> values, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 2 >> &limits)
tesseract_common::fromYAMLString
YAML::Node fromYAMLString(const std::string &string)
Converts yaml string to a YAML::Node.
Definition: yaml_utils.cpp:142
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::ProfileDictionary::hasProfileEntry
bool hasProfileEntry(std::size_t key, const std::string &ns) const
Check if a profile entry exists.
Definition: profile_dictionary.cpp:78
tesseract_common::AllowedCollisionMatrix::removeAllowedCollision
virtual void removeAllowedCollision(const std::string &link_name1, const std::string &link_name2)
Remove disabled collision pair from allowed collision matrix.
Definition: allowed_collision_matrix.cpp:79
tesseract_common::JointState::time
double time
The Time from start at the waypoint (optional)
Definition: joint_state.h:71
tesseract_common::CollisionMarginData::incrementMargins
void incrementMargins(double increment)
Increment all margins by input amount. Useful for inflating or reducing margins.
Definition: collision_margin_data.cpp:210
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::ProfileDictionary::clear
void clear()
Clear the dictionary.
Definition: profile_dictionary.cpp:261
TestHasMemberFunction::add
int add(int a) const
Definition: tesseract_common_unit.cpp:341
tesseract_common::satisfiesLimits< double >
template bool satisfiesLimits< double >(const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 1 >> &values, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 2 >> &limits, double max_diff, double max_rel_diff)
tesseract_common::ProfileDictionary
This class is used to store profiles used by various tasks.
Definition: profile_dictionary.h:46
CREATE_MEMBER_FUNC_INVOCABLE_CHECK
CREATE_MEMBER_FUNC_INVOCABLE_CHECK(update, std::shared_ptr< T > &)
runAnyPolyIntegralTest
void runAnyPolyIntegralTest(T value, const std::string &type_str)
Definition: tesseract_common_unit.cpp:611
TestMissingMemberFunction::add
double add(int a) const
Definition: tesseract_common_unit.cpp:353
tesseract_common::CollisionMarginPairData
Definition: collision_margin_data.h:68
tesseract_common::KinematicsPluginInfo::fwd_plugin_infos
std::map< std::string, tesseract_common::PluginInfoContainer > fwd_plugin_infos
A map of group name to forward kinematics plugin information.
Definition: plugin_info.h:107
tesseract_common::ContactManagersPluginInfo::search_libraries
std::set< std::string > search_libraries
A list of library names without the prefix or suffix that contain plugins.
Definition: plugin_info.h:141
TestProfile::~TestProfile
~TestProfile() override=default
tesseract_common::Stopwatch::elapsedSeconds
double elapsedSeconds() const
Get the elapsed time in seconds.
Definition: stopwatch.cpp:51
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
TestHasMemberFunction
Definition: tesseract_common_unit.cpp:338
tesseract_common::isNumeric
bool isNumeric(const std::string &s)
Determine if a string is a number.
Definition: utils.cpp:267
tesseract_common::AllowedCollisionMatrix
Definition: allowed_collision_matrix.h:28
tesseract_common::ContactManagersPluginInfo::insert
void insert(const ContactManagersPluginInfo &other)
Insert the content of an other ContactManagersPluginInfo.
Definition: plugin_info.cpp:180
resource_locator.h
Locate and retrieve resource data.
tesseract_common::Resource::Ptr
std::shared_ptr< Resource > Ptr
Definition: resource_locator.h:150
tesseract_common::AllowedCollisionMatrix::clearAllowedCollisions
void clearAllowedCollisions()
Clears the list of allowed collisions, so that no collision will be allowed.
Definition: allowed_collision_matrix.cpp:107
TestHasMemberFunction::update
bool update() const
Definition: tesseract_common_unit.cpp:340
tesseract_common::rtrim
void rtrim(std::string &s)
Right trim string.
Definition: utils.cpp:302
TestHasMemberWithArgFunction::update
bool update(std::shared_ptr< TestHasMemberWithArgFunction > &p)
Definition: tesseract_common_unit.cpp:346
tesseract_common::Stopwatch::elapsedMilliseconds
double elapsedMilliseconds() const
Get the elapsed time in milliseconds.
Definition: stopwatch.cpp:43
tesseract_common::ManipulatorInfo::manipulator
std::string manipulator
Name of the manipulator group.
Definition: manipulator_info.h:57
tesseract_common::ProfileDictionary::hasProfile
bool hasProfile(std::size_t key, const std::string &ns, const std::string &profile_name) const
Check if a profile exists.
Definition: profile_dictionary.cpp:216
tesseract_common::CollisionMarginData::setDefaultCollisionMargin
void setDefaultCollisionMargin(double default_collision_margin)
Set the default collision margin.
Definition: collision_margin_data.cpp:179
tesseract_common::Timer::stop
void stop()
Stop the timer.
Definition: timer.cpp:46
tesseract_common::makeOrderedLinkPair
LinkNamesPair makeOrderedLinkPair(const std::string &link_name1, const std::string &link_name2)
Create a pair of strings, where the pair.first is always <= pair.second.
Definition: types.cpp:44
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
kinematic_limits.h
Common Tesseract Kinematic Limits and Related Utility Functions.
TestMissingMemberFunction
Definition: tesseract_common_unit.cpp:350
tesseract_common::GeneralResourceLocator
A general resource loaders using environment variable.
Definition: resource_locator.h:84
tesseract_common::TaskComposerPluginInfo::clear
void clear()
Clear the contents.
Definition: plugin_info.cpp:259
tesseract_common::AllowedCollisionMatrix::addAllowedCollision
virtual void addAllowedCollision(const std::string &link_name1, const std::string &link_name2, const std::string &reason)
Disable collision between two collision objects.
Definition: allowed_collision_matrix.cpp:69
tesseract_common::ResourceLocator::Ptr
std::shared_ptr< ResourceLocator > Ptr
Definition: resource_locator.h:56
profile_dictionary.h
This is a profile dictionary for storing all profiles.
tesseract_common::toNumeric< double >
template bool toNumeric< double >(const std::string &, double &)
tesseract_common::ContactManagersPluginInfo::CONFIG_KEY
static const std::string CONFIG_KEY
Definition: plugin_info.h:159
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::ProfileDictionary::getProfileEntry
std::unordered_map< std::string, Profile::ConstPtr > getProfileEntry(std::size_t key, const std::string &ns) const
Get a profile entry.
Definition: profile_dictionary.cpp:101
tesseract_common::JointState::position
Eigen::VectorXd position
The joint position at the waypoint.
Definition: joint_state.h:59
runThrowNestedException
void runThrowNestedException()
Definition: tesseract_common_unit.cpp:1222
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::Stopwatch::start
void start()
Start the timer.
Definition: stopwatch.cpp:31
yaml_extenstions.h
YAML Type conversions.
tesseract_common::ManipulatorInfo::empty
bool empty() const
Check if any data is current being stored.
Definition: manipulator_info.cpp:72
TestMissingMemberFunction::missingUpdate
bool missingUpdate() const
Definition: tesseract_common_unit.cpp:352
tesseract_common::PluginInfoContainer::plugins
PluginInfoMap plugins
Definition: plugin_info.h:84
tesseract_common::Stopwatch
A simple stopwatch class leveraging chrono high resolution clock.
Definition: stopwatch.h:37
tesseract_common::CalibrationInfo::insert
void insert(const CalibrationInfo &other)
Insert the content of an other CalibrationInfo.
Definition: calibration_info.cpp:41
TestProfile
Definition: tesseract_common_serialization_unit.cpp:73
tesseract_common::ContactManagersPluginInfo::search_paths
std::set< std::string > search_paths
A list of paths to search for plugins.
Definition: plugin_info.h:138
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::KinematicsPluginInfo::search_libraries
std::set< std::string > search_libraries
A list of library names without the prefix or suffix that contain plugins.
Definition: plugin_info.h:104
tesseract_common::ProfileDictionary::addProfile
void addProfile(const std::string &ns, const std::string &profile_name, const Profile::ConstPtr &profile)
Add a profile.
Definition: profile_dictionary.cpp:124
tesseract_common::ManipulatorInfo::working_frame
std::string working_frame
The working frame to which waypoints are relative.
Definition: manipulator_info.h:64
stopwatch.h
Simple stopwatch class using chrono.
tesseract_common::KinematicsPluginInfo::CONFIG_KEY
static const std::string CONFIG_KEY
Definition: plugin_info.h:122
tesseract_common::isWithinLimits< double >
template bool isWithinLimits< double >(const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 1 >> &values, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 2 >> &limits)
tesseract_common::PairsCollisionMarginData
std::unordered_map< tesseract_common::LinkNamesPair, double, tesseract_common::PairHash > PairsCollisionMarginData
Definition: collision_margin_data.h:66
tesseract_common::KinematicsPluginInfo::empty
bool empty() const
Check if structure is empty.
Definition: plugin_info.cpp:149
tesseract_common::AnyPoly::isNull
bool isNull() const
Check if the poly type is null.
Definition: any_poly.cpp:79
tesseract_common::CollisionMarginPairOverrideType::NONE
@ NONE
Do not apply contact margin data.

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