tesseract_urdf_link_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 <Eigen/Geometry>
5 TESSERACT_COMMON_IGNORE_WARNINGS_POP
6 
7 #include <tesseract_geometry/impl/box.h>
8 #include <tesseract_urdf/box.h>
9 #include <tesseract_urdf/link.h>
10 #include <tesseract_common/resource_locator.h>
11 #include "tesseract_urdf_common_unit.h"
12 
13 TEST(TesseractURDFUnit, parse_link) // NOLINT
14 {
15  tesseract_common::GeneralResourceLocator resource_locator;
16  const bool global_make_convex = false;
17  const auto parse_link_fn =
18  [&](const tinyxml2::XMLElement* xml_element,
19  const tesseract_common::ResourceLocator& locator,
20  std::unordered_map<std::string, std::shared_ptr<tesseract_scene_graph::Material>>& available_materials) {
21  return tesseract_urdf::parseLink(xml_element, locator, global_make_convex, available_materials);
22  };
23 
24  {
25  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
26  std::string str = R"(<link name="my_link" extra="0 0 0">
27  <inertial>
28  <origin xyz="0 0 0.5" rpy="0 0 0"/>
29  <mass value="1" />
30  <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
31  </inertial>
32  <visual>
33  <origin xyz="0 0 0" rpy="0 0 0" />
34  <geometry>
35  <box size="1 1 1" />
36  </geometry>
37  <material name="Cyan">
38  <color rgba="0 1.0 1.0 1.0" />
39  </material>
40  </visual>
41  <collision>
42  <origin xyz="0 0 0" rpy="0 0 0" />
43  <geometry>
44  <cylinder radius="1" length="0.5" />
45  </geometry>
46  </collision>
47  </link>)";
48  tesseract_scene_graph::Link::Ptr elem;
49  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
50  parse_link_fn,
51  str,
52  tesseract_urdf::LINK_ELEMENT_NAME.data(),
53  resource_locator,
54  empty_available_materials));
55  EXPECT_TRUE(elem->getName() == "my_link");
56  EXPECT_TRUE(elem->inertial != nullptr);
57  EXPECT_TRUE(elem->visual.size() == 1);
58  EXPECT_TRUE(elem->collision.size() == 1);
59  EXPECT_TRUE(empty_available_materials.size() == 1);
60  }
61 
62  {
63  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
64  std::string str = R"(<link name="my_link" extra="0 0 0">
65  <inertial>
66  <origin xyz="0 0 0.5" rpy="0 0 0"/>
67  <mass value="1" />
68  <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
69  </inertial>
70  <visual>
71  <origin xyz="0 0 0" rpy="0 0 0" />
72  <geometry>
73  <box size="1 1 1" />
74  </geometry>
75  <material name="Cyan">
76  <color rgba="0 1.0 1.0 1.0" />
77  </material>
78  </visual>
79  <visual>
80  <origin xyz="0 0 0" rpy="0 0 0" />
81  <geometry>
82  <box size="1 1 1" />
83  </geometry>
84  <material name="Cyan">
85  <color rgba="0 1.0 1.0 1.0" />
86  </material>
87  </visual>
88  <collision>
89  <origin xyz="0 0 0" rpy="0 0 0" />
90  <geometry>
91  <cylinder radius="1" length="0.5" />
92  </geometry>
93  </collision>
94  <collision>
95  <origin xyz="0 0 0" rpy="0 0 0" />
96  <geometry>
97  <cylinder radius="1" length="0.5" />
98  </geometry>
99  </collision>
100  </link>)";
101  tesseract_scene_graph::Link::Ptr elem;
102  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
103  parse_link_fn,
104  str,
105  tesseract_urdf::LINK_ELEMENT_NAME.data(),
106  resource_locator,
107  empty_available_materials));
108  EXPECT_TRUE(elem->getName() == "my_link");
109  EXPECT_TRUE(elem->inertial != nullptr);
110  EXPECT_TRUE(elem->visual.size() == 2);
111  EXPECT_TRUE(elem->collision.size() == 2);
112  EXPECT_TRUE(empty_available_materials.size() == 1);
113  }
114 
115  {
116  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
117  std::string str = R"(<link name="my_link">
118  <visual>
119  <origin xyz="0 0 0" rpy="0 0 0" />
120  <geometry>
121  <box size="1 1 1" />
122  </geometry>
123  <material name="Cyan">
124  <color rgba="0 1.0 1.0 1.0" />
125  </material>
126  </visual>
127  <collision>
128  <origin xyz="0 0 0" rpy="0 0 0" />
129  <geometry>
130  <cylinder radius="1" length="0.5" />
131  </geometry>
132  </collision>
133  </link>)";
134  tesseract_scene_graph::Link::Ptr elem;
135  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
136  parse_link_fn,
137  str,
138  tesseract_urdf::LINK_ELEMENT_NAME.data(),
139  resource_locator,
140  empty_available_materials));
141  EXPECT_TRUE(elem->getName() == "my_link");
142  EXPECT_TRUE(elem->inertial == nullptr);
143  EXPECT_TRUE(elem->visual.size() == 1);
144  EXPECT_TRUE(elem->collision.size() == 1);
145  EXPECT_TRUE(empty_available_materials.size() == 1);
146  }
147 
148  {
149  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
150  std::string str = R"(<link name="my_link">
151  <visual>
152  <origin xyz="0 0 0" rpy="0 0 0" />
153  <geometry>
154  <box size="1 1 1" />
155  </geometry>
156  <material name="Cyan">
157  <color rgba="0 1.0 1.0 1.0" />
158  </material>
159  </visual>
160  <visual>
161  <origin xyz="0 0 0" rpy="0 0 0" />
162  <geometry>
163  <box size="1 1 1" />
164  </geometry>
165  <material name="Cyan" />>
166  </visual>
167  <collision>
168  <origin xyz="0 0 0" rpy="0 0 0" />
169  <geometry>
170  <cylinder radius="1" length="0.5" />
171  </geometry>
172  </collision>
173  <collision>
174  <origin xyz="0 0 0" rpy="0 0 0" />
175  <geometry>
176  <cylinder radius="1" length="0.5" />
177  </geometry>
178  </collision>
179  </link>)";
180  tesseract_scene_graph::Link::Ptr elem;
181  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
182  parse_link_fn,
183  str,
184  tesseract_urdf::LINK_ELEMENT_NAME.data(),
185  resource_locator,
186  empty_available_materials));
187  EXPECT_TRUE(elem->getName() == "my_link");
188  EXPECT_TRUE(elem->inertial == nullptr);
189  EXPECT_TRUE(elem->visual.size() == 2);
190  EXPECT_TRUE(elem->collision.size() == 2);
191  EXPECT_TRUE(empty_available_materials.size() == 1);
192  }
193 
194  {
195  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
196  std::string str = R"(<link name="my_link">
197  <visual>
198  <origin xyz="0 0 0" rpy="0 0 0" />
199  <geometry>
200  <box size="1 1 1" />
201  </geometry>
202  <material name="Cyan">
203  <color rgba="0 1.0 1.0 1.0" />
204  </material>
205  </visual>
206  </link>)";
207  tesseract_scene_graph::Link::Ptr elem;
208  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
209  parse_link_fn,
210  str,
211  tesseract_urdf::LINK_ELEMENT_NAME.data(),
212  resource_locator,
213  empty_available_materials));
214  EXPECT_TRUE(elem->getName() == "my_link");
215  EXPECT_TRUE(elem->inertial == nullptr);
216  EXPECT_TRUE(elem->visual.size() == 1);
217  EXPECT_TRUE(elem->collision.empty());
218  EXPECT_TRUE(empty_available_materials.size() == 1);
219  }
220 
221  {
222  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
223  std::string str = R"(<link name="my_link">
224  <visual>
225  <origin xyz="0 0 0" rpy="0 0 0" />
226  <geometry>
227  <box size="1 1 1" />
228  </geometry>
229  <material name="Cyan">
230  <color rgba="0 1.0 1.0 1.0" />
231  </material>
232  </visual>
233  <visual>
234  <origin xyz="0 0 0" rpy="0 0 0" />
235  <geometry>
236  <box size="1 1 1" />
237  </geometry>
238  <material name="Cyan" />
239  </visual>
240  </link>)";
241  tesseract_scene_graph::Link::Ptr elem;
242  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
243  parse_link_fn,
244  str,
245  tesseract_urdf::LINK_ELEMENT_NAME.data(),
246  resource_locator,
247  empty_available_materials));
248  EXPECT_TRUE(elem->getName() == "my_link");
249  EXPECT_TRUE(elem->inertial == nullptr);
250  EXPECT_TRUE(elem->visual.size() == 2);
251  EXPECT_TRUE(elem->collision.empty());
252  EXPECT_TRUE(empty_available_materials.size() == 1);
253  }
254 
255  {
256  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
257  std::string str = R"(<link name="my_link">
258  <collision>
259  <origin xyz="0 0 0" rpy="0 0 0" />
260  <geometry>
261  <cylinder radius="1" length="0.5" />
262  </geometry>
263  </collision>
264  </link>)";
265  tesseract_scene_graph::Link::Ptr elem;
266  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
267  parse_link_fn,
268  str,
269  tesseract_urdf::LINK_ELEMENT_NAME.data(),
270  resource_locator,
271  empty_available_materials));
272  EXPECT_TRUE(elem->getName() == "my_link");
273  EXPECT_TRUE(elem->inertial == nullptr);
274  EXPECT_TRUE(elem->visual.empty());
275  EXPECT_TRUE(elem->collision.size() == 1);
276  EXPECT_TRUE(empty_available_materials.empty());
277  }
278 
279  {
280  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
281  std::string str = R"(<link name="my_link">
282  <collision>
283  <origin xyz="0 0 0" rpy="0 0 0" />
284  <geometry>
285  <cylinder radius="1" length="0.5" />
286  </geometry>
287  </collision>
288  <collision>
289  <origin xyz="0 0 0" rpy="0 0 0" />
290  <geometry>
291  <cylinder radius="1" length="0.5" />
292  </geometry>
293  </collision>
294  </link>)";
295  tesseract_scene_graph::Link::Ptr elem;
296  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
297  parse_link_fn,
298  str,
299  tesseract_urdf::LINK_ELEMENT_NAME.data(),
300  resource_locator,
301  empty_available_materials));
302  EXPECT_TRUE(elem->getName() == "my_link");
303  EXPECT_TRUE(elem->inertial == nullptr);
304  EXPECT_TRUE(elem->visual.empty());
305  EXPECT_TRUE(elem->collision.size() == 2);
306  EXPECT_TRUE(empty_available_materials.empty());
307  }
308 
309  {
310  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
311  std::string str = R"(<link name="my_link"/>)";
312  tesseract_scene_graph::Link::Ptr elem;
313  EXPECT_TRUE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
314  parse_link_fn,
315  str,
316  tesseract_urdf::LINK_ELEMENT_NAME.data(),
317  resource_locator,
318  empty_available_materials));
319  EXPECT_TRUE(elem->getName() == "my_link");
320  EXPECT_TRUE(elem->inertial == nullptr);
321  EXPECT_TRUE(elem->visual.empty());
322  EXPECT_TRUE(elem->collision.empty());
323  }
324 
325  {
326  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
327  std::string str = R"(<link>
328  <visual>
329  <origin xyz="0 0 0" rpy="0 0 0" />
330  <geometry>
331  <box size="1 1 1" />
332  </geometry>
333  <material name="Cyan">
334  <color rgba="0 1.0 1.0 1.0" />
335  </material>
336  </visual>
337  </link>)";
338  tesseract_scene_graph::Link::Ptr elem;
339  EXPECT_FALSE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
340  parse_link_fn,
341  str,
342  tesseract_urdf::LINK_ELEMENT_NAME.data(),
343  resource_locator,
344  empty_available_materials));
345  }
346 
347  {
348  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
349  std::string str = R"(<link name="my_link">
350  <visual>
351  <origin xyz="0 0 0" rpy="0 0 a" />
352  <geometry>
353  <box size="1 1 1" />
354  </geometry>
355  <material name="Cyan">
356  <color rgba="0 1.0 1.0 1.0" />
357  </material>
358  </visual>
359  </link>)";
360  tesseract_scene_graph::Link::Ptr elem;
361  EXPECT_FALSE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
362  parse_link_fn,
363  str,
364  tesseract_urdf::LINK_ELEMENT_NAME.data(),
365  resource_locator,
366  empty_available_materials));
367  }
368 
369  {
370  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
371  std::string str = R"(<link name="my_link">
372  <inertial>
373  <mass value="a"/>
374  <inertia ixx="1.0" ixy="2.0" ixz="3.0" iyy="4.0" iyz="5.0" izz="6.0"/>
375  </inertial>
376  <collision>
377  <origin xyz="0 0 0" rpy="0 0 0" />
378  <geometry>
379  <cylinder radius="1" length="0.5" />
380  </geometry>
381  </collision>
382  </link>)";
383  tesseract_scene_graph::Link::Ptr elem;
384  EXPECT_FALSE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
385  parse_link_fn,
386  str,
387  tesseract_urdf::LINK_ELEMENT_NAME.data(),
388  resource_locator,
389  empty_available_materials));
390  }
391 
392  {
393  std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_available_materials;
394  std::string str = R"(<link name="my_link">
395  <collision>
396  <origin xyz="a 0 0" rpy="0 0 0" />
397  <geometry>
398  <cylinder radius="1" length="0.5" />
399  </geometry>
400  </collision>
401  </link>)";
402  tesseract_scene_graph::Link::Ptr elem;
403  EXPECT_FALSE(runTest<tesseract_scene_graph::Link::Ptr>(elem,
404  parse_link_fn,
405  str,
406  tesseract_urdf::LINK_ELEMENT_NAME.data(),
407  resource_locator,
408  empty_available_materials));
409  }
410 }
411 
412 TEST(TesseractURDFUnit, write_link) // NOLINT
413 {
414  { // Trigger id adjustments and inertial
415  tesseract_scene_graph::Link::Ptr link =
416  std::make_shared<tesseract_scene_graph::Link>(tesseract_urdf::LINK_ELEMENT_NAME.data());
417  link->inertial = std::make_shared<tesseract_scene_graph::Inertial>();
418 
419  tesseract_scene_graph::Collision::Ptr collision = std::make_shared<tesseract_scene_graph::Collision>();
420  collision->name = "test";
421  collision->origin = Eigen::Isometry3d::Identity();
422  collision->geometry = std::make_shared<tesseract_geometry::Box>(1.0, 1.0, 1.0);
423  link->collision.push_back(collision);
424  link->collision.push_back(collision);
425 
426  tesseract_scene_graph::Visual::Ptr visual = std::make_shared<tesseract_scene_graph::Visual>();
427  visual->name = "test";
428  visual->origin = Eigen::Isometry3d::Identity();
429  visual->geometry = std::make_shared<tesseract_geometry::Box>(1.0, 1.0, 1.0);
430  link->visual.push_back(visual);
431  link->visual.push_back(visual);
432 
433  std::string text;
434  EXPECT_EQ(0,
435  writeTest<tesseract_scene_graph::Link::Ptr>(
436  link, &tesseract_urdf::writeLink, text, tesseract_common::getTempPath()));
437  EXPECT_NE(text, "");
438  }
439 
440  { // Trigger nullptr collision
441  tesseract_scene_graph::Link::Ptr link =
442  std::make_shared<tesseract_scene_graph::Link>(tesseract_urdf::LINK_ELEMENT_NAME.data());
443  link->inertial = std::make_shared<tesseract_scene_graph::Inertial>();
444 
445  link->collision.push_back(nullptr);
446 
447  tesseract_scene_graph::Visual::Ptr visual = std::make_shared<tesseract_scene_graph::Visual>();
448  visual->name = "test";
449  visual->origin = Eigen::Isometry3d::Identity();
450  visual->geometry = std::make_shared<tesseract_geometry::Box>(1.0, 1.0, 1.0);
451  link->visual.push_back(visual);
452 
453  std::string text;
454  EXPECT_EQ(1,
455  writeTest<tesseract_scene_graph::Link::Ptr>(
456  link, &tesseract_urdf::writeLink, text, tesseract_common::getTempPath()));
457  EXPECT_EQ(text, "");
458  }
459 
460  { // Trigger nullptr visual
461  tesseract_scene_graph::Link::Ptr link =
462  std::make_shared<tesseract_scene_graph::Link>(tesseract_urdf::LINK_ELEMENT_NAME.data());
463  link->inertial = std::make_shared<tesseract_scene_graph::Inertial>();
464 
465  tesseract_scene_graph::Collision::Ptr collision = std::make_shared<tesseract_scene_graph::Collision>();
466  collision->name = "test";
467  collision->origin = Eigen::Isometry3d::Identity();
468  collision->geometry = std::make_shared<tesseract_geometry::Box>(1.0, 1.0, 1.0);
469  link->collision.push_back(collision);
470 
471  link->visual.push_back(nullptr);
472 
473  std::string text;
474  EXPECT_EQ(1,
475  writeTest<tesseract_scene_graph::Link::Ptr>(
476  link, &tesseract_urdf::writeLink, text, tesseract_common::getTempPath()));
477  EXPECT_EQ(text, "");
478  }
479 
480  {
481  tesseract_scene_graph::Link::Ptr link = nullptr;
482  std::string text;
483  EXPECT_EQ(1,
484  writeTest<tesseract_scene_graph::Link::Ptr>(
485  link, &tesseract_urdf::writeLink, text, tesseract_common::getTempPath()));
486  EXPECT_EQ(text, "");
487  }
488 }
tesseract_scene_graph::Link::Ptr
std::shared_ptr< Link > Ptr
tesseract_common::getTempPath
std::string getTempPath()
resource_locator.h
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
EXPECT_TRUE
#define EXPECT_TRUE(args)
box.h
Parse box from xml string.
tesseract_urdf::writeLink
tinyxml2::XMLElement * writeLink(const std::shared_ptr< const tesseract_scene_graph::Link > &link, tinyxml2::XMLDocument &doc, const std::string &package_path)
writeLink Write a link to URDF XML
Definition: link.cpp:111
tesseract_urdf_common_unit.h
tesseract_scene_graph::Collision::Ptr
std::shared_ptr< Collision > Ptr
tesseract_common::ResourceLocator
TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_urdf::parseLink
std::shared_ptr< tesseract_scene_graph::Link > parseLink(const tinyxml2::XMLElement *xml_element, const tesseract_common::ResourceLocator &locator, bool make_convex_meshes, std::unordered_map< std::string, std::shared_ptr< tesseract_scene_graph::Material >> &available_materials)
Parse xml element link.
TEST
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_link)
Definition: tesseract_urdf_link_unit.cpp:13
tesseract_scene_graph::Visual::Ptr
std::shared_ptr< Visual > Ptr
tesseract_common::GeneralResourceLocator
tesseract_urdf::LINK_ELEMENT_NAME
static constexpr std::string_view LINK_ELEMENT_NAME
Definition: link.h:47
macros.h
box.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)

tesseract_urdf
Author(s): Levi Armstrong
autogenerated on Thu Apr 24 2025 03:10:44