tesseract_srdf_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 <iostream>
5 #include <fstream>
6 #include <yaml-cpp/yaml.h>
7 #include <tinyxml2.h>
8 TESSERACT_COMMON_IGNORE_WARNINGS_POP
9 
10 #include <tesseract_common/utils.h>
11 #include <tesseract_common/resource_locator.h>
12 #include <tesseract_common/collision_margin_data.h>
13 #include <tesseract_common/yaml_utils.h>
14 #include <tesseract_common/yaml_extenstions.h>
15 #include <tesseract_srdf/collision_margins.h>
16 #include <tesseract_srdf/configs.h>
17 #include <tesseract_srdf/disabled_collisions.h>
18 #include <tesseract_srdf/group_states.h>
19 #include <tesseract_srdf/group_tool_center_points.h>
20 #include <tesseract_srdf/groups.h>
21 #include <tesseract_srdf/srdf_model.h>
22 #include <tesseract_srdf/utils.h>
23 #include <tesseract_scene_graph/graph.h>
24 #include <tesseract_scene_graph/link.h>
25 #include <tesseract_scene_graph/joint.h>
26 
27 enum class ABBConfig : std::uint8_t
28 {
29  ROBOT_ONLY,
30  ROBOT_ON_RAIL,
31  ROBOT_WITH_POSITIONER
32 };
33 
34 tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBConfig::ROBOT_ONLY)
35 {
36  using namespace tesseract_scene_graph;
37  using namespace tesseract_srdf;
38 
39  auto g = std::make_shared<SceneGraph>();
40 
41  g->setName("abb_irb2400");
42 
43  Link base_link("base_link");
44  Link link_1("link_1");
45  Link link_2("link_2");
46  Link link_3("link_3");
47  Link link_4("link_4");
48  Link link_5("link_5");
49  Link link_6("link_6");
50  Link tool0("tool0");
51 
52  EXPECT_TRUE(g->addLink(base_link));
53  EXPECT_TRUE(g->addLink(link_1));
54  EXPECT_TRUE(g->addLink(link_2));
55  EXPECT_TRUE(g->addLink(link_3));
56  EXPECT_TRUE(g->addLink(link_4));
57  EXPECT_TRUE(g->addLink(link_5));
58  EXPECT_TRUE(g->addLink(link_6));
59  EXPECT_TRUE(g->addLink(tool0));
60 
61  if (config == ABBConfig::ROBOT_ON_RAIL)
62  {
63  EXPECT_TRUE(g->addLink(Link("world")));
64  EXPECT_TRUE(g->addLink(Link("axis_1")));
65  EXPECT_TRUE(g->addLink(Link("axis_2")));
66 
67  Joint joint_a("joint_axis_1");
68  joint_a.axis = Eigen::Vector3d(0, 1, 0);
69  joint_a.parent_link_name = "world";
70  joint_a.child_link_name = "axis_1";
71  joint_a.type = JointType::PRISMATIC;
72  joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
73  EXPECT_TRUE(g->addJoint(joint_a));
74 
75  Joint joint_b("joint_axis_2");
76  joint_b.axis = Eigen::Vector3d(1, 0, 0);
77  joint_b.parent_link_name = "axis_1";
78  joint_b.child_link_name = "axis_2";
79  joint_b.type = JointType::PRISMATIC;
80  joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
81  EXPECT_TRUE(g->addJoint(joint_b));
82 
83  Joint joint_c("joint_base_link");
84  joint_c.axis = Eigen::Vector3d(0, 1, 0);
85  joint_c.parent_link_name = "axis_1";
86  joint_c.child_link_name = "base_link";
87  joint_c.type = JointType::FIXED;
88  EXPECT_TRUE(g->addJoint(joint_c));
89  }
90  else if (config == ABBConfig::ROBOT_WITH_POSITIONER)
91  {
92  EXPECT_TRUE(g->addLink(Link("world")));
93  EXPECT_TRUE(g->addLink(Link("axis_1")));
94  EXPECT_TRUE(g->addLink(Link("axis_2")));
95 
96  Joint joint_a("joint_axis_1");
97  joint_a.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(1, 0, 0);
98  joint_a.axis = Eigen::Vector3d(0, 1, 0);
99  joint_a.parent_link_name = "world";
100  joint_a.child_link_name = "axis_1";
101  joint_a.type = JointType::PRISMATIC;
102  joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
103  EXPECT_TRUE(g->addJoint(joint_a));
104 
105  Joint joint_b("joint_axis_2");
106  joint_b.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(1, 0, 0);
107  joint_b.axis = Eigen::Vector3d(1, 0, 0);
108  joint_b.parent_link_name = "axis_1";
109  joint_b.child_link_name = "axis_2";
110  joint_b.type = JointType::PRISMATIC;
111  joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
112  EXPECT_TRUE(g->addJoint(joint_b));
113 
114  Joint joint_c("joint_base_link");
115  joint_c.parent_link_name = "world";
116  joint_c.child_link_name = "base_link";
117  joint_c.type = JointType::FIXED;
118  EXPECT_TRUE(g->addJoint(joint_c));
119  }
120 
121  Joint joint_1("joint_1");
122  joint_1.parent_link_name = "base_link";
123  joint_1.child_link_name = "link_1";
124  joint_1.type = JointType::FIXED;
125  EXPECT_TRUE(g->addJoint(joint_1));
126 
127  Joint joint_2("joint_2");
128  joint_2.parent_link_name = "link_1";
129  joint_2.child_link_name = "link_2";
130  joint_2.type = JointType::REVOLUTE;
131  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
132  EXPECT_TRUE(g->addJoint(joint_2));
133 
134  Joint joint_3("joint_3");
135  joint_3.parent_to_joint_origin_transform.translation()(0) = 1.25;
136  joint_3.parent_link_name = "link_2";
137  joint_3.child_link_name = "link_3";
138  joint_3.type = JointType::REVOLUTE;
139  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
140  EXPECT_TRUE(g->addJoint(joint_3));
141 
142  Joint joint_4("joint_4");
143  joint_4.parent_to_joint_origin_transform.translation()(0) = 1.25;
144  joint_4.parent_link_name = "link_3";
145  joint_4.child_link_name = "link_4";
146  joint_4.type = JointType::REVOLUTE;
147  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
148  EXPECT_TRUE(g->addJoint(joint_4));
149 
150  Joint joint_5("joint_5");
151  joint_5.parent_to_joint_origin_transform.translation()(1) = 1.25;
152  joint_5.parent_link_name = "link_4";
153  joint_5.child_link_name = "link_5";
154  joint_5.type = JointType::REVOLUTE;
155  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
156  EXPECT_TRUE(g->addJoint(joint_5));
157 
158  Joint joint_6("joint_6");
159  joint_6.parent_to_joint_origin_transform.translation()(1) = 1.25;
160  joint_6.parent_link_name = "link_5";
161  joint_6.child_link_name = "link_6";
162  joint_6.type = JointType::REVOLUTE;
163  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
164  EXPECT_TRUE(g->addJoint(joint_6));
165 
166  Joint joint_tool0("joint_tool0");
167  joint_tool0.parent_link_name = "link_6";
168  joint_tool0.child_link_name = "tool0";
169  joint_tool0.type = JointType::FIXED;
170  EXPECT_TRUE(g->addJoint(joint_tool0));
171 
172  return g;
173 }
174 
175 tesseract_scene_graph::SceneGraph buildTestSceneGraph()
176 {
177  using namespace tesseract_scene_graph;
178  SceneGraph g;
179 
180  Link base_link("base_link");
181  Link link_1("link_1");
182  Link link_2("link_2");
183  Link link_3("link_3");
184  Link link_4("link_4");
185  Link link_5("link_5");
186 
187  EXPECT_TRUE(g.addLink(base_link));
188  EXPECT_TRUE(g.addLink(link_1));
189  EXPECT_TRUE(g.addLink(link_2));
190  EXPECT_TRUE(g.addLink(link_3));
191  EXPECT_TRUE(g.addLink(link_4));
192  EXPECT_TRUE(g.addLink(link_5));
193 
194  Joint base_joint("base_joint");
195  base_joint.parent_link_name = "base_link";
196  base_joint.child_link_name = "link_1";
197  base_joint.type = JointType::FIXED;
198  EXPECT_TRUE(g.addJoint(base_joint));
199 
200  Joint joint_1("joint_1");
201  joint_1.parent_link_name = "link_1";
202  joint_1.child_link_name = "link_2";
203  joint_1.type = JointType::REVOLUTE;
204  joint_1.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
205  EXPECT_TRUE(g.addJoint(joint_1));
206 
207  Joint joint_2("joint_2");
208  joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
209  joint_2.parent_link_name = "link_2";
210  joint_2.child_link_name = "link_3";
211  joint_2.type = JointType::REVOLUTE;
212  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
213  EXPECT_TRUE(g.addJoint(joint_2));
214 
215  Joint joint_3("joint_3");
216  joint_3.parent_to_joint_origin_transform.translation()(0) = 1.25;
217  joint_3.parent_link_name = "link_3";
218  joint_3.child_link_name = "link_4";
219  joint_3.type = JointType::REVOLUTE;
220  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
221  EXPECT_TRUE(g.addJoint(joint_3));
222 
223  Joint joint_4("joint_4");
224  joint_4.parent_to_joint_origin_transform.translation()(1) = 1.25;
225  joint_4.parent_link_name = "link_2";
226  joint_4.child_link_name = "link_5";
227  joint_4.type = JointType::REVOLUTE;
228  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
229  EXPECT_TRUE(g.addJoint(joint_4));
230 
231  return g;
232 }
233 
234 TEST(TesseractSRDFUnit, LoadSRDFFileUnit) // NOLINT
235 {
236  using namespace tesseract_scene_graph;
237  using namespace tesseract_srdf;
238  using namespace tesseract_common;
239 
240  GeneralResourceLocator locator;
241  std::string srdf_file =
242  locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath();
243 
244  SceneGraph g;
245 
246  g.setName("kuka_lbr_iiwa_14_r820");
247 
248  Link base_link("base_link");
249  Link link_1("link_1");
250  Link link_2("link_2");
251  Link link_3("link_3");
252  Link link_4("link_4");
253  Link link_5("link_5");
254  Link link_6("link_6");
255  Link link_7("link_7");
256  Link tool0("tool0");
257 
258  EXPECT_TRUE(g.addLink(base_link));
259  EXPECT_TRUE(g.addLink(link_1));
260  EXPECT_TRUE(g.addLink(link_2));
261  EXPECT_TRUE(g.addLink(link_3));
262  EXPECT_TRUE(g.addLink(link_4));
263  EXPECT_TRUE(g.addLink(link_5));
264  EXPECT_TRUE(g.addLink(link_6));
265  EXPECT_TRUE(g.addLink(link_7));
266  EXPECT_TRUE(g.addLink(tool0));
267 
268  Joint joint_1("joint_a1");
269  joint_1.parent_link_name = "base_link";
270  joint_1.child_link_name = "link_1";
271  joint_1.type = JointType::FIXED;
272  EXPECT_TRUE(g.addJoint(joint_1));
273 
274  Joint joint_2("joint_a2");
275  joint_2.parent_link_name = "link_1";
276  joint_2.child_link_name = "link_2";
277  joint_2.type = JointType::REVOLUTE;
278  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
279  EXPECT_TRUE(g.addJoint(joint_2));
280 
281  Joint joint_3("joint_a3");
282  joint_3.parent_to_joint_origin_transform.translation()(0) = 1.25;
283  joint_3.parent_link_name = "link_2";
284  joint_3.child_link_name = "link_3";
285  joint_3.type = JointType::REVOLUTE;
286  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
287  EXPECT_TRUE(g.addJoint(joint_3));
288 
289  Joint joint_4("joint_a4");
290  joint_4.parent_to_joint_origin_transform.translation()(0) = 1.25;
291  joint_4.parent_link_name = "link_3";
292  joint_4.child_link_name = "link_4";
293  joint_4.type = JointType::REVOLUTE;
294  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
295  EXPECT_TRUE(g.addJoint(joint_4));
296 
297  Joint joint_5("joint_a5");
298  joint_5.parent_to_joint_origin_transform.translation()(1) = 1.25;
299  joint_5.parent_link_name = "link_4";
300  joint_5.child_link_name = "link_5";
301  joint_5.type = JointType::REVOLUTE;
302  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
303  EXPECT_TRUE(g.addJoint(joint_5));
304 
305  Joint joint_6("joint_a6");
306  joint_6.parent_to_joint_origin_transform.translation()(1) = 1.25;
307  joint_6.parent_link_name = "link_5";
308  joint_6.child_link_name = "link_6";
309  joint_6.type = JointType::REVOLUTE;
310  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
311  EXPECT_TRUE(g.addJoint(joint_6));
312 
313  Joint joint_7("joint_a7");
314  joint_7.parent_to_joint_origin_transform.translation()(1) = 1.25;
315  joint_7.parent_link_name = "link_6";
316  joint_7.child_link_name = "link_7";
317  joint_7.type = JointType::REVOLUTE;
318  joint_7.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
319  EXPECT_TRUE(g.addJoint(joint_7));
320 
321  Joint joint_tool0("joint_tool0");
322  joint_tool0.parent_link_name = "link_7";
323  joint_tool0.child_link_name = "tool0";
324  joint_tool0.type = JointType::FIXED;
325  EXPECT_TRUE(g.addJoint(joint_tool0));
326 
327  SRDFModel srdf;
328  srdf.initFile(g, srdf_file, locator);
329  EXPECT_EQ(srdf.name, "kuka_lbr_iiwa_14_r820");
330  EXPECT_EQ(srdf.version[0], 1);
331  EXPECT_EQ(srdf.version[1], 0);
332  EXPECT_EQ(srdf.version[2], 0);
333 
334  processSRDFAllowedCollisions(g, srdf);
335 
336  tesseract_common::AllowedCollisionMatrix::ConstPtr acm = g.getAllowedCollisionMatrix();
337 
338  // collision between link1 and link2 should be allowed
339  EXPECT_TRUE(acm->isCollisionAllowed("link_1", "link_2"));
340 
341  // collision between link1 and link2 should be allowed
342  EXPECT_FALSE(acm->isCollisionAllowed("base_link", "link_5"));
343 
344  g.removeAllowedCollision("link_1", "link_2");
345  // now collision link1 and link2 is not allowed anymore
346  EXPECT_FALSE(acm->isCollisionAllowed("link_1", "link_2"));
347 
348  g.clearAllowedCollisions();
349  EXPECT_EQ(acm->getAllAllowedCollisions().size(), 0);
350 }
351 
352 TEST(TesseractSRDFUnit, TesseractSRDFModelUnit) // NOLINT
353 {
354  using namespace tesseract_scene_graph;
355  using namespace tesseract_srdf;
356  using namespace tesseract_common;
357 
358  GeneralResourceLocator locator;
359  SRDFModel srdf;
360 
361  // Set Name
362  srdf.name = "test_srdf";
363  EXPECT_TRUE(srdf.name == "test_srdf");
364 
365  // Add chain groups
366  auto& chain_groups = srdf.kinematics_information.chain_groups;
367  EXPECT_TRUE(chain_groups.empty());
368 
369  chain_groups["manipulator_chain"] = { std::make_pair("base_link", "link_5") };
370  EXPECT_FALSE(srdf.kinematics_information.chain_groups.empty());
371 
372  // Add joint groups
373  auto& joint_groups = srdf.kinematics_information.joint_groups;
374  EXPECT_TRUE(joint_groups.empty());
375 
376  joint_groups["manipulator_joint"] = { "joint_1", "joint_2", "joint_3", "joint_4" };
377  EXPECT_FALSE(srdf.kinematics_information.joint_groups.empty());
378 
379  // Add link groups
380  auto& link_groups = srdf.kinematics_information.link_groups;
381  EXPECT_TRUE(link_groups.empty());
382  link_groups["manipulator_link"] = { "base_link", "link_1", "link_2", "link_3", "link_4", "link_5" };
383  EXPECT_FALSE(srdf.kinematics_information.link_groups.empty());
384 
385  // Add group states
386  auto& group_state = srdf.kinematics_information.group_states;
387  EXPECT_TRUE(group_state.empty());
388  GroupsJointState joint_state;
389  joint_state["joint_1"] = 0;
390  joint_state["joint_2"] = 0;
391  joint_state["joint_3"] = 0;
392  joint_state["joint_4"] = 0;
393  group_state["manipulator_chain"]["All Zeros"] = joint_state;
394  group_state["manipulator_joint"]["All Zeros"] = joint_state;
395  group_state["manipulator_link"]["All Zeros"] = joint_state;
396  EXPECT_EQ(srdf.kinematics_information.group_states.size(), 3);
397 
398  // Add Tool Center Points
399  auto& group_tcps = srdf.kinematics_information.group_tcps;
400  EXPECT_TRUE(group_tcps.empty());
401  group_tcps["manipulator_chain"]["laser"] = Eigen::Isometry3d::Identity();
402  group_tcps["manipulator_joint"]["laser"] = Eigen::Isometry3d::Identity();
403  group_tcps["manipulator_link"]["laser"] = Eigen::Isometry3d::Identity();
404  EXPECT_FALSE(srdf.kinematics_information.group_tcps.empty());
405 
406  // Add disabled collisions
407  auto& acm = srdf.acm;
408  EXPECT_TRUE(acm.getAllAllowedCollisions().empty());
409  acm.addAllowedCollision("link_1", "link_3", "Adjacent");
410  acm.addAllowedCollision("link_1", "link_2", "Adjacent");
411  acm.addAllowedCollision("link_2", "link_3", "Adjacent");
412  acm.addAllowedCollision("link_3", "link_4", "Adjacent");
413  acm.addAllowedCollision("link_4", "link_5", "Adjacent");
414  acm.addAllowedCollision("base_link", "link_1", "Adjacent");
415  EXPECT_FALSE(acm.getAllAllowedCollisions().empty());
416  srdf.saveToFile(tesseract_common::getTempPath() + "test.srdf");
417 
418  SceneGraph g = buildTestSceneGraph();
419 
420  SRDFModel srdf_reload;
421  srdf_reload.initFile(g, tesseract_common::getTempPath() + "test.srdf", locator);
422  EXPECT_TRUE(srdf_reload.name == "test_srdf");
423  EXPECT_FALSE(srdf_reload.kinematics_information.chain_groups.empty());
424  EXPECT_FALSE(srdf_reload.kinematics_information.joint_groups.empty());
425  EXPECT_FALSE(srdf_reload.kinematics_information.link_groups.empty());
426  EXPECT_EQ(srdf_reload.kinematics_information.group_states.size(), 3);
427  EXPECT_TRUE(srdf_reload.kinematics_information.group_states["manipulator_chain"].find("All Zeros") !=
428  srdf_reload.kinematics_information.group_states["manipulator_chain"].end());
429  EXPECT_TRUE(srdf_reload.kinematics_information.group_states["manipulator_joint"].find("All Zeros") !=
430  srdf_reload.kinematics_information.group_states["manipulator_joint"].end());
431  EXPECT_TRUE(srdf_reload.kinematics_information.group_states["manipulator_link"].find("All Zeros") !=
432  srdf_reload.kinematics_information.group_states["manipulator_link"].end());
433  EXPECT_FALSE(srdf_reload.kinematics_information.group_tcps.empty());
434  EXPECT_TRUE(srdf_reload.kinematics_information.group_tcps["manipulator_chain"].find("laser") !=
435  srdf_reload.kinematics_information.group_tcps["manipulator_chain"].end());
436  EXPECT_TRUE(srdf_reload.kinematics_information.group_tcps["manipulator_joint"].find("laser") !=
437  srdf_reload.kinematics_information.group_tcps["manipulator_joint"].end());
438  EXPECT_TRUE(srdf_reload.kinematics_information.group_tcps["manipulator_link"].find("laser") !=
439  srdf_reload.kinematics_information.group_tcps["manipulator_link"].end());
440  EXPECT_FALSE(srdf_reload.acm.getAllAllowedCollisions().empty());
441  srdf_reload.saveToFile(tesseract_common::getTempPath() + "test_reload.srdf");
442 }
443 
444 TEST(TesseractSRDFUnit, LoadSRDFFailureCasesUnit) // NOLINT
445 {
446  using namespace tesseract_scene_graph;
447  using namespace tesseract_srdf;
448  using namespace tesseract_common;
449 
450  GeneralResourceLocator locator;
451  SceneGraph::Ptr g = getABBSceneGraph();
452 
453  { // Success
454  std::string xml_string =
455  R"(<robot name="abb_irb2400" version="1.0.0">
456  <group name="manipulator">
457  <chain base_link="base_link" tip_link="tool0" />
458  </group>
459  </robot>)";
460 
461  SRDFModel srdf;
462  srdf.initString(*g, xml_string, locator);
463  EXPECT_EQ(srdf.name, "abb_irb2400");
464  EXPECT_EQ(srdf.version[0], 1);
465  EXPECT_EQ(srdf.version[1], 0);
466  EXPECT_EQ(srdf.version[2], 0);
467  }
468 
469  { // Success version with no patch
470  std::string xml_string =
471  R"(<robot name="abb_irb2400" version="1.0">
472  <group name="manipulator">
473  <chain base_link="base_link" tip_link="tool0" />
474  </group>
475  </robot>)";
476 
477  SRDFModel srdf;
478  srdf.initString(*g, xml_string, locator);
479  EXPECT_EQ(srdf.name, "abb_irb2400");
480  EXPECT_EQ(srdf.version[0], 1);
481  EXPECT_EQ(srdf.version[1], 0);
482  EXPECT_EQ(srdf.version[2], 0);
483  }
484 
485  // Now test failures
486  { // missing name
487  std::string xml_string =
488  R"(<robot version="1.0.0">
489  <group name="manipulator">
490  <chain base_link="base_link" tip_link="tool0" />
491  </group>
492  </robot>)";
493 
494  SRDFModel srdf;
495  EXPECT_ANY_THROW(srdf.initString(*g, xml_string, locator)); // NOLINT
496  }
497  { // invalid version
498  std::string xml_string =
499  R"(<robot name="abb_irb2400" version="1 0 0">
500  <group name="manipulator">
501  <chain base_link="base_link" tip_link="tool0" />
502  </group>
503  </robot>)";
504 
505  SRDFModel srdf;
506  EXPECT_ANY_THROW(srdf.initString(*g, xml_string, locator)); // NOLINT
507  }
508  { // initXml missing name element
509  std::string xml_string =
510  R"(<robot version="1.0.0">
511  <group name="manipulator">
512  <chain base_link="base_link" tip_link="tool0" />
513  </group>
514  </missing_robot>)";
515 
516  SRDFModel srdf;
517  EXPECT_ANY_THROW(srdf.initString(*g, xml_string, locator)); // NOLINT
518  }
519  { // invalid xml
520  std::string xml_string =
521  R"(<robot name="abb_irb2400" version="1 0 0">
522  <group name="manipulator">
523  <chain base_link="base_link" tip_link="tool0" />
524  </group>
525  </robot_invalid>)";
526 
527  SRDFModel srdf;
528  EXPECT_ANY_THROW(srdf.initString(*g, xml_string, locator)); // NOLINT
529  }
530  { // initXml missing robot element
531  std::string xml_string =
532  R"(<missing_robot name="abb_irb2400" version="1.0.0">
533  <group name="manipulator">
534  <chain base_link="base_link" tip_link="tool0" />
535  </group>
536  </missing_robot>)";
537 
538  SRDFModel srdf;
539  EXPECT_ANY_THROW(srdf.initString(*g, xml_string, locator)); // NOLINT
540  }
541  { // initFile file path does not exist
542  SRDFModel srdf;
543  EXPECT_ANY_THROW(srdf.initFile(*g, "/tmp/file_does_not_exist.srdf", locator)); // NOLINT
544  }
545 }
546 
547 class TempResourceLocator : public tesseract_common::ResourceLocator
548 {
549 public:
550  std::shared_ptr<tesseract_common::Resource> locateResource(const std::string& url) const override final
551  {
552  std::filesystem::path mod_url(url);
553  if (!mod_url.is_absolute())
554  {
555  mod_url = std::filesystem::path(tesseract_common::getTempPath() + url);
556  }
557 
558  return std::make_shared<tesseract_common::SimpleLocatedResource>(
559  url, mod_url.string(), std::make_shared<TempResourceLocator>(*this));
560  }
561 };
562 
563 TEST(TesseractSRDFUnit, LoadSRDFSaveUnit) // NOLINT
564 {
565  using namespace tesseract_scene_graph;
566  using namespace tesseract_srdf;
567  using namespace tesseract_common;
568 
569  SceneGraph::Ptr g = getABBSceneGraph(ABBConfig::ROBOT_ON_RAIL);
570  TempResourceLocator locator;
571 
572  std::string xml_string =
573  R"(<robot name="abb_irb2400" version="1.0.0">
574  <group name="manipulator">
575  <chain base_link="base_link" tip_link="tool0" />
576  </group>
577  <group name="positioner">
578  <chain base_link="world" tip_link="base_link" />
579  </group>
580  <group name="gantry">
581  <chain base_link="world" tip_link="tool0" />
582  </group>
583 
584  <group name="manipulator_joint">
585  <joint name="joint_1"/>
586  <joint name="joint_2"/>
587  <joint name="joint_3"/>
588  <joint name="joint_4"/>
589  <joint name="joint_5"/>
590  <joint name="joint_6"/>
591  <joint name="joint_tool0"/>
592  </group>
593 
594  <group_state name="all-zeros" group="manipulator">
595  <joint name="joint_1" value="0"/>
596  <joint name="joint_2" value="0"/>
597  <joint name="joint_3" value="0"/>
598  <joint name="joint_4" value="0"/>
599  <joint name="joint_5" value="0"/>
600  <joint name="joint_6" value="0"/>
601  </group_state>
602 
603  <group_tcps group="gantry">
604  <tcp name="laser" xyz="1 .1 1" rpy="0 1.57 0" />
605  <tcp name="welder" xyz=".1 1 .2" wxyz="1 0 0 0" />
606  </group_tcps>
607 
608  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
609  <disable_collisions link1="base_link" link2="link_2" reason="Never" />
610  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
611 
612  <collision_margins default_margin="0.025">
613  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
614  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
615  </collision_margins>
616  </robot>)";
617 
618  std::string yaml_kin_plugins_string =
619  R"(kinematic_plugins:
620  fwd_kin_plugins:
621  manipulator:
622  default: KDLFwdKinChain
623  plugins:
624  KDLFwdKinChain:
625  class: KDLFwdKinChainFactory
626  default: true
627  config:
628  base_link: base_link
629  tip_link: tool0
630  inv_kin_plugins:
631  manipulator:
632  default: KDLInvKinChainLMA
633  plugins:
634  KDLInvKinChainLMA:
635  class: KDLInvKinChainLMAFactory
636  default: true
637  config:
638  base_link: base_link
639  tip_link: tool0
640  KDLInvKinChainNR:
641  class: KDLInvKinChainNRFactory
642  config:
643  base_link: base_link
644  tip_link: tool0)";
645 
646  std::string yaml_cm_plugins_string =
647  R"(contact_manager_plugins:
648  search_paths:
649  - /usr/local/lib
650  search_libraries:
651  - tesseract_collision_bullet_factories
652  - tesseract_collision_fcl_factories
653  discrete_plugins:
654  default: BulletDiscreteBVHManager
655  plugins:
656  BulletDiscreteBVHManager:
657  class: BulletDiscreteBVHManagerFactory
658  default: true
659  BulletDiscreteSimpleManager:
660  class: BulletDiscreteSimpleManagerFactory
661  FCLDiscreteBVHManager:
662  class: FCLDiscreteBVHManagerFactory
663  continuous_plugins:
664  default: BulletCastBVHManager
665  plugins:
666  BulletCastBVHManager:
667  class: BulletCastBVHManagerFactory
668  default: true
669  BulletCastSimpleManager:
670  class: BulletCastSimpleManagerFactory)";
671 
672  std::string yaml_calibration_string =
673  R"(calibration:
674  joints:
675  joint_1:
676  position:
677  x: 1
678  y: 2
679  z: 3
680  orientation:
681  x: 0
682  y: 0
683  z: 0
684  w: 1
685  joint_2:
686  position:
687  x: 4
688  y: 5
689  z: 6
690  orientation:
691  x: 0
692  y: 0
693  z: 0
694  w: 1)";
695 
696  SRDFModel srdf_save;
697  srdf_save.initString(*g, xml_string, locator);
698 
699  YAML::Node kinematics_plugin_config = tesseract_common::loadYamlString(yaml_kin_plugins_string, locator);
700  srdf_save.kinematics_information.kinematics_plugin_info =
701  kinematics_plugin_config[KinematicsPluginInfo::CONFIG_KEY].as<KinematicsPluginInfo>();
702 
703  YAML::Node contact_managers_plugin_config = tesseract_common::loadYamlString(yaml_cm_plugins_string, locator);
704  srdf_save.contact_managers_plugin_info =
705  contact_managers_plugin_config[ContactManagersPluginInfo::CONFIG_KEY].as<ContactManagersPluginInfo>();
706 
707  YAML::Node calibration_config = tesseract_common::loadYamlString(yaml_calibration_string, locator);
708  srdf_save.calibration_info = calibration_config[CalibrationInfo::CONFIG_KEY].as<CalibrationInfo>();
709 
710  std::string save_path = tesseract_common::getTempPath() + "unit_test_save_srdf.srdf";
711  EXPECT_TRUE(srdf_save.saveToFile(save_path));
712 
713  SRDFModel srdf;
714  srdf.initFile(*g, save_path, locator);
715  EXPECT_EQ(srdf.name, "abb_irb2400");
716  EXPECT_EQ(srdf.version[0], 1);
717  EXPECT_EQ(srdf.version[1], 0);
718  EXPECT_EQ(srdf.version[2], 0);
719 
720  EXPECT_FALSE(srdf.kinematics_information.kinematics_plugin_info.empty());
721  EXPECT_FALSE(srdf.contact_managers_plugin_info.empty());
722  EXPECT_FALSE(srdf.calibration_info.empty());
723 
724  processSRDFAllowedCollisions(*g, srdf);
725 
726  KinematicsInformation& kin_info = srdf.kinematics_information;
727 
728  // Check for tcp information
729  EXPECT_EQ(kin_info.group_tcps.size(), 1);
730  auto tcp_it = kin_info.group_tcps.find("gantry");
731  EXPECT_TRUE(tcp_it != kin_info.group_tcps.end());
732  EXPECT_EQ(tcp_it->second.size(), 2);
733  EXPECT_TRUE(tcp_it->second.find("laser") != tcp_it->second.end());
734  EXPECT_TRUE(tcp_it->second.find("welder") != tcp_it->second.end());
735 
736  // Check for chain group information
737  EXPECT_EQ(kin_info.chain_groups.size(), 3);
738  auto chain_gantry_it = kin_info.chain_groups.find("gantry");
739  auto chain_manipulator_it = kin_info.chain_groups.find("manipulator");
740  auto chain_positioner_it = kin_info.chain_groups.find("positioner");
741  EXPECT_TRUE(chain_gantry_it != kin_info.chain_groups.end());
742  EXPECT_TRUE(chain_manipulator_it != kin_info.chain_groups.end());
743  EXPECT_TRUE(chain_positioner_it != kin_info.chain_groups.end());
744 
745  // Check for joint group information
746  EXPECT_EQ(kin_info.joint_groups.size(), 1);
747  auto joint_manipulator_it = kin_info.joint_groups.find("manipulator_joint");
748  EXPECT_TRUE(joint_manipulator_it != kin_info.joint_groups.end());
749 
750  // Check for group states information
751  EXPECT_EQ(kin_info.group_states.size(), 1);
752  auto group_state_it = kin_info.group_states.find("manipulator");
753  EXPECT_TRUE(group_state_it != kin_info.group_states.end());
754  EXPECT_EQ(group_state_it->second.size(), 1);
755  EXPECT_TRUE(group_state_it->second.find("all-zeros") != group_state_it->second.end());
756 
757  tesseract_common::AllowedCollisionMatrix::ConstPtr acm = g->getAllowedCollisionMatrix();
758  EXPECT_TRUE(acm->isCollisionAllowed("base_link", "link_1"));
759  EXPECT_TRUE(acm->isCollisionAllowed("base_link", "link_2"));
760  EXPECT_TRUE(acm->isCollisionAllowed("base_link", "link_3"));
761 
762  EXPECT_TRUE(srdf.collision_margin_data != nullptr);
763  EXPECT_NEAR(srdf.collision_margin_data->getDefaultCollisionMargin(), 0.025, 1e-6);
764  EXPECT_NEAR(srdf.collision_margin_data->getMaxCollisionMargin(), 0.025, 1e-6);
765  EXPECT_EQ(srdf.collision_margin_data->getCollisionMarginPairData().getCollisionMargins().size(), 2);
766  EXPECT_NEAR(srdf.collision_margin_data->getCollisionMargin("link_5", "link_6"), 0.01, 1e-6);
767  EXPECT_NEAR(srdf.collision_margin_data->getCollisionMargin("link_5", "link_4"), 0.015, 1e-6);
768 
769  // Calibration failure joint does not exist
770  yaml_calibration_string =
771  R"(calibration:
772  joints:
773  does_not_exist:
774  position:
775  x: 1
776  y: 2
777  z: 3
778  orientation:
779  x: 0
780  y: 0
781  z: 0
782  w: 1
783  joint_2:
784  position:
785  x: 4
786  y: 5
787  z: 6
788  orientation:
789  x: 0
790  y: 0
791  z: 0
792  w: 1)";
793  YAML::Node bad_calibration_config = tesseract_common::loadYamlString(yaml_calibration_string, locator);
794  srdf_save.calibration_info = bad_calibration_config[CalibrationInfo::CONFIG_KEY].as<CalibrationInfo>();
795 
796  save_path = tesseract_common::getTempPath() + "unit_test_save_bad_srdf.srdf";
797  EXPECT_TRUE(srdf_save.saveToFile(save_path));
798 
799  SRDFModel bad_srdf;
800  EXPECT_ANY_THROW(bad_srdf.initFile(*g, save_path, locator)); // NOLINT
801 }
802 
803 TEST(TesseractSRDFUnit, LoadSRDFAllowedCollisionMatrixUnit) // NOLINT
804 {
805  using namespace tesseract_scene_graph;
806  using namespace tesseract_srdf;
807 
808  SceneGraph::Ptr g = getABBSceneGraph();
809  TempResourceLocator locator;
810 
811  std::string xml_string =
812  R"(<robot name="abb_irb2400">
813  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
814  <disable_collisions link1="base_link" link2="link_2" reason="Never" />
815  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
816  </robot>)";
817  tinyxml2::XMLDocument xml_doc;
818  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
819 
820  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
821  EXPECT_TRUE(element != nullptr);
822 
823  tesseract_common::AllowedCollisionMatrix acm = parseDisabledCollisions(*g, element, std::array<int, 3>({ 1, 0, 0 }));
824  EXPECT_TRUE(acm.isCollisionAllowed("base_link", "link_1"));
825  EXPECT_TRUE(acm.isCollisionAllowed("base_link", "link_2"));
826  EXPECT_TRUE(acm.isCollisionAllowed("base_link", "link_3"));
827 
828  // Now test failures
829  auto is_failure = [g](const std::string& xml_string) {
830  tinyxml2::XMLDocument xml_doc;
831  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
832 
833  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
834  EXPECT_TRUE(element != nullptr);
835 
836  try
837  {
838  parseDisabledCollisions(*g, element, std::array<int, 3>({ 1, 0, 0 }));
839  }
840  catch (const std::exception& e)
841  {
842  tesseract_common::printNestedException(e);
843  return true;
844  }
845  return false;
846  };
847 
848  { // missing link1
849  std::string xml_string =
850  R"(<robot name="abb_irb2400">
851  <disable_collisions link2="link_1" reason="Adjacent" />
852  <disable_collisions link1="base_link" link2="link_2" reason="Never" />
853  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
854  </robot>)";
855  EXPECT_TRUE(is_failure(xml_string));
856 
857  tesseract_srdf::SRDFModel srdf_model;
858  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
859  }
860  { // missing link2
861  std::string xml_string =
862  R"(<robot name="abb_irb2400">
863  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
864  <disable_collisions link1="base_link" reason="Never" />
865  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
866  </robot>)";
867  EXPECT_TRUE(is_failure(xml_string));
868 
869  tesseract_srdf::SRDFModel srdf_model;
870  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
871  }
872  { // missing reason but should not fail
873  std::string xml_string =
874  R"(<robot name="abb_irb2400">
875  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
876  <disable_collisions link1="base_link" link2="link_2" reason="Never" />
877  <disable_collisions link1="base_link" link2="link_3" />
878  </robot>)";
879  EXPECT_FALSE(is_failure(xml_string));
880 
881  tesseract_srdf::SRDFModel srdf_model;
882  EXPECT_NO_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
883  }
884  { // invalid link1 but should not fail
885  std::string xml_string =
886  R"(<robot name="abb_irb2400">
887  <disable_collisions link1="missing_link" link2="link_1" reason="Adjacent" />
888  <disable_collisions link1="base_link" link2="link_2" reason="Never" />
889  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
890  </robot>)";
891  EXPECT_FALSE(is_failure(xml_string));
892 
893  tesseract_srdf::SRDFModel srdf_model;
894  EXPECT_NO_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
895  }
896  { // invalid link2 but should not fail
897  std::string xml_string =
898  R"(<robot name="abb_irb2400">
899  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
900  <disable_collisions link1="base_link" link2="missing_link" reason="Never" />
901  <disable_collisions link1="base_link" link2="link_3" reason="Never" />
902  </robot>)";
903  EXPECT_FALSE(is_failure(xml_string));
904 
905  tesseract_srdf::SRDFModel srdf_model;
906  EXPECT_NO_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
907  }
908  { // The reason is numeric but still a valid string so should not fail
909  std::string xml_string =
910  R"(<robot name="abb_irb2400">
911  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
912  <disable_collisions link1="base_link" link2="missing_link" reason="Never" />
913  <disable_collisions link1="base_link" link2="link_3" reason="2.335" />
914  </robot>)";
915  EXPECT_FALSE(is_failure(xml_string));
916 
917  tesseract_srdf::SRDFModel srdf_model;
918  EXPECT_NO_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
919  }
920 }
921 
922 TEST(TesseractSRDFUnit, SRDFChainGroupUnit) // NOLINT
923 {
924  using namespace tesseract_scene_graph;
925  using namespace tesseract_srdf;
926 
927  SceneGraph::Ptr g = getABBSceneGraph();
928  TempResourceLocator locator;
929 
930  std::string str = R"(<robot name="abb_irb2400">
931  <group name="manipulator">
932  <chain base_link="base_link" tip_link="tool0" />
933  </group>
934  </robot>)";
935 
936  tinyxml2::XMLDocument xml_doc;
937  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
938 
939  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
940  EXPECT_TRUE(element != nullptr);
941 
942  auto [group_names, chain_groups, joint_groups, link_groups] =
943  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
944 
945  EXPECT_EQ(group_names.size(), 1);
946  EXPECT_EQ(*group_names.begin(), "manipulator");
947  EXPECT_EQ(chain_groups.size(), 1);
948  EXPECT_TRUE(joint_groups.empty());
949  EXPECT_TRUE(link_groups.empty());
950  EXPECT_EQ(chain_groups["manipulator"].size(), 1);
951  EXPECT_EQ(chain_groups["manipulator"][0].first, "base_link");
952  EXPECT_EQ(chain_groups["manipulator"][0].second, "tool0");
953 
954  // Now test failures
955  auto is_failure = [g](const std::string& xml_string) {
956  tinyxml2::XMLDocument xml_doc;
957  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
958 
959  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
960  EXPECT_TRUE(element != nullptr);
961 
962  try
963  {
964  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
965  }
966  catch (const std::exception& e)
967  {
968  tesseract_common::printNestedException(e);
969  return true;
970  }
971  return false;
972  };
973 
974  { // missing name
975  std::string str = R"(<robot name="abb_irb2400">
976  <group>
977  <chain base_link="base_link" tip_link="tool0" />
978  </group>
979  </robot>)";
980  EXPECT_TRUE(is_failure(str));
981 
982  tesseract_srdf::SRDFModel srdf_model;
983  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
984  }
985  { // missing chains
986  std::string str = R"(<robot name="abb_irb2400">
987  <group name="manipulator"/>
988  </robot>)";
989  EXPECT_TRUE(is_failure(str));
990 
991  tesseract_srdf::SRDFModel srdf_model;
992  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
993  }
994  { // missing chain base_link
995  std::string str = R"(<robot name="abb_irb2400">
996  <group name="manipulator">
997  <chain tip_link="tool0" />
998  </group>
999  </robot>)";
1000  EXPECT_TRUE(is_failure(str));
1001 
1002  tesseract_srdf::SRDFModel srdf_model;
1003  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1004  }
1005  { // missing chain tip_link
1006  std::string str = R"(<robot name="abb_irb2400">
1007  <group name="manipulator">
1008  <chain base_link="base_link" />
1009  </group>
1010  </robot>)";
1011  EXPECT_TRUE(is_failure(str));
1012 
1013  tesseract_srdf::SRDFModel srdf_model;
1014  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1015  }
1016  { // invalid chain base_link
1017  std::string str = R"(<robot name="abb_irb2400">
1018  <group name="manipulator">
1019  <chain base_link="missing_link" tip_link="tool0" />
1020  </group>
1021  </robot>)";
1022  EXPECT_TRUE(is_failure(str));
1023 
1024  tesseract_srdf::SRDFModel srdf_model;
1025  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1026  }
1027  { // invalid chain tip_link
1028  std::string str = R"(<robot name="abb_irb2400">
1029  <group name="manipulator">
1030  <chain base_link="base_link" tip_link="missing_link" />
1031  </group>
1032  </robot>)";
1033  EXPECT_TRUE(is_failure(str));
1034 
1035  tesseract_srdf::SRDFModel srdf_model;
1036  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1037  }
1038 }
1039 
1040 TEST(TesseractSRDFUnit, SRDFJointGroupUnit) // NOLINT
1041 {
1042  using namespace tesseract_scene_graph;
1043  using namespace tesseract_srdf;
1044 
1045  SceneGraph::Ptr g = getABBSceneGraph();
1046  TempResourceLocator locator;
1047 
1048  std::string str = R"(<robot name="abb_irb2400">
1049  <group name="manipulator">
1050  <joint name="joint_1"/>
1051  <joint name="joint_2"/>
1052  <joint name="joint_3"/>
1053  <joint name="joint_4"/>
1054  <joint name="joint_5"/>
1055  <joint name="joint_6"/>
1056  <joint name="joint_tool0"/>
1057  </group>
1058  </robot>)";
1059 
1060  tinyxml2::XMLDocument xml_doc;
1061  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1062 
1063  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1064  EXPECT_TRUE(element != nullptr);
1065 
1066  auto [group_names, chain_groups, joint_groups, link_groups] =
1067  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1068 
1069  EXPECT_EQ(group_names.size(), 1);
1070  EXPECT_EQ(*group_names.begin(), "manipulator");
1071  EXPECT_TRUE(chain_groups.empty());
1072  EXPECT_EQ(joint_groups.size(), 1);
1073  EXPECT_TRUE(link_groups.empty());
1074  EXPECT_EQ(joint_groups["manipulator"].size(), 7);
1075 
1076  // Now test failures
1077  auto is_failure = [g](const std::string& xml_string) {
1078  tinyxml2::XMLDocument xml_doc;
1079  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1080 
1081  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1082  EXPECT_TRUE(element != nullptr);
1083 
1084  try
1085  {
1086  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1087  }
1088  catch (const std::exception& e)
1089  {
1090  tesseract_common::printNestedException(e);
1091  return true;
1092  }
1093  return false;
1094  };
1095 
1096  { // missing name
1097  std::string str = R"(<robot name="abb_irb2400">
1098  <group>
1099  <joint name="joint_1"/>
1100  </group>
1101  </robot>)";
1102 
1103  EXPECT_TRUE(is_failure(str));
1104 
1105  tesseract_srdf::SRDFModel srdf_model;
1106  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1107  }
1108  { // missing joints
1109  std::string str = R"(<robot name="abb_irb2400">
1110  <group name="manipulator"/>
1111  </robot>)";
1112 
1113  EXPECT_TRUE(is_failure(str));
1114 
1115  tesseract_srdf::SRDFModel srdf_model;
1116  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1117  }
1118  { // missing joint name
1119  std::string str = R"(<robot name="abb_irb2400">
1120  <group name="manipulator">
1121  <joint/>
1122  </group>
1123  </robot>)";
1124 
1125  EXPECT_TRUE(is_failure(str));
1126 
1127  tesseract_srdf::SRDFModel srdf_model;
1128  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1129  }
1130 
1131  { // joint does not exist
1132  std::string str = R"(<robot name="abb_irb2400">
1133  <group name="manipulator">
1134  <joint name="missing_joint"/>
1135  </group>
1136  </robot>)";
1137 
1138  EXPECT_TRUE(is_failure(str));
1139 
1140  tesseract_srdf::SRDFModel srdf_model;
1141  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1142  }
1143 }
1144 
1145 TEST(TesseractSRDFUnit, SRDFLinkGroupUnit) // NOLINT
1146 {
1147  using namespace tesseract_scene_graph;
1148  using namespace tesseract_srdf;
1149 
1150  SceneGraph::Ptr g = getABBSceneGraph();
1151  TempResourceLocator locator;
1152 
1153  std::string str = R"(<robot name="abb_irb2400">
1154  <group name="manipulator">
1155  <link name="base_link"/>
1156  <link name="link_1"/>
1157  <link name="link_2"/>
1158  <link name="link_3"/>
1159  <link name="link_4"/>
1160  <link name="link_5"/>
1161  <link name="link_6"/>
1162  <link name="tool0"/>
1163  </group>
1164  </robot>)";
1165 
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("robot");
1170  EXPECT_TRUE(element != nullptr);
1171 
1172  auto [group_names, chain_groups, joint_groups, link_groups] =
1173  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1174 
1175  EXPECT_EQ(group_names.size(), 1);
1176  EXPECT_EQ(*group_names.begin(), "manipulator");
1177  EXPECT_TRUE(chain_groups.empty());
1178  EXPECT_TRUE(joint_groups.empty());
1179  EXPECT_EQ(link_groups.size(), 1);
1180  EXPECT_EQ(link_groups["manipulator"].size(), 8);
1181 
1182  // Now test failures
1183  auto is_failure = [g](const std::string& xml_string) {
1184  tinyxml2::XMLDocument xml_doc;
1185  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1186 
1187  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1188  EXPECT_TRUE(element != nullptr);
1189 
1190  try
1191  {
1192  parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1193  }
1194  catch (const std::exception& e)
1195  {
1196  tesseract_common::printNestedException(e);
1197  return true;
1198  }
1199  return false;
1200  };
1201 
1202  { // missing name
1203  std::string str = R"(<robot name="abb_irb2400">
1204  <group>
1205  <link name="joint_1"/>
1206  </group>
1207  </robot>)";
1208 
1209  EXPECT_TRUE(is_failure(str));
1210 
1211  tesseract_srdf::SRDFModel srdf_model;
1212  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1213  }
1214  { // missing joints
1215  std::string str = R"(<robot name="abb_irb2400">
1216  <group name="manipulator"/>
1217  </robot>)";
1218 
1219  EXPECT_TRUE(is_failure(str));
1220  }
1221  { // missing joint name
1222  std::string str = R"(<robot name="abb_irb2400">
1223  <group name="manipulator">
1224  <link/>
1225  </group>
1226  </robot>)";
1227 
1228  EXPECT_TRUE(is_failure(str));
1229 
1230  tesseract_srdf::SRDFModel srdf_model;
1231  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1232  }
1233  { // link does not exist
1234  std::string str = R"(<robot name="abb_irb2400">
1235  <group name="manipulator">
1236  <link name="missing_link"/>
1237  </group>
1238  </robot>)";
1239 
1240  EXPECT_TRUE(is_failure(str));
1241 
1242  tesseract_srdf::SRDFModel srdf_model;
1243  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1244  }
1245 }
1246 
1247 TEST(TesseractSRDFUnit, LoadSRDFGroupStatesUnit) // NOLINT
1248 {
1249  using namespace tesseract_scene_graph;
1250  using namespace tesseract_srdf;
1251 
1252  SceneGraph::Ptr g = getABBSceneGraph();
1253  TempResourceLocator locator;
1254 
1255  std::string xml_string =
1256  R"(<robot name="abb_irb2400">
1257  <group_state name="all-zeros" group="manipulator">
1258  <joint name="joint_1" value="0"/>
1259  <joint name="joint_2" value="0"/>
1260  <joint name="joint_3" value="0"/>
1261  <joint name="joint_4" value="0"/>
1262  <joint name="joint_5" value="0"/>
1263  <joint name="joint_6" value="0"/>
1264  </group_state>
1265  </robot>)";
1266  tinyxml2::XMLDocument xml_doc;
1267  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1268 
1269  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1270  EXPECT_TRUE(element != nullptr);
1271 
1272  GroupNames group_names = { "manipulator" };
1273  GroupJointStates group_states = parseGroupStates(*g, group_names, element, std::array<int, 3>({ 1, 0, 0 }));
1274  EXPECT_EQ(group_states.size(), 1);
1275 
1276  auto it = group_states.find("manipulator");
1277  EXPECT_TRUE(it != group_states.end());
1278 
1279  auto it2 = it->second.find("all-zeros");
1280  EXPECT_TRUE(it2 != it->second.end());
1281  EXPECT_EQ(it2->second.size(), 6);
1282  EXPECT_DOUBLE_EQ(it2->second["joint_1"], 0);
1283  EXPECT_DOUBLE_EQ(it2->second["joint_2"], 0);
1284  EXPECT_DOUBLE_EQ(it2->second["joint_3"], 0);
1285  EXPECT_DOUBLE_EQ(it2->second["joint_4"], 0);
1286  EXPECT_DOUBLE_EQ(it2->second["joint_5"], 0);
1287  EXPECT_DOUBLE_EQ(it2->second["joint_6"], 0);
1288 
1289  // Now test failures
1290  auto is_failure = [g, &group_names](const std::string& xml_string) {
1291  tinyxml2::XMLDocument xml_doc;
1292  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1293 
1294  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1295  EXPECT_TRUE(element != nullptr);
1296 
1297  try
1298  {
1299  parseGroupStates(*g, group_names, element, std::array<int, 3>({ 1, 0, 0 }));
1300  }
1301  catch (const std::exception& e)
1302  {
1303  tesseract_common::printNestedException(e);
1304  return true;
1305  }
1306  return false;
1307  };
1308 
1309  { // missing name
1310  std::string xml_string =
1311  R"(<robot name="abb_irb2400">
1312  <group_state group="manipulator">
1313  <joint name="joint_1" value="0"/>
1314  </group_state>
1315  </robot>)";
1316  EXPECT_TRUE(is_failure(xml_string));
1317 
1318  tesseract_srdf::SRDFModel srdf_model;
1319  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1320  }
1321  { // missing group
1322  std::string xml_string =
1323  R"(<robot name="abb_irb2400">
1324  <group_state name="all-zeros">
1325  <joint name="joint_1" value="0"/>
1326  </group_state>
1327  </robot>)";
1328  EXPECT_TRUE(is_failure(xml_string));
1329 
1330  tesseract_srdf::SRDFModel srdf_model;
1331  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1332  }
1333  { // invalid group
1334  std::string xml_string =
1335  R"(<robot name="abb_irb2400">
1336  <group_state name="all-zeros" group="missing_group">
1337  <joint name="joint_1" value="0"/>
1338  </group_state>
1339  </robot>)";
1340  EXPECT_TRUE(is_failure(xml_string));
1341 
1342  tesseract_srdf::SRDFModel srdf_model;
1343  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1344  }
1345  { // no joints
1346  std::string xml_string =
1347  R"(<robot name="abb_irb2400">
1348  <group_state name="all-zeros" group="manipulator"/>
1349  </robot>)";
1350  EXPECT_TRUE(is_failure(xml_string));
1351 
1352  tesseract_srdf::SRDFModel srdf_model;
1353  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1354  }
1355  { // missing joint name
1356  std::string xml_string =
1357  R"(<robot name="abb_irb2400">
1358  <group_state name="all-zeros" group="manipulator">
1359  <joint value="0"/>
1360  </group_state>
1361  </robot>)";
1362  EXPECT_TRUE(is_failure(xml_string));
1363 
1364  tesseract_srdf::SRDFModel srdf_model;
1365  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1366  }
1367  { // missing joint value
1368  std::string xml_string =
1369  R"(<robot name="abb_irb2400">
1370  <group_state name="all-zeros" group="manipulator">
1371  <joint name="joint_1"/>
1372  </group_state>
1373  </robot>)";
1374  EXPECT_TRUE(is_failure(xml_string));
1375 
1376  tesseract_srdf::SRDFModel srdf_model;
1377  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1378  }
1379  { // invalid joint value
1380  std::string xml_string =
1381  R"(<robot name="abb_irb2400">
1382  <group_state name="all-zeros" group="manipulator">
1383  <joint name="joint_1" value="abc"/>
1384  </group_state>
1385  </robot>)";
1386  EXPECT_TRUE(is_failure(xml_string));
1387 
1388  tesseract_srdf::SRDFModel srdf_model;
1389  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1390  }
1391  { // invalid joint name
1392  std::string xml_string =
1393  R"(<robot name="abb_irb2400">
1394  <group_state name="all-zeros" group="manipulator">
1395  <joint name="missing_joint" value="0"/>
1396  </group_state>
1397  </robot>)";
1398  EXPECT_TRUE(is_failure(xml_string));
1399 
1400  tesseract_srdf::SRDFModel srdf_model;
1401  EXPECT_ANY_THROW(srdf_model.initString(*g, xml_string, locator)); // NOLINT
1402  }
1403 }
1404 
1405 TEST(TesseractSRDFUnit, SRDFGroupTCPsUnit) // NOLINT
1406 {
1407  using namespace tesseract_scene_graph;
1408  using namespace tesseract_srdf;
1409 
1410  SceneGraph::Ptr g = getABBSceneGraph();
1411  TempResourceLocator locator;
1412 
1413  std::string str = R"(<robot name="abb_irb2400">
1414  <group_tcps group="manipulator">
1415  <tcp name="laser" xyz="1 .1 1" rpy="0 1.57 0" />
1416  <tcp name="welder" xyz=".1 1 .2" wxyz="1 0 0 0" />
1417  </group_tcps>
1418  </robot>)";
1419 
1420  tinyxml2::XMLDocument xml_doc;
1421  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1422 
1423  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1424  EXPECT_TRUE(element != nullptr);
1425 
1426  GroupTCPs group_tcps = parseGroupTCPs(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1427 
1428  EXPECT_EQ(group_tcps.size(), 1);
1429 
1430  auto it = group_tcps.find("manipulator");
1431  EXPECT_TRUE(it != group_tcps.end());
1432  EXPECT_EQ(it->second.size(), 2);
1433 
1434  auto it2 = it->second.find("laser");
1435  EXPECT_TRUE(it2 != it->second.end());
1436 
1437  auto it3 = it->second.find("welder");
1438  EXPECT_TRUE(it3 != it->second.end());
1439 
1440  // Now test failures
1441  auto is_failure = [g](const std::string& xml_string) {
1442  tinyxml2::XMLDocument xml_doc;
1443  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1444 
1445  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1446  EXPECT_TRUE(element != nullptr);
1447 
1448  try
1449  {
1450  parseGroupTCPs(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1451  }
1452  catch (const std::exception& e)
1453  {
1454  tesseract_common::printNestedException(e);
1455  return true;
1456  }
1457  return false;
1458  };
1459 
1460  { // missing group
1461  std::string str = R"(<robot name="abb_irb2400">
1462  <group_tcps>
1463  <tcp name="laser" xyz="1 .1 1" rpy="0 1.57 0" />
1464  </group_tcps>
1465  </robot>)";
1466  EXPECT_TRUE(is_failure(str));
1467 
1468  tesseract_srdf::SRDFModel srdf_model;
1469  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1470  }
1471  { // missing tcp element
1472  std::string str = R"(<robot name="abb_irb2400">
1473  <group_tcps group="manipulator"/>
1474  </robot>)";
1475  EXPECT_TRUE(is_failure(str));
1476 
1477  tesseract_srdf::SRDFModel srdf_model;
1478  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1479  }
1480  { // missing tcp name
1481  std::string str = R"(<robot name="abb_irb2400">
1482  <group_tcps group="manipulator">
1483  <tcp xyz="1 .1 1" rpy="0 1.57 0" />
1484  </group_tcps>
1485  </robot>)";
1486  EXPECT_TRUE(is_failure(str));
1487 
1488  tesseract_srdf::SRDFModel srdf_model;
1489  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1490  }
1491  { // missing tcp xyz
1492  std::string str = R"(<robot name="abb_irb2400">
1493  <group_tcps group="manipulator">
1494  <tcp name="laser" rpy="0 1.57 0" />
1495  </group_tcps>
1496  </robot>)";
1497  EXPECT_TRUE(is_failure(str));
1498 
1499  tesseract_srdf::SRDFModel srdf_model;
1500  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1501  }
1502  { // missing tcp orientation
1503  std::string str = R"(<robot name="abb_irb2400">
1504  <group_tcps group="manipulator">
1505  <tcp name="laser" xyz="1 .1 1" />
1506  </group_tcps>
1507  </robot>)";
1508  EXPECT_TRUE(is_failure(str));
1509 
1510  tesseract_srdf::SRDFModel srdf_model;
1511  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1512  }
1513  { // invalid tcp xyz
1514  std::string str = R"(<robot name="abb_irb2400">
1515  <group_tcps group="manipulator">
1516  <tcp name="laser" xyz="a .1 1" rpy="0 1.57 0" />
1517  </group_tcps>
1518  </robot>)";
1519  EXPECT_TRUE(is_failure(str));
1520 
1521  tesseract_srdf::SRDFModel srdf_model;
1522  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1523  }
1524  { // invalid orientation
1525  std::string str = R"(<robot name="abb_irb2400">
1526  <group_tcps group="manipulator">
1527  <tcp name="laser" xyz="1 .1 1" rpy="a 1.57 0" />
1528  </group_tcps>
1529  </robot>)";
1530  EXPECT_TRUE(is_failure(str));
1531 
1532  tesseract_srdf::SRDFModel srdf_model;
1533  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1534  }
1535  { // invalid orientation
1536  std::string str = R"(<robot name="abb_irb2400">
1537  <group_tcps group="manipulator">
1538  <tcp name="laser" xyz="1 .1 1" wxyz="a 1.57 0 0" />
1539  </group_tcps>
1540  </robot>)";
1541  EXPECT_TRUE(is_failure(str));
1542 
1543  tesseract_srdf::SRDFModel srdf_model;
1544  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1545  }
1546 }
1547 
1548 TEST(TesseractSRDFUnit, SRDFCollisionMarginsUnit) // NOLINT
1549 {
1550  using namespace tesseract_scene_graph;
1551  using namespace tesseract_srdf;
1552 
1553  SceneGraph::Ptr g = getABBSceneGraph();
1554  TempResourceLocator locator;
1555 
1556  { // Testing having default margin and pair margin
1557  std::string str = R"(<robot name="abb_irb2400">
1558  <collision_margins default_margin="0.025">
1559  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
1560  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
1561  </collision_margins>
1562  </robot>)";
1563 
1564  tinyxml2::XMLDocument xml_doc;
1565  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1566 
1567  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1568  EXPECT_TRUE(element != nullptr);
1569 
1570  tesseract_common::CollisionMarginData::Ptr margin_data =
1571  parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1572 
1573  EXPECT_TRUE(margin_data != nullptr);
1574  EXPECT_NEAR(margin_data->getDefaultCollisionMargin(), 0.025, 1e-6);
1575  EXPECT_NEAR(margin_data->getMaxCollisionMargin(), 0.025, 1e-6);
1576  EXPECT_EQ(margin_data->getCollisionMarginPairData().getCollisionMargins().size(), 2);
1577  EXPECT_NEAR(margin_data->getCollisionMargin("link_5", "link_6"), 0.01, 1e-6);
1578  EXPECT_NEAR(margin_data->getCollisionMargin("link_5", "link_4"), 0.015, 1e-6);
1579  }
1580 
1581  { // Test only having default margin
1582  std::string str = R"(<robot name="abb_irb2400">
1583  <collision_margins default_margin="0.025"/>
1584  </robot>)";
1585 
1586  tinyxml2::XMLDocument xml_doc;
1587  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1588 
1589  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1590  EXPECT_TRUE(element != nullptr);
1591 
1592  tesseract_common::CollisionMarginData::Ptr margin_data =
1593  parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1594 
1595  EXPECT_TRUE(margin_data != nullptr);
1596  EXPECT_NEAR(margin_data->getDefaultCollisionMargin(), 0.025, 1e-6);
1597  EXPECT_NEAR(margin_data->getMaxCollisionMargin(), 0.025, 1e-6);
1598  EXPECT_EQ(margin_data->getCollisionMarginPairData().getCollisionMargins().size(), 0);
1599  }
1600 
1601  { // Testing having negative default margin and pair margin
1602  std::string str = R"(<robot name="abb_irb2400">
1603  <collision_margins default_margin="-0.025">
1604  <pair_margin link1="link_6" link2="link_5" margin="-0.01"/>
1605  <pair_margin link1="link_5" link2="link_4" margin="-0.015"/>
1606  </collision_margins>
1607  </robot>)";
1608 
1609  tinyxml2::XMLDocument xml_doc;
1610  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1611 
1612  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1613  EXPECT_TRUE(element != nullptr);
1614 
1615  tesseract_common::CollisionMarginData::Ptr margin_data =
1616  parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1617 
1618  EXPECT_TRUE(margin_data != nullptr);
1619  EXPECT_NEAR(margin_data->getDefaultCollisionMargin(), -0.025, 1e-6);
1620  EXPECT_NEAR(margin_data->getMaxCollisionMargin(), -0.01, 1e-6);
1621  EXPECT_EQ(margin_data->getCollisionMarginPairData().getCollisionMargins().size(), 2);
1622  EXPECT_NEAR(margin_data->getCollisionMargin("link_5", "link_6"), -0.01, 1e-6);
1623  EXPECT_NEAR(margin_data->getCollisionMargin("link_5", "link_4"), -0.015, 1e-6);
1624  }
1625 
1626  { // Test not having collision margin data
1627  std::string str = R"(<robot name="abb_irb2400">
1628  </robot>)";
1629 
1630  tinyxml2::XMLDocument xml_doc;
1631  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1632 
1633  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1634  EXPECT_TRUE(element != nullptr);
1635 
1636  tesseract_common::CollisionMarginData::Ptr margin_data =
1637  parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1638 
1639  EXPECT_TRUE(margin_data == nullptr);
1640  }
1641 
1642  // Now test failures
1643  auto is_failure = [g](const std::string& xml_string) {
1644  tinyxml2::XMLDocument xml_doc;
1645  EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
1646 
1647  tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
1648  EXPECT_TRUE(element != nullptr);
1649 
1650  try
1651  {
1652  parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }));
1653  }
1654  catch (const std::exception& e)
1655  {
1656  tesseract_common::printNestedException(e);
1657  return true;
1658  }
1659  return false;
1660  };
1661 
1662  { // missing default_margin
1663  std::string str = R"(<robot name="abb_irb2400">
1664  <collision_margins>
1665  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
1666  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
1667  </collision_margins>
1668  </robot>)";
1669  EXPECT_TRUE(is_failure(str));
1670 
1671  tesseract_srdf::SRDFModel srdf_model;
1672  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1673  }
1674 
1675  { // missing pair link1
1676  std::string str = R"(<robot name="abb_irb2400">
1677  <collision_margins default_margin="0.025">
1678  <pair_margin link2="link_5" margin="0.01"/>
1679  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
1680  </collision_margins>
1681  </robot>)";
1682  EXPECT_TRUE(is_failure(str));
1683 
1684  tesseract_srdf::SRDFModel srdf_model;
1685  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1686  }
1687 
1688  { // missing pair link2
1689  std::string str = R"(<robot name="abb_irb2400">
1690  <collision_margins default_margin="0.025">
1691  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
1692  <pair_margin link1="link_5" margin="0.015"/>
1693  </collision_margins>
1694  </robot>)";
1695  EXPECT_TRUE(is_failure(str));
1696 
1697  tesseract_srdf::SRDFModel srdf_model;
1698  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1699  }
1700 
1701  { // missing pair margin
1702  std::string str = R"(<robot name="abb_irb2400">
1703  <collision_margins default_margin="0.025">
1704  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
1705  <pair_margin link1="link_5" link2="link_4"/>
1706  </collision_margins>
1707  </robot>)";
1708  EXPECT_TRUE(is_failure(str));
1709 
1710  tesseract_srdf::SRDFModel srdf_model;
1711  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1712  }
1713 
1714  { // empty default margin
1715  std::string str = R"(<robot name="abb_irb2400">
1716  <collision_margins default_margin="">
1717  <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
1718  <pair_margin link1="link_5" link2="link_4" margin="0.01"/>
1719  </collision_margins>
1720  </robot>)";
1721  EXPECT_TRUE(is_failure(str));
1722 
1723  tesseract_srdf::SRDFModel srdf_model;
1724  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1725  }
1726 
1727  { // empty pair margin
1728  std::string str = R"(<robot name="abb_irb2400">
1729  <collision_margins default_margin="0.025">
1730  <pair_margin link1="link_6" link2="link_5" margin=""/>
1731  <pair_margin link1="link_5" link2="link_4" margin="0.01"/>
1732  </collision_margins>
1733  </robot>)";
1734  EXPECT_TRUE(is_failure(str));
1735 
1736  tesseract_srdf::SRDFModel srdf_model;
1737  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1738  }
1739 
1740  { // invalid link name 1, but should not fail
1741  std::string str = R"(<robot name="abb_irb2400">
1742  <collision_margins default_margin="-0.025">
1743  <pair_margin link1="missing_link" link2="link_5" margin="0.01"/>
1744  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
1745  </collision_margins>
1746  </robot>)";
1747  EXPECT_FALSE(is_failure(str));
1748 
1749  tesseract_srdf::SRDFModel srdf_model;
1750  EXPECT_NO_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1751  }
1752 
1753  { // invalid link name 2, but should not fail
1754  std::string str = R"(<robot name="abb_irb2400">
1755  <collision_margins default_margin="-0.025">
1756  <pair_margin link1="link_6" link2="missing_link" margin="0.01"/>
1757  <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
1758  </collision_margins>
1759  </robot>)";
1760  EXPECT_FALSE(is_failure(str));
1761 
1762  tesseract_srdf::SRDFModel srdf_model;
1763  EXPECT_NO_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
1764  }
1765 }
1766 
1767 TEST(TesseractSRDFUnit, AddRemoveChainGroupUnit) // NOLINT
1768 {
1769  using namespace tesseract_srdf;
1770  KinematicsInformation info;
1771 
1772  // ADD
1773  ChainGroup chain_group;
1774  chain_group.emplace_back("base_link", "tool0");
1775  info.addChainGroup("manipulator", chain_group);
1776  EXPECT_TRUE(info.hasChainGroup("manipulator"));
1777  EXPECT_TRUE(info.chain_groups.at("manipulator") == chain_group);
1778  EXPECT_EQ(info.chain_groups.size(), 1);
1779  EXPECT_EQ(info.group_names.size(), 1);
1780  EXPECT_TRUE(info.hasGroup("manipulator"));
1781 
1782  // Copy Equal
1783  KinematicsInformation info1_copy = info;
1784  EXPECT_EQ(info1_copy, info);
1785 
1786  // Not equal
1787  chain_group = ChainGroup();
1788  chain_group.emplace_back("tool0", "base_link");
1789  info1_copy.addChainGroup("manipulator", chain_group);
1790  EXPECT_NE(info1_copy, info);
1791 
1792  // Insert
1793  info1_copy.insert(info);
1794  EXPECT_EQ(info1_copy, info);
1795 
1796  // Remove
1797  info.removeChainGroup("manipulator");
1798  EXPECT_FALSE(info.hasChainGroup("manipulator"));
1799  EXPECT_FALSE(info.hasGroup("manipulator"));
1800  EXPECT_EQ(info.chain_groups.size(), 0);
1801  EXPECT_EQ(info.group_names.size(), 0);
1802 }
1803 
1804 TEST(TesseractSRDFUnit, AddRemoveJointGroupUnit) // NOLINT
1805 {
1806  using namespace tesseract_srdf;
1807  KinematicsInformation info;
1808 
1809  // ADD
1810  JointGroup joint_group = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
1811  info.addJointGroup("manipulator", joint_group);
1812  EXPECT_TRUE(info.hasJointGroup("manipulator"));
1813  EXPECT_TRUE(info.hasGroup("manipulator"));
1814  EXPECT_TRUE(info.joint_groups.at("manipulator") == joint_group);
1815  EXPECT_EQ(info.joint_groups.size(), 1);
1816  EXPECT_EQ(info.group_names.size(), 1);
1817 
1818  // Copy Equal
1819  KinematicsInformation info1_copy = info;
1820  EXPECT_EQ(info1_copy, info);
1821 
1822  // Different order equal
1823  joint_group = { "joint_6", "joint_5", "joint_4", "joint_3", "joint_2", "joint_1" };
1824  info1_copy.addJointGroup("manipulator", joint_group);
1825  EXPECT_EQ(info1_copy, info);
1826 
1827  // Not Equal
1828  joint_group = { "joint_6", "joint_5", "joint_4", "joint_3", "joint_2", "joint_0" };
1829  info1_copy.addJointGroup("manipulator", joint_group);
1830  EXPECT_NE(info1_copy, info);
1831 
1832  // Insert
1833  info1_copy.insert(info);
1834  EXPECT_EQ(info1_copy, info);
1835 
1836  // Remove
1837  info.removeJointGroup("manipulator");
1838  EXPECT_FALSE(info.hasJointGroup("manipulator"));
1839  EXPECT_FALSE(info.hasGroup("manipulator"));
1840  EXPECT_EQ(info.joint_groups.size(), 0);
1841  EXPECT_EQ(info.group_names.size(), 0);
1842 }
1843 
1844 TEST(TesseractSRDFUnit, AddRemoveLinkGroupUnit) // NOLINT
1845 {
1846  using namespace tesseract_srdf;
1847  KinematicsInformation info;
1848 
1849  // ADD
1850  LinkGroup link_group = { "link_1", "link_2", "link_3", "link_4", "link_5", "link_6" };
1851  info.addLinkGroup("manipulator", link_group);
1852  EXPECT_TRUE(info.hasLinkGroup("manipulator"));
1853  EXPECT_TRUE(info.hasGroup("manipulator"));
1854  EXPECT_EQ(info.link_groups.size(), 1);
1855  EXPECT_EQ(info.group_names.size(), 1);
1856 
1857  // Copy Equal
1858  KinematicsInformation info1_copy = info;
1859  EXPECT_EQ(info1_copy, info);
1860 
1861  // Different order equal
1862  link_group = { "link_6", "link_5", "link_4", "link_3", "link_2", "link_1" };
1863  info1_copy.addLinkGroup("manipulator", link_group);
1864  EXPECT_EQ(info1_copy, info);
1865 
1866  // Not Equal
1867  link_group = { "link_6", "link_5", "link_4", "link_3", "link_2", "link_0" };
1868  info1_copy.addLinkGroup("manipulator", link_group);
1869  EXPECT_NE(info1_copy, info);
1870 
1871  // Insert
1872  info1_copy.insert(info);
1873  EXPECT_EQ(info1_copy, info);
1874 
1875  // Remove
1876  info.removeLinkGroup("manipulator");
1877  EXPECT_FALSE(info.hasLinkGroup("manipulator"));
1878  EXPECT_FALSE(info.hasGroup("manipulator"));
1879  EXPECT_EQ(info.link_groups.size(), 0);
1880  EXPECT_EQ(info.group_names.size(), 0);
1881 }
1882 
1883 TEST(TesseractSRDFUnit, AddRemoveGroupJointStateUnit) // NOLINT
1884 {
1885  using namespace tesseract_srdf;
1886  KinematicsInformation info;
1887 
1888  // ADD
1889  GroupsJointState group_states;
1890  group_states["joint_1"] = 0;
1891  group_states["joint_2"] = 0;
1892  group_states["joint_3"] = 0;
1893  group_states["joint_4"] = 0;
1894  group_states["joint_5"] = 0;
1895  group_states["joint_6"] = 0;
1896 
1897  info.addGroupJointState("manipulator", "all-zeros", group_states);
1898  EXPECT_TRUE(info.hasGroupJointState("manipulator", "all-zeros"));
1899  EXPECT_TRUE(info.group_states.at("manipulator").at("all-zeros") == group_states);
1900  EXPECT_EQ(info.group_states.at("manipulator").size(), 1);
1901  EXPECT_EQ(info.group_states.size(), 1);
1902 
1903  // Copy Equal
1904  KinematicsInformation info1_copy = info;
1905  EXPECT_EQ(info1_copy, info);
1906 
1907  // Not Equal
1908  group_states["joint_1"] = 1;
1909  group_states["joint_2"] = 2;
1910  group_states["joint_3"] = 3;
1911  group_states["joint_4"] = 4;
1912  group_states["joint_5"] = 5;
1913  group_states["joint_6"] = 6;
1914 
1915  info1_copy.addGroupJointState("manipulator", "all-zeros", group_states);
1916  EXPECT_NE(info1_copy, info);
1917 
1918  // Insert
1919  info1_copy.insert(info);
1920  EXPECT_EQ(info1_copy, info);
1921 
1922  // Remove
1923  info.removeGroupJointState("manipulator", "all-zeros");
1924  EXPECT_FALSE(info.hasGroupJointState("manipulator", "all-zeros"));
1925  EXPECT_EQ(info.group_states.size(), 0);
1926 }
1927 
1928 TEST(TesseractSRDFUnit, AddRemoveGroupTCPUnit) // NOLINT
1929 {
1930  using namespace tesseract_srdf;
1931  KinematicsInformation info;
1932 
1933  // ADD
1934  Eigen::Isometry3d tcp_laser = Eigen::Isometry3d::Identity();
1935  tcp_laser.translation() = Eigen::Vector3d(1, 0.1, 1);
1936 
1937  Eigen::Isometry3d tcp_welder = Eigen::Isometry3d::Identity();
1938  tcp_welder.translation() = Eigen::Vector3d(0.1, 1, 0.2);
1939 
1940  info.addGroupTCP("manipulator", "laser", tcp_laser);
1941  info.addGroupTCP("manipulator", "welder", tcp_welder);
1942  EXPECT_TRUE(info.hasGroupTCP("manipulator", "laser"));
1943  EXPECT_TRUE(info.hasGroupTCP("manipulator", "welder"));
1944  EXPECT_TRUE(info.group_tcps.at("manipulator").at("laser").isApprox(tcp_laser, 1e-6));
1945  EXPECT_TRUE(info.group_tcps.at("manipulator").at("welder").isApprox(tcp_welder, 1e-6));
1946  EXPECT_EQ(info.group_tcps.at("manipulator").size(), 2);
1947  EXPECT_EQ(info.group_tcps.size(), 1);
1948 
1949  // Copy Equal
1950  KinematicsInformation info1_copy = info;
1951  EXPECT_EQ(info1_copy, info);
1952 
1953  // Not Equal
1954  tcp_laser = Eigen::Isometry3d::Identity();
1955  tcp_laser.translation() = Eigen::Vector3d(0.1, 1, 0.2);
1956 
1957  tcp_welder = Eigen::Isometry3d::Identity();
1958  tcp_welder.translation() = Eigen::Vector3d(1, 0.1, 1);
1959 
1960  info1_copy.addGroupTCP("manipulator", "laser", tcp_laser);
1961  info1_copy.addGroupTCP("manipulator", "welder", tcp_welder);
1962 
1963  EXPECT_NE(info1_copy, info);
1964 
1965  // Insert
1966  info1_copy.insert(info);
1967  EXPECT_EQ(info1_copy, info);
1968 
1969  // Remove
1970  info.removeGroupTCP("manipulator", "laser");
1971  info.removeGroupTCP("manipulator", "welder");
1972  EXPECT_FALSE(info.hasGroupTCP("manipulator", "laser"));
1973  EXPECT_FALSE(info.hasGroupTCP("manipulator", "welder"));
1974  EXPECT_EQ(info.group_tcps.size(), 0);
1975 }
1976 
1977 TEST(TesseractSRDFUnit, ParseConfigFilePathUnit) // NOLINT
1978 {
1979  std::array<int, 3> version{ 1, 0, 0 };
1980  tesseract_common::GeneralResourceLocator locator;
1981  tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph();
1982 
1983  { // valid
1984  std::string str = R"(<robot name="abb_irb2400">
1985  <contact_managers_plugin_config filename="package://tesseract_support/urdf/contact_manager_plugins.yaml"/>
1986  </robot>)";
1987 
1988  tinyxml2::XMLDocument xml_doc;
1989  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
1990 
1991  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
1992  EXPECT_TRUE(robot_element != nullptr);
1993 
1994  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
1995  EXPECT_TRUE(element != nullptr);
1996 
1997  std::filesystem::path path = tesseract_srdf::parseConfigFilePath(locator, element, version);
1998  EXPECT_TRUE(std::filesystem::exists(path));
1999  }
2000 
2001  { // failures (incorrect attribute)
2002  std::string str = R"(<robot name="abb_irb2400">
2003  <contact_managers_plugin_config incorrect_attribute="package://tesseract_support/urdf/contact_manager_plugins.yaml"/>
2004  </robot>)";
2005 
2006  tinyxml2::XMLDocument xml_doc;
2007  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2008 
2009  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2010  EXPECT_TRUE(robot_element != nullptr);
2011 
2012  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2013  EXPECT_TRUE(element != nullptr);
2014 
2015  EXPECT_ANY_THROW(tesseract_srdf::parseConfigFilePath(locator, element, version)); // NOLINT
2016 
2017  tesseract_srdf::SRDFModel srdf_model;
2018  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2019  }
2020 
2021  { // failures (resource does not exist)
2022  std::string str = R"(<robot name="abb_irb2400">
2023  <contact_managers_plugin_config filename="package://tesseract_support/urdf/does_not_exist.yaml"/>
2024  </robot>)";
2025 
2026  tinyxml2::XMLDocument xml_doc;
2027  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2028 
2029  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2030  EXPECT_TRUE(robot_element != nullptr);
2031 
2032  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2033  EXPECT_TRUE(element != nullptr);
2034 
2035  EXPECT_ANY_THROW(tesseract_srdf::parseConfigFilePath(locator, element, version)); // NOLINT
2036 
2037  tesseract_srdf::SRDFModel srdf_model;
2038  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2039  }
2040 
2041  { // failures (resource not found)
2042  std::string str = R"(<robot name="abb_irb2400">
2043  <contact_managers_plugin_config filename="does_not_exist.yaml"/>
2044  </robot>)";
2045 
2046  tinyxml2::XMLDocument xml_doc;
2047  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2048 
2049  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2050  EXPECT_TRUE(robot_element != nullptr);
2051 
2052  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2053  EXPECT_TRUE(element != nullptr);
2054 
2055  EXPECT_ANY_THROW(tesseract_srdf::parseConfigFilePath(locator, element, version)); // NOLINT
2056 
2057  tesseract_srdf::SRDFModel srdf_model;
2058  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2059  }
2060 }
2061 
2062 TEST(TesseractSRDFUnit, ParseContactManagersPluginConfigUnit) // NOLINT
2063 {
2064  std::array<int, 3> version{ 1, 0, 0 };
2065  tesseract_common::GeneralResourceLocator locator;
2066  tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph();
2067 
2068  { // valid
2069  std::string str = R"(<robot name="abb_irb2400">
2070  <contact_managers_plugin_config filename="package://tesseract_support/urdf/contact_manager_plugins.yaml"/>
2071  </robot>)";
2072 
2073  tinyxml2::XMLDocument xml_doc;
2074  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2075 
2076  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2077  EXPECT_TRUE(robot_element != nullptr);
2078 
2079  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2080  EXPECT_TRUE(element != nullptr);
2081 
2082  tesseract_common::ContactManagersPluginInfo info =
2083  tesseract_srdf::parseContactManagersPluginConfig(locator, element, version);
2084  EXPECT_FALSE(info.empty());
2085  }
2086 
2087  { // failure
2088  std::string str = R"(<robot name="abb_irb2400">
2089  <contact_managers_plugin_config filename="package://tesseract_support/urdf/does_not_exist.yaml"/>
2090  </robot>)";
2091 
2092  tinyxml2::XMLDocument xml_doc;
2093  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2094 
2095  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2096  EXPECT_TRUE(robot_element != nullptr);
2097 
2098  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2099  EXPECT_TRUE(element != nullptr);
2100 
2101  EXPECT_ANY_THROW(tesseract_srdf::parseContactManagersPluginConfig(locator, element, version)); // NOLINT
2102 
2103  tesseract_srdf::SRDFModel srdf_model;
2104  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2105  }
2106 
2107  { // failure
2108  std::string str = R"(<robot name="abb_irb2400">
2109  <contact_managers_plugin_config filename="package://tesseract_support/urdf/malformed_config.yaml"/>
2110  </robot>)";
2111 
2112  tinyxml2::XMLDocument xml_doc;
2113  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2114 
2115  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2116  EXPECT_TRUE(robot_element != nullptr);
2117 
2118  tinyxml2::XMLElement* element = robot_element->FirstChildElement("contact_managers_plugin_config");
2119  EXPECT_TRUE(element != nullptr);
2120 
2121  EXPECT_ANY_THROW(tesseract_srdf::parseContactManagersPluginConfig(locator, element, version)); // NOLINT
2122 
2123  tesseract_srdf::SRDFModel srdf_model;
2124  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2125  }
2126 }
2127 
2128 TEST(TesseractSRDFUnit, ParseKinematicsPluginConfigUnit) // NOLINT
2129 {
2130  std::array<int, 3> version{ 1, 0, 0 };
2131  tesseract_common::GeneralResourceLocator locator;
2132  tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph();
2133 
2134  { // valid
2135  std::string str = R"(<robot name="abb_irb2400">
2136  <kinematics_plugin_config filename="package://tesseract_support/urdf/abb_irb2400_plugins.yaml"/>
2137  </robot>)";
2138 
2139  tinyxml2::XMLDocument xml_doc;
2140  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2141 
2142  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2143  EXPECT_TRUE(robot_element != nullptr);
2144 
2145  tinyxml2::XMLElement* element = robot_element->FirstChildElement("kinematics_plugin_config");
2146  EXPECT_TRUE(element != nullptr);
2147 
2148  tesseract_common::KinematicsPluginInfo info =
2149  tesseract_srdf::parseKinematicsPluginConfig(locator, element, version);
2150  EXPECT_FALSE(info.empty());
2151  }
2152 
2153  { // failure
2154  std::string str = R"(<robot name="abb_irb2400">
2155  <kinematics_plugin_config/>
2156  </robot>)";
2157 
2158  tinyxml2::XMLDocument xml_doc;
2159  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2160 
2161  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2162  EXPECT_TRUE(robot_element != nullptr);
2163 
2164  tinyxml2::XMLElement* element = robot_element->FirstChildElement("kinematics_plugin_config");
2165  EXPECT_TRUE(element != nullptr);
2166 
2167  EXPECT_ANY_THROW(tesseract_srdf::parseKinematicsPluginConfig(locator, element, version)); // NOLINT
2168 
2169  tesseract_srdf::SRDFModel srdf_model;
2170  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2171  }
2172 
2173  { // failure
2174  std::string str = R"(<robot name="abb_irb2400">
2175  <kinematics_plugin_config filename="package://tesseract_support/urdf/does_not_exist.yaml"/>
2176  </robot>)";
2177 
2178  tinyxml2::XMLDocument xml_doc;
2179  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2180 
2181  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2182  EXPECT_TRUE(robot_element != nullptr);
2183 
2184  tinyxml2::XMLElement* element = robot_element->FirstChildElement("kinematics_plugin_config");
2185  EXPECT_TRUE(element != nullptr);
2186 
2187  EXPECT_ANY_THROW(tesseract_srdf::parseKinematicsPluginConfig(locator, element, version)); // NOLINT
2188 
2189  tesseract_srdf::SRDFModel srdf_model;
2190  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2191  }
2192 
2193  { // failure
2194  std::string str = R"(<robot name="abb_irb2400">
2195  <kinematics_plugin_config filename="package://tesseract_support/urdf/malformed_config.yaml"/>
2196  </robot>)";
2197 
2198  tinyxml2::XMLDocument xml_doc;
2199  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2200 
2201  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2202  EXPECT_TRUE(robot_element != nullptr);
2203 
2204  tinyxml2::XMLElement* element = robot_element->FirstChildElement("kinematics_plugin_config");
2205  EXPECT_TRUE(element != nullptr);
2206 
2207  EXPECT_ANY_THROW(tesseract_srdf::parseKinematicsPluginConfig(locator, element, version)); // NOLINT
2208 
2209  tesseract_srdf::SRDFModel srdf_model;
2210  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2211  }
2212 }
2213 
2214 TEST(TesseractSRDFUnit, ParseCalibrationConfigUnit) // NOLINT
2215 {
2216  std::array<int, 3> version{ 1, 0, 0 };
2217  tesseract_common::GeneralResourceLocator locator;
2218  tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph();
2219 
2220  { // valid
2221  std::string str = R"(<robot name="abb_irb2400">
2222  <calibration_config filename="package://tesseract_support/urdf/abb_irb2400_calibration.yaml"/>
2223  </robot>)";
2224 
2225  tinyxml2::XMLDocument xml_doc;
2226  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2227 
2228  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2229  EXPECT_TRUE(robot_element != nullptr);
2230 
2231  tinyxml2::XMLElement* element = robot_element->FirstChildElement("calibration_config");
2232  EXPECT_TRUE(element != nullptr);
2233 
2234  tesseract_common::CalibrationInfo info = tesseract_srdf::parseCalibrationConfig(*g, locator, element, version);
2235  EXPECT_FALSE(info.empty());
2236  }
2237 
2238  { // failure
2239  std::string str = R"(<robot name="abb_irb2400">
2240  <calibration_config filename="package://tesseract_support/urdf/does_not_exist.yaml"/>
2241  </robot>)";
2242 
2243  tinyxml2::XMLDocument xml_doc;
2244  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2245 
2246  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2247  EXPECT_TRUE(robot_element != nullptr);
2248 
2249  tinyxml2::XMLElement* element = robot_element->FirstChildElement("calibration_config");
2250  EXPECT_TRUE(element != nullptr);
2251 
2252  EXPECT_ANY_THROW(tesseract_srdf::parseCalibrationConfig(*g, locator, element, version)); // NOLINT
2253 
2254  tesseract_srdf::SRDFModel srdf_model;
2255  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2256  }
2257 
2258  { // failure
2259  std::string str = R"(<robot name="abb_irb2400">
2260  <calibration_config/>
2261  </robot>)";
2262 
2263  tinyxml2::XMLDocument xml_doc;
2264  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2265 
2266  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2267  EXPECT_TRUE(robot_element != nullptr);
2268 
2269  tinyxml2::XMLElement* element = robot_element->FirstChildElement("calibration_config");
2270  EXPECT_TRUE(element != nullptr);
2271 
2272  EXPECT_ANY_THROW(tesseract_srdf::parseCalibrationConfig(*g, locator, element, version)); // NOLINT
2273 
2274  tesseract_srdf::SRDFModel srdf_model;
2275  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2276  }
2277 
2278  { // failure
2279  std::string str = R"(<robot name="abb_irb2400">
2280  <calibration_config filename="package://tesseract_support/urdf/malformed_config.yaml"/>
2281  </robot>)";
2282 
2283  tinyxml2::XMLDocument xml_doc;
2284  EXPECT_TRUE(xml_doc.Parse(str.c_str()) == tinyxml2::XML_SUCCESS);
2285 
2286  tinyxml2::XMLElement* robot_element = xml_doc.FirstChildElement("robot");
2287  EXPECT_TRUE(robot_element != nullptr);
2288 
2289  tinyxml2::XMLElement* element = robot_element->FirstChildElement("calibration_config");
2290  EXPECT_TRUE(element != nullptr);
2291 
2292  EXPECT_ANY_THROW(tesseract_srdf::parseCalibrationConfig(*g, locator, element, version)); // NOLINT
2293 
2294  tesseract_srdf::SRDFModel srdf_model;
2295  EXPECT_ANY_THROW(srdf_model.initString(*g, str, locator)); // NOLINT
2296  }
2297 }
2298 
2299 int main(int argc, char** argv)
2300 {
2301  testing::InitGoogleTest(&argc, argv);
2302 
2303  return RUN_ALL_TESTS();
2304 }
tesseract_scene_graph::JointType::REVOLUTE
@ REVOLUTE
tesseract_common::ContactManagersPluginInfo::empty
bool empty() const
tesseract_srdf::KinematicsInformation::addJointGroup
void addJointGroup(const std::string &group_name, const JointGroup &joint_group)
Add joint group.
Definition: kinematics_information.cpp:105
tesseract_common::CalibrationInfo::empty
bool empty() const
yaml_extenstions.h
graph.h
tesseract_srdf::JointGroup
std::vector< std::string > JointGroup
Definition: kinematics_information.h:56
tesseract_srdf::parseGroupTCPs
GroupTCPs parseGroupTCPs(const tesseract_scene_graph::SceneGraph &scene_graph, const tinyxml2::XMLElement *srdf_xml, const std::array< int, 3 > &version)
Parse groups tool center points from srdf xml element.
Definition: group_tool_center_points.cpp:47
tesseract_srdf
Main namespace.
Definition: collision_margins.h:43
tesseract_common::getTempPath
std::string getTempPath()
tesseract_srdf::SRDFModel::name
std::string name
The name of the srdf model.
Definition: srdf_model.h:94
tesseract_common
tesseract_srdf::parseGroupStates
GroupJointStates parseGroupStates(const tesseract_scene_graph::SceneGraph &scene_graph, const GroupNames &group_names, const tinyxml2::XMLElement *srdf_xml, const std::array< int, 3 > &version)
Parse groups states from srdf xml element.
Definition: group_states.cpp:38
tesseract_srdf::KinematicsInformation::group_tcps
GroupTCPs group_tcps
A map of group tool center points.
Definition: kinematics_information.h:88
tesseract_srdf::parseContactManagersPluginConfig
tesseract_common::ContactManagersPluginInfo parseContactManagersPluginConfig(const tesseract_common::ResourceLocator &locator, const tinyxml2::XMLElement *xml_element, const std::array< int, 3 > &version)
Parse contact managers plugin config xml element.
Definition: configs.cpp:123
tesseract_common::AllowedCollisionMatrix::isCollisionAllowed
virtual bool isCollisionAllowed(const std::string &link_name1, const std::string &link_name2) const
tesseract_srdf::SRDFModel::contact_managers_plugin_info
tesseract_common::ContactManagersPluginInfo contact_managers_plugin_info
The contact managers plugin information.
Definition: srdf_model.h:103
tesseract_common::AllowedCollisionMatrix::getAllAllowedCollisions
const AllowedCollisionEntries & getAllAllowedCollisions() const
tesseract_srdf::KinematicsInformation::removeJointGroup
void removeJointGroup(const std::string &group_name)
Remove joint group.
Definition: kinematics_information.cpp:111
tesseract_srdf::KinematicsInformation::hasJointGroup
bool hasJointGroup(const std::string &group_name) const
Check if joint group exists.
Definition: kinematics_information.cpp:117
utils.h
tesseract_srdf::SRDFModel::saveToFile
bool saveToFile(const std::string &file_path) const
Save the model to a file.
Definition: srdf_model.cpp:253
group_states.h
Parse group states data from srdf file.
ABBConfig
ABBConfig
Definition: tesseract_srdf_unit.cpp:27
ABBConfig::ROBOT_ONLY
@ ROBOT_ONLY
resource_locator.h
tesseract_srdf::parseConfigFilePath
std::filesystem::path parseConfigFilePath(const tesseract_common::ResourceLocator &locator, const tinyxml2::XMLElement *xml_element, const std::array< int, 3 > &version)
Parse a config xml element.
Definition: configs.cpp:41
tesseract_srdf::KinematicsInformation::removeChainGroup
void removeChainGroup(const std::string &group_name)
Remove chain group.
Definition: kinematics_information.cpp:94
collision_margin_data.h
buildTestSceneGraph
tesseract_scene_graph::SceneGraph buildTestSceneGraph()
Definition: tesseract_srdf_unit.cpp:175
joint.h
tesseract_scene_graph::Joint::parent_to_joint_origin_transform
Eigen::Isometry3d parent_to_joint_origin_transform
tesseract_scene_graph::SceneGraph::getAllowedCollisionMatrix
std::shared_ptr< tesseract_common::AllowedCollisionMatrix > getAllowedCollisionMatrix()
tesseract_srdf::SRDFModel::collision_margin_data
std::shared_ptr< tesseract_common::CollisionMarginData > collision_margin_data
Collision margin data.
Definition: srdf_model.h:109
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
tesseract_srdf::SRDFModel::kinematics_information
KinematicsInformation kinematics_information
Contact information related to kinematics.
Definition: srdf_model.h:100
tesseract_scene_graph::Joint::axis
Eigen::Vector3d axis
tesseract_srdf::KinematicsInformation::hasGroup
bool hasGroup(const std::string &group_name) const
Check if group exists.
Definition: kinematics_information.cpp:83
tesseract_scene_graph::SceneGraph
tesseract_srdf::SRDFModel
Representation of semantic information about the robot.
Definition: srdf_model.h:54
tesseract_srdf::KinematicsInformation::hasChainGroup
bool hasChainGroup(const std::string &group_name) const
Check if chain group exists.
Definition: kinematics_information.cpp:100
tesseract_srdf::KinematicsInformation::removeGroupJointState
void removeGroupJointState(const std::string &group_name, const std::string &state_name)
Remove group joint state.
Definition: kinematics_information.cpp:146
tesseract_scene_graph::SceneGraph::addLink
bool addLink(const Link &link, bool replace_allowed=false)
tesseract_common::CollisionMarginData::Ptr
std::shared_ptr< CollisionMarginData > Ptr
tesseract_srdf::KinematicsInformation::addLinkGroup
void addLinkGroup(const std::string &group_name, const LinkGroup &link_group)
Add link group.
Definition: kinematics_information.cpp:122
TempResourceLocator
Definition: tesseract_srdf_unit.cpp:519
tesseract_srdf::parseCollisionMargins
std::shared_ptr< tesseract_common::CollisionMarginData > parseCollisionMargins(const tesseract_scene_graph::SceneGraph &scene_graph, const tinyxml2::XMLElement *srdf_xml, const std::array< int, 3 > &version)
Parse allowed collisions from srdf xml element.
Definition: collision_margins.cpp:41
configs.h
tesseract_srdf::KinematicsInformation::addGroupTCP
void addGroupTCP(const std::string &group_name, const std::string &tcp_name, const Eigen::Isometry3d &tcp)
Add group tool center point.
Definition: kinematics_information.cpp:163
tesseract_srdf::SRDFModel::initString
void initString(const tesseract_scene_graph::SceneGraph &scene_graph, const std::string &xmlstring, const tesseract_common::ResourceLocator &locator)
Load Model from a XML-string.
Definition: srdf_model.cpp:94
utils.h
Tesseract SRDF utility functions.
tesseract_srdf::KinematicsInformation::link_groups
LinkGroups link_groups
A map of link groups.
Definition: kinematics_information.h:82
tesseract_srdf::ChainGroup
std::vector< std::pair< std::string, std::string > > ChainGroup
Definition: kinematics_information.h:54
tesseract_srdf::KinematicsInformation::addGroupJointState
void addGroupJointState(const std::string &group_name, const std::string &state_name, const GroupsJointState &joint_state)
Add group joint state.
Definition: kinematics_information.cpp:139
tesseract_srdf::SRDFModel::initFile
void initFile(const tesseract_scene_graph::SceneGraph &scene_graph, const std::string &filename, const tesseract_common::ResourceLocator &locator)
Load Model given a filename.
Definition: srdf_model.cpp:62
tesseract_common::AllowedCollisionMatrix::ConstPtr
std::shared_ptr< const AllowedCollisionMatrix > ConstPtr
tesseract_scene_graph::Joint::child_link_name
std::string child_link_name
tesseract_scene_graph::Joint::limits
JointLimits::Ptr limits
tesseract_srdf::KinematicsInformation::removeLinkGroup
void removeLinkGroup(const std::string &group_name)
Remove link group.
Definition: kinematics_information.cpp:128
tesseract_srdf::KinematicsInformation::group_names
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GroupNames group_names
A set of group names.
Definition: kinematics_information.h:73
tesseract_srdf::KinematicsInformation::insert
void insert(const KinematicsInformation &other)
Insert the content of an other KinematicsInformation.
Definition: kinematics_information.cpp:55
tesseract_srdf::KinematicsInformation::removeGroupTCP
void removeGroupTCP(const std::string &group_name, const std::string &tcp_name)
Remove group tool center point.
Definition: kinematics_information.cpp:170
tesseract_srdf::SRDFModel::version
std::array< int, 3 > version
The version number major.minor[.patch].
Definition: srdf_model.h:97
tesseract_common::CalibrationInfo
tesseract_scene_graph::SceneGraph::addJoint
bool addJoint(const Joint &joint)
tesseract_srdf::parseKinematicsPluginConfig
tesseract_common::KinematicsPluginInfo parseKinematicsPluginConfig(const tesseract_common::ResourceLocator &locator, const tinyxml2::XMLElement *xml_element, const std::array< int, 3 > &version)
Parse kinematics plugin config xml element.
Definition: configs.cpp:98
tesseract_srdf::parseGroups
std::tuple< GroupNames, ChainGroups, JointGroups, LinkGroups > parseGroups(const tesseract_scene_graph::SceneGraph &scene_graph, const tinyxml2::XMLElement *srdf_xml, const std::array< int, 3 > &version)
Parse groups from srdf xml element.
Definition: groups.cpp:40
tesseract_common::ContactManagersPluginInfo
tesseract_scene_graph::JointType::PRISMATIC
@ PRISMATIC
tesseract_srdf::GroupTCPs
tesseract_common::AlignedMap< std::string, GroupsTCPs > GroupTCPs
Definition: kinematics_information.h:53
tesseract_common::printNestedException
void printNestedException(const std::exception &e, int level=0)
tesseract_common::ResourceLocator
tesseract_srdf::SRDFModel::acm
tesseract_common::AllowedCollisionMatrix acm
The allowed collision matrix.
Definition: srdf_model.h:106
tesseract_scene_graph::SceneGraph::removeAllowedCollision
void removeAllowedCollision(const std::string &link_name)
tesseract_common::loadYamlString
YAML::Node loadYamlString(const std::string &yaml_string, const ResourceLocator &locator)
tesseract_srdf::KinematicsInformation
This hold the kinematics information used to create the SRDF and is the data container for the manipu...
Definition: kinematics_information.h:66
tesseract_scene_graph::Link
disabled_collisions.h
Parse disabled collision data from srdf file.
tesseract_common::KinematicsPluginInfo
tesseract_srdf::KinematicsInformation::hasLinkGroup
bool hasLinkGroup(const std::string &group_name) const
Check if link group exists.
Definition: kinematics_information.cpp:134
TEST
TEST(TesseractSRDFUnit, LoadSRDFFileUnit)
Definition: tesseract_srdf_unit.cpp:234
tesseract_srdf::parseDisabledCollisions
tesseract_common::AllowedCollisionMatrix parseDisabledCollisions(const tesseract_scene_graph::SceneGraph &scene_graph, const tinyxml2::XMLElement *srdf_xml, const std::array< int, 3 > &version)
Parse allowed collisions from srdf xml element.
Definition: disabled_collisions.cpp:40
tesseract_srdf::GroupNames
std::set< std::string > GroupNames
Definition: kinematics_information.h:60
tesseract_scene_graph::Joint
tesseract_common::GeneralResourceLocator::locateResource
std::shared_ptr< Resource > locateResource(const std::string &url) const override
tesseract_scene_graph::SceneGraph::clearAllowedCollisions
void clearAllowedCollisions()
tesseract_srdf::KinematicsInformation::joint_groups
JointGroups joint_groups
A map of joint groups.
Definition: kinematics_information.h:79
tesseract_srdf::GroupsJointState
std::unordered_map< std::string, double > GroupsJointState
Definition: kinematics_information.h:49
tesseract_srdf::KinematicsInformation::hasGroupJointState
bool hasGroupJointState(const std::string &group_name, const std::string &state_name) const
Check if group joint state exists.
Definition: kinematics_information.cpp:154
getABBSceneGraph
tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config=ABBConfig::ROBOT_ONLY)
Definition: tesseract_srdf_unit.cpp:34
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_scene_graph::SceneGraph::setName
void setName(const std::string &name)
tesseract_common::AllowedCollisionMatrix
tesseract_srdf::KinematicsInformation::hasGroupTCP
bool hasGroupTCP(const std::string &group_name, const std::string &tcp_name) const
Check if group tool center point exists.
Definition: kinematics_information.cpp:178
ABBConfig::ROBOT_WITH_POSITIONER
@ ROBOT_WITH_POSITIONER
yaml_utils.h
groups.h
Parse groups data from srdf file.
tesseract_common::GeneralResourceLocator
tesseract_scene_graph::Joint::type
JointType type
tesseract_srdf::parseCalibrationConfig
tesseract_common::CalibrationInfo parseCalibrationConfig(const tesseract_scene_graph::SceneGraph &scene_graph, const tesseract_common::ResourceLocator &locator, const tinyxml2::XMLElement *xml_element, const std::array< int, 3 > &version)
Parse calibration config xml element.
Definition: configs.cpp:65
tesseract_srdf::processSRDFAllowedCollisions
void processSRDFAllowedCollisions(tesseract_scene_graph::SceneGraph &scene_graph, const SRDFModel &srdf_model)
Add allowed collisions to the scene graph.
Definition: utils.cpp:33
srdf_model.h
Parse srdf xml.
tesseract_srdf::LinkGroup
std::vector< std::string > LinkGroup
Definition: kinematics_information.h:58
tesseract_srdf::SRDFModel::calibration_info
tesseract_common::CalibrationInfo calibration_info
The calibration information.
Definition: srdf_model.h:112
collision_margins.h
Parse collision margin data from srdf file.
macros.h
tesseract_scene_graph::SceneGraph::Ptr
std::shared_ptr< SceneGraph > Ptr
tesseract_srdf::KinematicsInformation::chain_groups
ChainGroups chain_groups
A map of chains groups.
Definition: kinematics_information.h:76
tesseract_scene_graph
tesseract_srdf::KinematicsInformation::group_states
GroupJointStates group_states
A map of group states.
Definition: kinematics_information.h:85
ABBConfig::ROBOT_ON_RAIL
@ ROBOT_ON_RAIL
tesseract_scene_graph::Joint::parent_link_name
std::string parent_link_name
tesseract_srdf::KinematicsInformation::kinematics_plugin_info
tesseract_common::KinematicsPluginInfo kinematics_plugin_info
The kinematics plugin information.
Definition: kinematics_information.h:91
group_tool_center_points.h
Parse group tool center points data from srdf file.
tesseract_scene_graph::JointType::FIXED
@ FIXED
tesseract_srdf::GroupJointStates
std::unordered_map< std::string, GroupsJointStates > GroupJointStates
Definition: kinematics_information.h:51
main
int main(int, char **)
Definition: parse_srdf_example.cpp:23
tesseract_common::KinematicsPluginInfo::empty
bool empty() const
tesseract_srdf::KinematicsInformation::addChainGroup
void addChainGroup(const std::string &group_name, const ChainGroup &chain_group)
Add chain group.
Definition: kinematics_information.cpp:88

tesseract_srdf
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:04