tesseract_urdf_sdf_mesh_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_urdf/sdf_mesh.h>
8 #include <tesseract_geometry/impl/sdf_mesh.h>
9 #include <tesseract_common/resource_locator.h>
10 #include "tesseract_urdf_common_unit.h"
11 
12 static std::string getTempPkgPath()
13 {
14  std::string tmp = tesseract_common::getTempPath();
15  std::string tmppkg = tmp + "tmppkg";
16  if (!std::filesystem::is_directory(tmppkg) || !std::filesystem::exists(tmppkg))
17  {
18  std::filesystem::create_directory(tmppkg);
19  }
20  return tmppkg;
21 }
22 
23 TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT
24 {
25  tesseract_common::GeneralResourceLocator resource_locator;
26  {
27  std::string str =
28  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl" scale="1 2 1" extra="0 0 0"/>)";
29  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
30  EXPECT_TRUE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
31  &tesseract_urdf::parseSDFMesh,
32  str,
33  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
34  resource_locator,
35  true));
36  EXPECT_TRUE(geom.size() == 1);
37  EXPECT_TRUE(geom[0]->getFaceCount() == 80);
38  EXPECT_TRUE(geom[0]->getVertexCount() == 240);
39  EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5);
40  EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5);
41  EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5);
42  }
43 
44  {
45  std::string str = R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl"/>)";
46  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
47  EXPECT_TRUE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
48  &tesseract_urdf::parseSDFMesh,
49  str,
50  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
51  resource_locator,
52  true));
53  EXPECT_TRUE(geom.size() == 1);
54  EXPECT_TRUE(geom[0]->getFaceCount() == 80);
55  EXPECT_TRUE(geom[0]->getVertexCount() == 240);
56  EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5);
57  EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5);
58  EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5);
59  }
60 
61  {
62  std::string str = R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl"/>)";
63  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
64  EXPECT_TRUE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
65  &tesseract_urdf::parseSDFMesh,
66  str,
67  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
68  resource_locator,
69  false));
70  EXPECT_TRUE(geom.size() == 1);
71  EXPECT_TRUE(geom[0]->getFaceCount() == 80);
72  EXPECT_TRUE(geom[0]->getVertexCount() == 42);
73  EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5);
74  EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5);
75  EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5);
76  }
77 
78  {
79  std::string str = R"(<tesseract:sdf_mesh filename="abc" scale="1 2 1"/>)";
80  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
81  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
82  &tesseract_urdf::parseSDFMesh,
83  str,
84  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
85  resource_locator,
86  true));
87  }
88 
89  {
90  std::string str =
91  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl" scale="1 a 1"/>)";
92  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
93  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
94  &tesseract_urdf::parseSDFMesh,
95  str,
96  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
97  resource_locator,
98  true));
99  }
100 
101  {
102  std::string str =
103  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl" scale="a 1 1"/>)";
104  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
105  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
106  &tesseract_urdf::parseSDFMesh,
107  str,
108  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
109  resource_locator,
110  true));
111  }
112 
113  {
114  std::string str =
115  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl" scale="1 1 a"/>)";
116  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
117  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
118  &tesseract_urdf::parseSDFMesh,
119  str,
120  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
121  resource_locator,
122  true));
123  }
124 
125  {
126  std::string str =
127  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.ply" scale="-1 2 1" />)";
128  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
129  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
130  &tesseract_urdf::parseSDFMesh,
131  str,
132  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
133  resource_locator,
134  true));
135  EXPECT_TRUE(geom.empty());
136  }
137 
138  {
139  std::string str =
140  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.ply" scale="1 -1 1" />)";
141  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
142  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
143  &tesseract_urdf::parseSDFMesh,
144  str,
145  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
146  resource_locator,
147  false));
148  EXPECT_TRUE(geom.empty());
149  }
150 
151  {
152  std::string str =
153  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.ply" scale="1 2 -1" />)";
154  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
155  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
156  &tesseract_urdf::parseSDFMesh,
157  str,
158  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
159  resource_locator,
160  false));
161  EXPECT_TRUE(geom.empty());
162  }
163 
164  {
165  std::string str =
166  R"(<tesseract:sdf_mesh filename="package://tesseract_support/meshes/sphere_p25m.stl" scale="1 2 1 3"/>)";
167  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
168  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
169  &tesseract_urdf::parseSDFMesh,
170  str,
171  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
172  resource_locator,
173  true));
174  }
175 
176  {
177  std::string str = R"(<tesseract:sdf_mesh scale="1 2 1"/>)";
178  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
179  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
180  &tesseract_urdf::parseSDFMesh,
181  str,
182  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
183  resource_locator,
184  true));
185  }
186 
187  {
188  std::string str = R"(<tesseract:sdf_mesh />)";
189  std::vector<tesseract_geometry::SDFMesh::Ptr> geom;
190  EXPECT_FALSE(runTest<std::vector<tesseract_geometry::SDFMesh::Ptr>>(geom,
191  &tesseract_urdf::parseSDFMesh,
192  str,
193  tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(),
194  resource_locator,
195  true));
196  }
197 }
198 
199 TEST(TesseractURDFUnit, write_sdf_mesh) // NOLINT
200 {
201  {
202  tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0),
203  Eigen::Vector3d(1, 0, 0),
204  Eigen::Vector3d(0, 1, 0) };
205  Eigen::VectorXi indices(4);
206  indices << 3, 0, 1, 2;
207  tesseract_geometry::SDFMesh::Ptr sdf_mesh = std::make_shared<tesseract_geometry::SDFMesh>(
208  std::make_shared<tesseract_common::VectorVector3d>(vertices), std::make_shared<Eigen::VectorXi>(indices));
209  std::string text;
210  EXPECT_EQ(0,
211  writeTest<tesseract_geometry::SDFMesh::Ptr>(
212  sdf_mesh, &tesseract_urdf::writeSDFMesh, text, getTempPkgPath(), std::string("sdf0.ply")));
213  EXPECT_EQ(text, R"(<tesseract:sdf_mesh filename="package://tmppkg/sdf0.ply"/>)");
214  }
215 
216  { // With scale
217  tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0),
218  Eigen::Vector3d(1, 0, 0),
219  Eigen::Vector3d(0, 1, 0) };
220 
221  Eigen::VectorXi indices(4);
222  indices << 3, 0, 1, 2;
223  Eigen::Vector3d scale(0.5, 0.5, 0.5);
224  tesseract_geometry::SDFMesh::Ptr sdf_mesh =
225  std::make_shared<tesseract_geometry::SDFMesh>(std::make_shared<tesseract_common::VectorVector3d>(vertices),
226  std::make_shared<Eigen::VectorXi>(indices),
227  nullptr,
228  scale);
229  std::string text;
230  EXPECT_EQ(0,
231  writeTest<tesseract_geometry::SDFMesh::Ptr>(
232  sdf_mesh, &tesseract_urdf::writeSDFMesh, text, getTempPkgPath(), std::string("sdf1.ply")));
233  EXPECT_EQ(text, R"(<tesseract:sdf_mesh filename="package://tmppkg/sdf1.ply" scale="0.5 0.5 0.5"/>)");
234  }
235 
236  {
237  tesseract_geometry::SDFMesh::Ptr sdf_mesh = nullptr;
238  std::string text;
239  EXPECT_EQ(
240  1,
241  writeTest<tesseract_geometry::SDFMesh::Ptr>(
242  sdf_mesh, &tesseract_urdf::writeSDFMesh, text, tesseract_common::getTempPath(), std::string("sdf2.ply")));
243  EXPECT_EQ(text, "");
244  }
245 }
tesseract_common::getTempPath
std::string getTempPath()
sdf_mesh.h
tesseract_geometry::SDFMesh::Ptr
std::shared_ptr< SDFMesh > Ptr
resource_locator.h
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_urdf_common_unit.h
runTest
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP bool runTest(ElementType &type, std::function< ElementType(const tinyxml2::XMLElement *)> func, const std::string &xml_string, const std::string &element_name)
Definition: tesseract_urdf_common_unit.h:21
tesseract_urdf::parseSDFMesh
std::vector< std::shared_ptr< tesseract_geometry::SDFMesh > > parseSDFMesh(const tinyxml2::XMLElement *xml_element, const tesseract_common::ResourceLocator &locator, bool visual)
Parse xml element sdf_mesh.
Definition: sdf_mesh.cpp:46
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
tesseract_common::VectorVector3d
std::vector< Eigen::Vector3d > VectorVector3d
TESSERACT_COMMON_IGNORE_WARNINGS_POP
TEST
TEST(TesseractURDFUnit, parse_sdf_mesh)
Definition: tesseract_urdf_sdf_mesh_unit.cpp:23
tesseract_common::GeneralResourceLocator
getTempPkgPath
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH static TESSERACT_COMMON_IGNORE_WARNINGS_POP std::string getTempPkgPath()
Definition: tesseract_urdf_sdf_mesh_unit.cpp:12
macros.h
sdf_mesh.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_urdf::SDF_MESH_ELEMENT_NAME
static constexpr std::string_view SDF_MESH_ELEMENT_NAME
Definition: sdf_mesh.h:46
tesseract_urdf::writeSDFMesh
tinyxml2::XMLElement * writeSDFMesh(const std::shared_ptr< const tesseract_geometry::SDFMesh > &sdf_mesh, tinyxml2::XMLDocument &doc, const std::string &package_path, const std::string &filename)
writeSDFMesh Write SDF Mesh to URDF XML. This is non-standard URDF / tesseract-exclusive
Definition: sdf_mesh.cpp:96

tesseract_urdf
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:07