From 3af9919fa230b31ed3bb6d47c27c93b5113a63f2 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 13 Dec 2024 15:47:05 -0600 Subject: [PATCH 1/3] Removed URDF parser tesseract version; added variables for element names; updated names of tesseract-specific elements --- tesseract_urdf/include/tesseract_urdf/box.h | 4 +- .../include/tesseract_urdf/calibration.h | 5 +- .../include/tesseract_urdf/capsule.h | 4 +- .../include/tesseract_urdf/collision.h | 5 +- tesseract_urdf/include/tesseract_urdf/cone.h | 4 +- .../include/tesseract_urdf/convex_mesh.h | 7 +- .../include/tesseract_urdf/cylinder.h | 4 +- .../include/tesseract_urdf/dynamics.h | 5 +- .../include/tesseract_urdf/geometry.h | 5 +- .../include/tesseract_urdf/inertial.h | 4 +- tesseract_urdf/include/tesseract_urdf/joint.h | 4 +- .../include/tesseract_urdf/limits.h | 4 +- tesseract_urdf/include/tesseract_urdf/link.h | 5 +- .../include/tesseract_urdf/material.h | 5 +- tesseract_urdf/include/tesseract_urdf/mesh.h | 5 +- tesseract_urdf/include/tesseract_urdf/mimic.h | 4 +- .../include/tesseract_urdf/octomap.h | 5 +- .../include/tesseract_urdf/octree.h | 5 +- .../include/tesseract_urdf/origin.h | 4 +- .../include/tesseract_urdf/point_cloud.h | 5 +- .../tesseract_urdf/safety_controller.h | 5 +- .../include/tesseract_urdf/sdf_mesh.h | 5 +- .../include/tesseract_urdf/sphere.h | 4 +- .../include/tesseract_urdf/visual.h | 5 +- tesseract_urdf/src/box.cpp | 11 +- tesseract_urdf/src/calibration.cpp | 13 ++- tesseract_urdf/src/capsule.cpp | 12 +- tesseract_urdf/src/collision.cpp | 26 +++-- tesseract_urdf/src/cone.cpp | 11 +- tesseract_urdf/src/convex_mesh.cpp | 22 ++-- tesseract_urdf/src/cylinder.cpp | 13 ++- tesseract_urdf/src/dynamics.cpp | 14 ++- tesseract_urdf/src/geometry.cpp | 107 ++++++++---------- tesseract_urdf/src/inertial.cpp | 15 ++- tesseract_urdf/src/joint.cpp | 24 ++-- tesseract_urdf/src/limits.cpp | 14 ++- tesseract_urdf/src/link.cpp | 25 ++-- tesseract_urdf/src/material.cpp | 20 ++-- tesseract_urdf/src/mesh.cpp | 21 ++-- tesseract_urdf/src/mimic.cpp | 13 ++- tesseract_urdf/src/octomap.cpp | 36 +++--- tesseract_urdf/src/octree.cpp | 23 ++-- tesseract_urdf/src/origin.cpp | 10 +- tesseract_urdf/src/point_cloud.cpp | 13 ++- tesseract_urdf/src/safety_controller.cpp | 14 ++- tesseract_urdf/src/sdf_mesh.cpp | 22 ++-- tesseract_urdf/src/sphere.cpp | 12 +- tesseract_urdf/src/urdf_parser.cpp | 38 +++---- tesseract_urdf/src/visual.cpp | 29 ++--- 49 files changed, 375 insertions(+), 295 deletions(-) diff --git a/tesseract_urdf/include/tesseract_urdf/box.h b/tesseract_urdf/include/tesseract_urdf/box.h index 26df475c380..6ce5715770b 100644 --- a/tesseract_urdf/include/tesseract_urdf/box.h +++ b/tesseract_urdf/include/tesseract_urdf/box.h @@ -41,12 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* BOX_ELEMENT_NAME = "box"; + /** * @brief Parse a xml box element * @param xml_element The xml element * @return Tesseract Geometry Box */ -std::shared_ptr parseBox(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseBox(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeBox(const std::shared_ptr& box, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/calibration.h b/tesseract_urdf/include/tesseract_urdf/calibration.h index 61bba134a75..dcf99a00203 100644 --- a/tesseract_urdf/include/tesseract_urdf/calibration.h +++ b/tesseract_urdf/include/tesseract_urdf/calibration.h @@ -41,13 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* CALIBRATION_ELEMENT_NAME = "calibration"; + /** * @brief Parse a xml calibration element * @param xml_element The xml element * @return Tesseract JointCalibration */ -std::shared_ptr parseCalibration(const tinyxml2::XMLElement* xml_element, - int version); +std::shared_ptr parseCalibration(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeCalibration(const std::shared_ptr& calibration, diff --git a/tesseract_urdf/include/tesseract_urdf/capsule.h b/tesseract_urdf/include/tesseract_urdf/capsule.h index 076294ab7ae..d86337fce5e 100644 --- a/tesseract_urdf/include/tesseract_urdf/capsule.h +++ b/tesseract_urdf/include/tesseract_urdf/capsule.h @@ -41,12 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* CAPSULE_ELEMENT_NAME = "tesseract:capsule"; + /** * @brief Parse a xml capsule element * @param xml_element The xml element * @return Tesseract Geometry Capsule */ -std::shared_ptr parseCapsule(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseCapsule(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeCapsule(const std::shared_ptr& capsule, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/collision.h b/tesseract_urdf/include/tesseract_urdf/collision.h index 66d2888e6ea..c6a917362a7 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -43,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* COLLISION_ELEMENT_NAME = "collision"; + /** * @brief Parse xml element collision * @param xml_element The xml element @@ -51,8 +53,7 @@ namespace tesseract_urdf * @return A Collision object */ std::shared_ptr parseCollision(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - int version); + const tesseract_common::ResourceLocator& locator); /** * @brief writeCollision Write collision object to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/cone.h b/tesseract_urdf/include/tesseract_urdf/cone.h index 63b826560f9..00ea755ecf2 100644 --- a/tesseract_urdf/include/tesseract_urdf/cone.h +++ b/tesseract_urdf/include/tesseract_urdf/cone.h @@ -41,12 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* CONE_ELEMENT_NAME = "tesseract:cone"; + /** * @brief Parse a xml cone element * @param xml_element The xml element * @return Tesseract Geometry Cone */ -std::shared_ptr parseCone(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseCone(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeCone(const std::shared_ptr& cone, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h index 64d10dc69ef..16c3b61c7c7 100644 --- a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h @@ -43,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* CONVEX_MESH_ELEMENT_NAME = "tesseract:convex_mesh"; + /** * @brief Parse xml element convex_mesh * @param xml_element The xml element @@ -52,10 +54,7 @@ namespace tesseract_urdf * @return A Tesseract Geometry ConvexMesh */ std::vector> -parseConvexMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version); +parseConvexMesh(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual); /** * @brief writeConvexMesh Write convex mesh to URDF XML. This is non-standard URDF / tesseract-exclusive diff --git a/tesseract_urdf/include/tesseract_urdf/cylinder.h b/tesseract_urdf/include/tesseract_urdf/cylinder.h index 6053a5768ca..bf60bd2d14d 100644 --- a/tesseract_urdf/include/tesseract_urdf/cylinder.h +++ b/tesseract_urdf/include/tesseract_urdf/cylinder.h @@ -41,12 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* CYLINDER_ELEMENT_NAME = "cylinder"; + /** * @brief Parse a xml cylinder element * @param xml_element The xml element * @return Tesseract Geometry Cylinder */ -std::shared_ptr parseCylinder(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseCylinder(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeCylinder(const std::shared_ptr& cylinder, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/dynamics.h b/tesseract_urdf/include/tesseract_urdf/dynamics.h index 090457552c2..dc9c40da764 100644 --- a/tesseract_urdf/include/tesseract_urdf/dynamics.h +++ b/tesseract_urdf/include/tesseract_urdf/dynamics.h @@ -41,13 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* DYNAMICS_ELEMENT_NAME = "dynamics"; + /** * @brief Parse a xml dynamics element * @param xml_element The xml element * @return Tesseract JointDynamics */ -std::shared_ptr parseDynamics(const tinyxml2::XMLElement* xml_element, - int version); +std::shared_ptr parseDynamics(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeDynamics(const std::shared_ptr& dynamics, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/geometry.h b/tesseract_urdf/include/tesseract_urdf/geometry.h index 1cedd772602..0d8d1e7e047 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -43,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* GEOMETRY_ELEMENT_NAME = "geometry"; + /** * @brief Parse xml element geometry * @param xml_element The xml element @@ -53,8 +55,7 @@ namespace tesseract_urdf */ std::shared_ptr parseGeometry(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - bool visual, - int version); + bool visual); /** * @brief writeGeometry Write geometry to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/inertial.h b/tesseract_urdf/include/tesseract_urdf/inertial.h index 825dda7cf4d..6e853454958 100644 --- a/tesseract_urdf/include/tesseract_urdf/inertial.h +++ b/tesseract_urdf/include/tesseract_urdf/inertial.h @@ -41,13 +41,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* INERTIAL_ELEMENT_NAME = "inertial"; + /** * @brief Parse xml element inertial * @param xml_element The xml element * @param version The version number * @return A Tesseract Inertial */ -std::shared_ptr parseInertial(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseInertial(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeInertial(const std::shared_ptr& inertial, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/joint.h b/tesseract_urdf/include/tesseract_urdf/joint.h index 0cba9d9995f..998e70d3741 100644 --- a/tesseract_urdf/include/tesseract_urdf/joint.h +++ b/tesseract_urdf/include/tesseract_urdf/joint.h @@ -41,13 +41,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* JOINT_ELEMENT_NAME = "joint"; + /** * @brief Parse xml element joint * @param xml_element The xml element * @param version The version number * @return A Tesseract Joint */ -std::shared_ptr parseJoint(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseJoint(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeJoint(const std::shared_ptr& joint, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/limits.h b/tesseract_urdf/include/tesseract_urdf/limits.h index 625400afc6a..05d397ec01f 100644 --- a/tesseract_urdf/include/tesseract_urdf/limits.h +++ b/tesseract_urdf/include/tesseract_urdf/limits.h @@ -41,13 +41,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* LIMITS_ELEMENT_NAME = "limit"; + /** * @brief Parse xml element limits * @param xml_element The xml element * @param version The version number * @return A Tesseract JointLimits */ -std::shared_ptr parseLimits(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseLimits(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeLimits(const std::shared_ptr& limits, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/link.h b/tesseract_urdf/include/tesseract_urdf/link.h index 353ee4e6347..eaa8436488e 100644 --- a/tesseract_urdf/include/tesseract_urdf/link.h +++ b/tesseract_urdf/include/tesseract_urdf/link.h @@ -43,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* LINK_ELEMENT_NAME = "link"; + /** * @brief Parse xml element link * @param xml_element The xml element @@ -54,8 +56,7 @@ namespace tesseract_urdf std::shared_ptr parseLink(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - std::unordered_map>& available_materials, - int version); + std::unordered_map>& available_materials); /** * @brief writeLink Write a link to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/material.h b/tesseract_urdf/include/tesseract_urdf/material.h index 9071ecd9530..30b8ce37e38 100644 --- a/tesseract_urdf/include/tesseract_urdf/material.h +++ b/tesseract_urdf/include/tesseract_urdf/material.h @@ -42,6 +42,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* MATERIAL_ELEMENT_NAME = "material"; + /** * @brief Parse xml element material * @param xml_element The xml element @@ -53,8 +55,7 @@ namespace tesseract_urdf std::shared_ptr parseMaterial(const tinyxml2::XMLElement* xml_element, std::unordered_map>& available_materials, - bool allow_anonymous, - int version); + bool allow_anonymous); tinyxml2::XMLElement* writeMaterial(const std::shared_ptr& material, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/mesh.h b/tesseract_urdf/include/tesseract_urdf/mesh.h index bcbe8aab8b3..e05e61be081 100644 --- a/tesseract_urdf/include/tesseract_urdf/mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/mesh.h @@ -43,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* MESH_ELEMENT_NAME = "mesh"; + /** * @brief Parse xml element mesh * @param xml_element The xml element @@ -53,8 +55,7 @@ namespace tesseract_urdf */ std::vector> parseMesh(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - bool visual, - int version); + bool visual); /** * @brief writeMesh Write a mesh to URDF XML and PLY file diff --git a/tesseract_urdf/include/tesseract_urdf/mimic.h b/tesseract_urdf/include/tesseract_urdf/mimic.h index fc11c4a3f57..2192750e165 100644 --- a/tesseract_urdf/include/tesseract_urdf/mimic.h +++ b/tesseract_urdf/include/tesseract_urdf/mimic.h @@ -41,13 +41,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* MIMIC_ELEMENT_NAME = "mimic"; + /** * @brief Parse xml element mimic * @param xml_element The xml element * @param version The version number * @return A Tesseract JointMimic */ -std::shared_ptr parseMimic(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseMimic(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeMimic(const std::shared_ptr& mimic, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/octomap.h b/tesseract_urdf/include/tesseract_urdf/octomap.h index 5bfc7586487..af467786d03 100644 --- a/tesseract_urdf/include/tesseract_urdf/octomap.h +++ b/tesseract_urdf/include/tesseract_urdf/octomap.h @@ -42,6 +42,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* OCTOMAP_ELEMENT_NAME = "tesseract:octomap"; + /** * @brief Parse xml element octomap * @param xml_element The xml element @@ -52,8 +54,7 @@ namespace tesseract_urdf */ std::shared_ptr parseOctomap(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - bool visual, - int version); + bool visual); /** * @brief writeOctomap Write octomap to URDF XML. This is non-standard URDF / tesseract-exclusive diff --git a/tesseract_urdf/include/tesseract_urdf/octree.h b/tesseract_urdf/include/tesseract_urdf/octree.h index 323e87aec92..3846df80f8f 100644 --- a/tesseract_urdf/include/tesseract_urdf/octree.h +++ b/tesseract_urdf/include/tesseract_urdf/octree.h @@ -42,6 +42,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* OCTREE_ELEMENT_NAME = "tesseract:octree"; + /** * @brief Parse xml element octree * @param xml_element The xml element @@ -53,8 +55,7 @@ namespace tesseract_urdf std::shared_ptr parseOctree(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, tesseract_geometry::OctreeSubType shape_type, - bool prune, - int version); + bool prune); /** * @brief writeOctree Write octree out to file, and generate appropriate xml diff --git a/tesseract_urdf/include/tesseract_urdf/origin.h b/tesseract_urdf/include/tesseract_urdf/origin.h index ab953b32c8a..443756306eb 100644 --- a/tesseract_urdf/include/tesseract_urdf/origin.h +++ b/tesseract_urdf/include/tesseract_urdf/origin.h @@ -39,13 +39,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* ORIGIN_ELEMENT_NAME = "origin"; + /** * @brief Parse xml element origin * @param xml_element The xml element * @param version The version number * @return A Eigen::Isometry3d */ -Eigen::Isometry3d parseOrigin(const tinyxml2::XMLElement* xml_element, int version); +Eigen::Isometry3d parseOrigin(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeOrigin(const Eigen::Isometry3d& origin, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/point_cloud.h b/tesseract_urdf/include/tesseract_urdf/point_cloud.h index 07a7a710a72..118b00cd43c 100644 --- a/tesseract_urdf/include/tesseract_urdf/point_cloud.h +++ b/tesseract_urdf/include/tesseract_urdf/point_cloud.h @@ -42,6 +42,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* POINT_CLOUD_ELEMENT_NAME = "tesseract:point_cloud"; + /** * @brief Parse xml element point_cloud * @param xml_element The xml element @@ -54,8 +56,7 @@ namespace tesseract_urdf std::shared_ptr parsePointCloud(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, tesseract_geometry::OctreeSubType shape_type, - bool prune, - int version); + bool prune); } // namespace tesseract_urdf #endif // TESSERACT_URDF_POINT_CLOUD_H diff --git a/tesseract_urdf/include/tesseract_urdf/safety_controller.h b/tesseract_urdf/include/tesseract_urdf/safety_controller.h index c5fe2745d1a..4fa54567c65 100644 --- a/tesseract_urdf/include/tesseract_urdf/safety_controller.h +++ b/tesseract_urdf/include/tesseract_urdf/safety_controller.h @@ -41,14 +41,15 @@ class XMLDocument; namespace tesseract_urdf { +static const char* SAFETY_CONTROLLER_ELEMENT_NAME = "safety_controller"; + /** * @brief Parse xml element safety_controller * @param xml_element The xml element * @param version The version number * @return A Tesseract JointSafety */ -std::shared_ptr parseSafetyController(const tinyxml2::XMLElement* xml_element, - int version); +std::shared_ptr parseSafetyController(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeSafetyController(const std::shared_ptr& safety, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h index 5111bd43e81..caa4899f723 100644 --- a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h @@ -42,6 +42,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* SDF_MESH_ELEMENT_NAME = "tesseract:sdf_mesh"; + /** * @brief Parse xml element sdf_mesh * @param xml_element The xml element @@ -52,8 +54,7 @@ namespace tesseract_urdf */ std::vector> parseSDFMesh(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - bool visual, - int version); + bool visual); /** * @brief writeSDFMesh Write SDF Mesh to URDF XML. This is non-standard URDF / tesseract-exclusive diff --git a/tesseract_urdf/include/tesseract_urdf/sphere.h b/tesseract_urdf/include/tesseract_urdf/sphere.h index 46f6c7d0e08..d85992bfcaa 100644 --- a/tesseract_urdf/include/tesseract_urdf/sphere.h +++ b/tesseract_urdf/include/tesseract_urdf/sphere.h @@ -41,12 +41,14 @@ class XMLDocument; namespace tesseract_urdf { +static const char* SPHERE_ELEMENT_NAME = "sphere"; + /** * @brief Parse a xml sphere element * @param xml_element The xml element * @return Tesseract Geometry Sphere */ -std::shared_ptr parseSphere(const tinyxml2::XMLElement* xml_element, int version); +std::shared_ptr parseSphere(const tinyxml2::XMLElement* xml_element); tinyxml2::XMLElement* writeSphere(const std::shared_ptr& sphere, tinyxml2::XMLDocument& doc); diff --git a/tesseract_urdf/include/tesseract_urdf/visual.h b/tesseract_urdf/include/tesseract_urdf/visual.h index d599d2d2ccf..b1ea80721d4 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -44,6 +44,8 @@ class XMLDocument; namespace tesseract_urdf { +static const char* VISUAL_ELEMENT_NAME = "visual"; + /** * @brief Parse xml element visual * @param xml_element The xml element @@ -54,8 +56,7 @@ namespace tesseract_urdf std::shared_ptr parseVisual(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - std::unordered_map>& available_materials, - int version); + std::unordered_map>& available_materials); /** * @brief writeVisual Write one visual geometry object to URDF XML diff --git a/tesseract_urdf/src/box.cpp b/tesseract_urdf/src/box.cpp index 90c1190cd45..c352ab6aff2 100644 --- a/tesseract_urdf/src/box.cpp +++ b/tesseract_urdf/src/box.cpp @@ -36,7 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_geometry::Box::Ptr tesseract_urdf::parseBox(const tinyxml2::XMLElement* xml_element, int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Box::Ptr parseBox(const tinyxml2::XMLElement* xml_element) { std::string size_string; if (tesseract_common::QueryStringAttribute(xml_element, "size", size_string) != tinyxml2::XML_SUCCESS) @@ -65,15 +67,16 @@ tesseract_geometry::Box::Ptr tesseract_urdf::parseBox(const tinyxml2::XMLElement return std::make_shared(l, w, h); } -tinyxml2::XMLElement* tesseract_urdf::writeBox(const std::shared_ptr& box, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeBox(const std::shared_ptr& box, tinyxml2::XMLDocument& doc) { if (box == nullptr) std::throw_with_nested(std::runtime_error("Box is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("box"); + tinyxml2::XMLElement* xml_element = doc.NewElement(BOX_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); std::stringstream size_string; size_string << Eigen::Vector3d(box->getX(), box->getY(), box->getZ()).format(eigen_format); xml_element->SetAttribute("size", size_string.str().c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/calibration.cpp b/tesseract_urdf/src/calibration.cpp index b0cf72e64cc..a5ea6d28d5a 100644 --- a/tesseract_urdf/src/calibration.cpp +++ b/tesseract_urdf/src/calibration.cpp @@ -36,8 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_scene_graph::JointCalibration::Ptr tesseract_urdf::parseCalibration(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::JointCalibration::Ptr parseCalibration(const tinyxml2::XMLElement* xml_element) { if (xml_element->Attribute("rising") == nullptr && xml_element->Attribute("falling") == nullptr) std::throw_with_nested(std::runtime_error("Calibration: Missing both attribute 'rising' and 'falling', either " @@ -62,13 +63,15 @@ tesseract_scene_graph::JointCalibration::Ptr tesseract_urdf::parseCalibration(co } tinyxml2::XMLElement* -tesseract_urdf::writeCalibration(const std::shared_ptr& calibration, - tinyxml2::XMLDocument& doc) +writeCalibration(const std::shared_ptr& calibration, + tinyxml2::XMLDocument& doc) { if (calibration == nullptr) std::throw_with_nested(std::runtime_error("Calibration is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("calibration"); + tinyxml2::XMLElement* xml_element = doc.NewElement(CALIBRATION_ELEMENT_NAME); xml_element->SetAttribute("rising", toString(calibration->rising).c_str()); xml_element->SetAttribute("falling", toString(calibration->falling).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/capsule.cpp b/tesseract_urdf/src/capsule.cpp index cce04d895d8..75bf1bab94d 100644 --- a/tesseract_urdf/src/capsule.cpp +++ b/tesseract_urdf/src/capsule.cpp @@ -35,7 +35,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_geometry::Capsule::Ptr tesseract_urdf::parseCapsule(const tinyxml2::XMLElement* xml_element, int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Capsule::Ptr parseCapsule(const tinyxml2::XMLElement* xml_element) { double r{ 0 }, l{ 0 }; if (xml_element->QueryDoubleAttribute("length", &(l)) != tinyxml2::XML_SUCCESS || !(l > 0)) @@ -47,13 +49,15 @@ tesseract_geometry::Capsule::Ptr tesseract_urdf::parseCapsule(const tinyxml2::XM return std::make_shared(r, l); } -tinyxml2::XMLElement* tesseract_urdf::writeCapsule(const std::shared_ptr& capsule, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeCapsule(const std::shared_ptr& capsule, + tinyxml2::XMLDocument& doc) { if (capsule == nullptr) std::throw_with_nested(std::runtime_error("Capsule is nullptr and cannot be written to XML file")); - tinyxml2::XMLElement* xml_element = doc.NewElement("capsule"); + tinyxml2::XMLElement* xml_element = doc.NewElement(CAPSULE_ELEMENT_NAME); xml_element->SetAttribute("length", toString(capsule->getLength()).c_str()); xml_element->SetAttribute("radius", toString(capsule->getRadius()).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/collision.cpp b/tesseract_urdf/src/collision.cpp index 2cca47d0690..ecea7b0b4ae 100644 --- a/tesseract_urdf/src/collision.cpp +++ b/tesseract_urdf/src/collision.cpp @@ -40,9 +40,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - int version) +namespace tesseract_urdf +{ +tesseract_scene_graph::Collision::Ptr parseCollision(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator) { // get name std::string collision_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -54,7 +55,7 @@ tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyx { try { - collision_origin = parseOrigin(origin, version); + collision_origin = parseOrigin(origin); } catch (...) { @@ -70,7 +71,7 @@ tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyx tesseract_geometry::Geometry::Ptr geom; try { - geom = parseGeometry(geometry, locator, false, version); + geom = parseGeometry(geometry, locator, false); } catch (...) { @@ -85,17 +86,16 @@ tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyx return collision; } -tinyxml2::XMLElement* -tesseract_urdf::writeCollision(const std::shared_ptr& collision, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& link_name, - const int id = -1) +tinyxml2::XMLElement* writeCollision(const std::shared_ptr& collision, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& link_name, + const int id = -1) { if (collision == nullptr) std::throw_with_nested(std::runtime_error("Collision is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("collision"); + tinyxml2::XMLElement* xml_element = doc.NewElement(COLLISION_ELEMENT_NAME); if (!collision->name.empty()) xml_element->SetAttribute("name", collision->name.c_str()); @@ -133,3 +133,5 @@ tesseract_urdf::writeCollision(const std::shared_ptr #include -tesseract_geometry::Cone::Ptr tesseract_urdf::parseCone(const tinyxml2::XMLElement* xml_element, int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Cone::Ptr parseCone(const tinyxml2::XMLElement* xml_element) { double r{ 0 }, l{ 0 }; if (xml_element->QueryDoubleAttribute("length", &(l)) != tinyxml2::XML_SUCCESS || !(l > 0)) @@ -47,13 +49,14 @@ tesseract_geometry::Cone::Ptr tesseract_urdf::parseCone(const tinyxml2::XMLEleme return std::make_shared(r, l); } -tinyxml2::XMLElement* tesseract_urdf::writeCone(const std::shared_ptr& cone, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeCone(const std::shared_ptr& cone, tinyxml2::XMLDocument& doc) { if (cone == nullptr) std::throw_with_nested(std::runtime_error("Cone is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("cone"); + tinyxml2::XMLElement* xml_element = doc.NewElement(CONE_ELEMENT_NAME); xml_element->SetAttribute("length", toString(cone->getLength()).c_str()); xml_element->SetAttribute("radius", toString(cone->getRadius()).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/convex_mesh.cpp b/tesseract_urdf/src/convex_mesh.cpp index bba577d008e..c6487b31772 100644 --- a/tesseract_urdf/src/convex_mesh.cpp +++ b/tesseract_urdf/src/convex_mesh.cpp @@ -40,11 +40,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -std::vector -tesseract_urdf::parseConvexMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int /*version*/) +namespace tesseract_urdf +{ +std::vector parseConvexMesh(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual) { std::vector meshes; @@ -112,14 +112,14 @@ tesseract_urdf::parseConvexMesh(const tinyxml2::XMLElement* xml_element, return meshes; } -tinyxml2::XMLElement* tesseract_urdf::writeConvexMesh(const std::shared_ptr& mesh, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeConvexMesh(const std::shared_ptr& mesh, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (mesh == nullptr) std::throw_with_nested(std::runtime_error("Mesh is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("convex_mesh"); + tinyxml2::XMLElement* xml_element = doc.NewElement(CONVEX_MESH_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); try @@ -146,3 +146,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeConvexMesh(const std::shared_ptr #include -tesseract_geometry::Cylinder::Ptr tesseract_urdf::parseCylinder(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Cylinder::Ptr parseCylinder(const tinyxml2::XMLElement* xml_element) { double r{ 0 }, l{ 0 }; if (xml_element->QueryDoubleAttribute("length", &(l)) != tinyxml2::XML_SUCCESS || !(l > 0)) @@ -48,13 +49,15 @@ tesseract_geometry::Cylinder::Ptr tesseract_urdf::parseCylinder(const tinyxml2:: return std::make_shared(r, l); } -tinyxml2::XMLElement* tesseract_urdf::writeCylinder(const std::shared_ptr& cylinder, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeCylinder(const std::shared_ptr& cylinder, + tinyxml2::XMLDocument& doc) { if (cylinder == nullptr) std::throw_with_nested(std::runtime_error("Cylinder is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("cylinder"); + tinyxml2::XMLElement* xml_element = doc.NewElement(CYLINDER_ELEMENT_NAME); xml_element->SetAttribute("length", toString(cylinder->getLength()).c_str()); xml_element->SetAttribute("radius", toString(cylinder->getRadius()).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/dynamics.cpp b/tesseract_urdf/src/dynamics.cpp index 4f419855c7e..e28d214c656 100644 --- a/tesseract_urdf/src/dynamics.cpp +++ b/tesseract_urdf/src/dynamics.cpp @@ -36,8 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_scene_graph::JointDynamics::Ptr tesseract_urdf::parseDynamics(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::JointDynamics::Ptr parseDynamics(const tinyxml2::XMLElement* xml_element) { if (xml_element->Attribute("damping") == nullptr && xml_element->Attribute("friction") == nullptr) std::throw_with_nested(std::runtime_error("Dynamics: Missing both attributes 'damping' and 'friction', remove tag " @@ -62,16 +63,17 @@ tesseract_scene_graph::JointDynamics::Ptr tesseract_urdf::parseDynamics(const ti return dynamics; } -tinyxml2::XMLElement* -tesseract_urdf::writeDynamics(const std::shared_ptr& dynamics, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeDynamics(const std::shared_ptr& dynamics, + tinyxml2::XMLDocument& doc) { if (dynamics == nullptr) std::throw_with_nested(std::runtime_error("Dynamics is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("dynamics"); + tinyxml2::XMLElement* xml_element = doc.NewElement(DYNAMICS_ELEMENT_NAME); xml_element->SetAttribute("damping", toString(dynamics->damping).c_str()); xml_element->SetAttribute("friction", toString(dynamics->damping).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/geometry.cpp b/tesseract_urdf/src/geometry.cpp index 7a82b5b9365..f71df8dc918 100644 --- a/tesseract_urdf/src/geometry.cpp +++ b/tesseract_urdf/src/geometry.cpp @@ -47,10 +47,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version) +namespace tesseract_urdf +{ +tesseract_geometry::Geometry::Ptr parseGeometry(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual) { const tinyxml2::XMLElement* geometry = xml_element->FirstChildElement(); if (geometry == nullptr) @@ -61,12 +62,13 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Geometry: Error parsing 'geometry' element, invalid geometry type!")); - if (geometry_type == "sphere") + // URDF-supported elements + if (geometry_type == SPHERE_ELEMENT_NAME) { tesseract_geometry::Sphere::Ptr sphere; try { - sphere = parseSphere(geometry, version); + sphere = parseSphere(geometry); } catch (...) { @@ -76,12 +78,12 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: return sphere; } - if (geometry_type == "box") + if (geometry_type == BOX_ELEMENT_NAME) { tesseract_geometry::Box::Ptr box; try { - box = parseBox(geometry, version); + box = parseBox(geometry); } catch (...) { @@ -91,12 +93,12 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: return box; } - if (geometry_type == "cylinder") + if (geometry_type == CYLINDER_ELEMENT_NAME) { tesseract_geometry::Cylinder::Ptr cylinder; try { - cylinder = parseCylinder(geometry, version); + cylinder = parseCylinder(geometry); } catch (...) { @@ -106,93 +108,76 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: return cylinder; } - if (geometry_type == "cone") + if (geometry_type == MESH_ELEMENT_NAME) { - tesseract_geometry::Cone::Ptr cone; + std::vector meshes; try { - cone = parseCone(geometry, version); + meshes = parseMesh(geometry, locator, visual); } catch (...) { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'cone'!")); + std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'mesh'!")); } - return cone; + if (meshes.size() > 1) + return std::make_shared(meshes); + + return meshes.front(); } - if (geometry_type == "capsule") + // Custom Tesseract elements + if (geometry_type == CONE_ELEMENT_NAME) { - tesseract_geometry::Capsule::Ptr capsule; + tesseract_geometry::Cone::Ptr cone; try { - capsule = parseCapsule(geometry, version); + cone = parseCone(geometry); } catch (...) { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'capsule'!")); + std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'cone'!")); } - return capsule; + return cone; } - if (geometry_type == "octomap") + if (geometry_type == CAPSULE_ELEMENT_NAME) { - tesseract_geometry::Octree::Ptr octree; + tesseract_geometry::Capsule::Ptr capsule; try { - octree = parseOctomap(geometry, locator, visual, version); + capsule = parseCapsule(geometry); } catch (...) { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'octomap'!")); + std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'capsule'!")); } - return octree; + return capsule; } - if (geometry_type == "mesh") + if (geometry_type == OCTOMAP_ELEMENT_NAME) { - std::vector meshes; + tesseract_geometry::Octree::Ptr octree; try { - meshes = parseMesh(geometry, locator, visual, version); + octree = parseOctomap(geometry, locator, visual); } catch (...) { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'mesh'!")); - } - - if (meshes.size() > 1) - { - std::vector> poly_meshes; - poly_meshes.reserve(meshes.size()); - - if (version < 2 && !visual) - { - for (const auto& mesh : meshes) - poly_meshes.push_back(tesseract_collision::makeConvexMesh(*mesh)); - } - else - { - for (const auto& mesh : meshes) - poly_meshes.push_back(mesh); - } - return std::make_shared(poly_meshes); + std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'octomap'!")); } - if (version < 2 && !visual) - return tesseract_collision::makeConvexMesh(*meshes.front()); - - return meshes.front(); + return octree; } - if (geometry_type == "convex_mesh") + if (geometry_type == CONVEX_MESH_ELEMENT_NAME) { std::vector meshes; try { - meshes = parseConvexMesh(geometry, locator, visual, version); + meshes = parseConvexMesh(geometry, locator, visual); } catch (...) { @@ -205,12 +190,12 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: return meshes.front(); } - if (geometry_type == "sdf_mesh") + if (geometry_type == SDF_MESH_ELEMENT_NAME) { std::vector meshes; try { - meshes = parseSDFMesh(geometry, locator, visual, version); + meshes = parseSDFMesh(geometry, locator, visual); } catch (...) { @@ -226,14 +211,14 @@ tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2:: std::throw_with_nested(std::runtime_error("Geometry: Invalid geometry type '" + geometry_type + "'!")); } -tinyxml2::XMLElement* tesseract_urdf::writeGeometry(const std::shared_ptr& geometry, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeGeometry(const std::shared_ptr& geometry, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (geometry == nullptr) std::throw_with_nested(std::runtime_error("Geometry is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("geometry"); + tinyxml2::XMLElement* xml_element = doc.NewElement(GEOMETRY_ELEMENT_NAME); tesseract_geometry::GeometryType type = geometry->getType(); @@ -367,3 +352,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeGeometry(const std::shared_ptr #include -tesseract_scene_graph::Inertial::Ptr tesseract_urdf::parseInertial(const tinyxml2::XMLElement* xml_element, int version) +namespace tesseract_urdf +{ +tesseract_scene_graph::Inertial::Ptr parseInertial(const tinyxml2::XMLElement* xml_element) { auto inertial = std::make_shared(); const tinyxml2::XMLElement* origin = xml_element->FirstChildElement("origin"); @@ -44,7 +46,7 @@ tesseract_scene_graph::Inertial::Ptr tesseract_urdf::parseInertial(const tinyxml { try { - inertial->origin = parseOrigin(origin, version); + inertial->origin = parseOrigin(origin); } catch (...) { @@ -84,13 +86,12 @@ tesseract_scene_graph::Inertial::Ptr tesseract_urdf::parseInertial(const tinyxml return inertial; } -tinyxml2::XMLElement* -tesseract_urdf::writeInertial(const std::shared_ptr& inertial, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeInertial(const std::shared_ptr& inertial, + tinyxml2::XMLDocument& doc) { if (inertial == nullptr) std::throw_with_nested(std::runtime_error("Inertial is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("inertial"); + tinyxml2::XMLElement* xml_element = doc.NewElement(INERTIAL_ELEMENT_NAME); if (!inertial->origin.matrix().isIdentity(std::numeric_limits::epsilon())) { @@ -113,3 +114,5 @@ tesseract_urdf::writeInertial(const std::shared_ptrInsertEndChild(xml_inertia); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/joint.cpp b/tesseract_urdf/src/joint.cpp index 2e01f389b74..bae0145b1cf 100644 --- a/tesseract_urdf/src/joint.cpp +++ b/tesseract_urdf/src/joint.cpp @@ -43,7 +43,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XMLElement* xml_element, int version) +namespace tesseract_urdf +{ +tesseract_scene_graph::Joint::Ptr parseJoint(const tinyxml2::XMLElement* xml_element) { // get joint name std::string joint_name; @@ -59,7 +61,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->parent_to_joint_origin_transform = parseOrigin(origin, version); + j->parent_to_joint_origin_transform = parseOrigin(origin); } catch (...) { @@ -155,7 +157,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->limits = parseLimits(limits, version); + j->limits = parseLimits(limits); } catch (...) { @@ -171,7 +173,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->safety = parseSafetyController(safety, version); + j->safety = parseSafetyController(safety); } catch (...) { @@ -186,7 +188,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->calibration = parseCalibration(calibration, version); + j->calibration = parseCalibration(calibration); } catch (...) { @@ -201,7 +203,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->mimic = parseMimic(mimic, version); + j->mimic = parseMimic(mimic); } catch (...) { @@ -216,7 +218,7 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML { try { - j->dynamics = parseDynamics(dynamics, version); + j->dynamics = parseDynamics(dynamics); } catch (...) { @@ -228,12 +230,12 @@ tesseract_scene_graph::Joint::Ptr tesseract_urdf::parseJoint(const tinyxml2::XML return j; } -tinyxml2::XMLElement* tesseract_urdf::writeJoint(const std::shared_ptr& joint, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeJoint(const std::shared_ptr& joint, + tinyxml2::XMLDocument& doc) { if (joint == nullptr) std::throw_with_nested(std::runtime_error("Joint is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("joint"); + tinyxml2::XMLElement* xml_element = doc.NewElement(JOINT_ELEMENT_NAME); // Set the joint name xml_element->SetAttribute("name", joint->getName().c_str()); @@ -340,3 +342,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeJoint(const std::shared_ptr #include -tesseract_scene_graph::JointLimits::Ptr tesseract_urdf::parseLimits(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::JointLimits::Ptr parseLimits(const tinyxml2::XMLElement* xml_element) { auto limits = std::make_shared(); @@ -70,13 +71,12 @@ tesseract_scene_graph::JointLimits::Ptr tesseract_urdf::parseLimits(const tinyxm return limits; } -tinyxml2::XMLElement* -tesseract_urdf::writeLimits(const std::shared_ptr& limits, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeLimits(const std::shared_ptr& limits, + tinyxml2::XMLDocument& doc) { if (limits == nullptr) std::throw_with_nested(std::runtime_error("Limits are nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("limit"); + tinyxml2::XMLElement* xml_element = doc.NewElement(LIMITS_ELEMENT_NAME); // if upper and lower are both zero, don't write it. This should only happen for continuous joints. if (!tesseract_common::almostEqualRelativeAndAbs(limits->lower, 0.0) || @@ -102,3 +102,5 @@ tesseract_urdf::writeLimits(const std::shared_ptr #include +namespace tesseract_urdf +{ tesseract_scene_graph::Link::Ptr -tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - std::unordered_map& available_materials, - int version) +parseLink(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + std::unordered_map& available_materials) { std::string link_name; if (tesseract_common::QueryStringAttribute(xml_element, "name", link_name) != tinyxml2::XML_SUCCESS) @@ -59,7 +60,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, { try { - l->inertial = parseInertial(inertial, version); + l->inertial = parseInertial(inertial); } catch (...) { @@ -75,7 +76,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, tesseract_scene_graph::Visual::Ptr temp_visual; try { - temp_visual = parseVisual(visual, locator, available_materials, version); + temp_visual = parseVisual(visual, locator, available_materials); } catch (...) { @@ -92,7 +93,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, tesseract_scene_graph::Collision::Ptr temp_collision; try { - temp_collision = parseCollision(collision, locator, version); + temp_collision = parseCollision(collision, locator); } catch (...) { @@ -106,13 +107,13 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, return l; } -tinyxml2::XMLElement* tesseract_urdf::writeLink(const std::shared_ptr& link, - tinyxml2::XMLDocument& doc, - const std::string& package_path) +tinyxml2::XMLElement* writeLink(const std::shared_ptr& link, + tinyxml2::XMLDocument& doc, + const std::string& package_path) { if (link == nullptr) std::throw_with_nested(std::runtime_error("Link is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("link"); + tinyxml2::XMLElement* xml_element = doc.NewElement(LINK_ELEMENT_NAME); // Set name xml_element->SetAttribute("name", link->getName().c_str()); @@ -163,3 +164,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeLink(const std::shared_ptr #include -tesseract_scene_graph::Material::Ptr tesseract_urdf::parseMaterial( - const tinyxml2::XMLElement* xml_element, - std::unordered_map& available_materials, - bool allow_anonymous, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::Material::Ptr +parseMaterial(const tinyxml2::XMLElement* xml_element, + std::unordered_map& available_materials, + bool allow_anonymous) { std::string material_name; if (tesseract_common::QueryStringAttribute(xml_element, "name", material_name) != tinyxml2::XML_SUCCESS) @@ -120,13 +121,12 @@ tesseract_scene_graph::Material::Ptr tesseract_urdf::parseMaterial( return m; } -tinyxml2::XMLElement* -tesseract_urdf::writeMaterial(const std::shared_ptr& material, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeMaterial(const std::shared_ptr& material, + tinyxml2::XMLDocument& doc) { if (material == nullptr) std::throw_with_nested(std::runtime_error("Material is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("material"); + tinyxml2::XMLElement* xml_element = doc.NewElement(MATERIAL_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); xml_element->SetAttribute("name", material->getName().c_str()); @@ -146,3 +146,5 @@ tesseract_urdf::writeMaterial(const std::shared_ptr #include -std::vector tesseract_urdf::parseMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int /*version*/) +namespace tesseract_urdf +{ +std::vector parseMesh(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual) { std::vector meshes; @@ -92,14 +93,14 @@ std::vector tesseract_urdf::parseMesh(const tinyx return meshes; } -tinyxml2::XMLElement* tesseract_urdf::writeMesh(const std::shared_ptr& mesh, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeMesh(const std::shared_ptr& mesh, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (mesh == nullptr) std::throw_with_nested(std::runtime_error("Mesh is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("mesh"); + tinyxml2::XMLElement* xml_element = doc.NewElement(MESH_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); try @@ -121,3 +122,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeMesh(const std::shared_ptr #include -tesseract_scene_graph::JointMimic::Ptr tesseract_urdf::parseMimic(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::JointMimic::Ptr parseMimic(const tinyxml2::XMLElement* xml_element) { auto m = std::make_shared(); if (tesseract_common::QueryStringAttribute(xml_element, "joint", m->joint_name) != tinyxml2::XML_SUCCESS) @@ -62,12 +63,12 @@ tesseract_scene_graph::JointMimic::Ptr tesseract_urdf::parseMimic(const tinyxml2 return m; } -tinyxml2::XMLElement* tesseract_urdf::writeMimic(const std::shared_ptr& mimic, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeMimic(const std::shared_ptr& mimic, + tinyxml2::XMLDocument& doc) { if (mimic == nullptr) std::throw_with_nested(std::runtime_error("Mimic Joint is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("mimic"); + tinyxml2::XMLElement* xml_element = doc.NewElement(MIMIC_ELEMENT_NAME); xml_element->SetAttribute("joint", mimic->joint_name.c_str()); xml_element->SetAttribute("offset", toString(mimic->offset).c_str()); @@ -75,3 +76,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeMimic(const std::shared_ptr #endif -tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctomap(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - const bool /*visual*/, - int version) +namespace tesseract_urdf +{ +tesseract_geometry::Octree::Ptr parseOctomap(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + const bool /*visual*/) { std::string shape_type; if (tesseract_common::QueryStringAttribute(xml_element, "shape_type", shape_type) != tinyxml2::XML_SUCCESS) @@ -65,45 +66,46 @@ tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctomap(const tinyxml2::XML bool prune = false; xml_element->QueryBoolAttribute("prune", &prune); - const tinyxml2::XMLElement* octree_element = xml_element->FirstChildElement("octree"); + const tinyxml2::XMLElement* octree_element = xml_element->FirstChildElement(OCTREE_ELEMENT_NAME); if (octree_element != nullptr) { try { - return parseOctree(octree_element, locator, sub_type, prune, version); + return parseOctree(octree_element, locator, sub_type, prune); } catch (...) { - std::throw_with_nested(std::runtime_error("Octomap: Failed parsing element 'octree'")); + std::throw_with_nested(std::runtime_error("Octomap:")); } } #ifdef TESSERACT_PARSE_POINT_CLOUDS - const tinyxml2::XMLElement* pcd_element = xml_element->FirstChildElement("point_cloud"); + const tinyxml2::XMLElement* pcd_element = xml_element->FirstChildElement(POINT_CLOUD_ELEMENT_NAME); if (pcd_element != nullptr) { try { - return parsePointCloud(pcd_element, locator, sub_type, prune, version); + return parsePointCloud(pcd_element, locator, sub_type, prune); } catch (...) { - std::throw_with_nested(std::runtime_error("Octomap: Failed parsing element 'pointcloud'")); + std::throw_with_nested(std::runtime_error("Octomap:")); } } #endif - std::throw_with_nested(std::runtime_error("Octomap: Missing element 'octree' or 'point_cloud', must define one!")); + std::throw_with_nested(std::runtime_error("Octomap: Missing element '" + std::string(OCTREE_ELEMENT_NAME) + "' or '" + + std::string(POINT_CLOUD_ELEMENT_NAME) + "'; must define one!")); } -tinyxml2::XMLElement* tesseract_urdf::writeOctomap(const std::shared_ptr& octree, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeOctomap(const std::shared_ptr& octree, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (octree == nullptr) std::throw_with_nested(std::runtime_error("Octree is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("octree"); + tinyxml2::XMLElement* xml_element = doc.NewElement(OCTOMAP_ELEMENT_NAME); std::string type_string; if (octree->getSubType() == tesseract_geometry::OctreeSubType::BOX) @@ -130,3 +132,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeOctomap(const std::shared_ptr #include -tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctree(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - tesseract_geometry::OctreeSubType shape_type, - bool prune, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Octree::Ptr parseOctree(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + tesseract_geometry::OctreeSubType shape_type, + bool prune) { std::string filename; if (tesseract_common::QueryStringAttribute(xml_element, "filename", filename) != tinyxml2::XML_SUCCESS) @@ -69,14 +70,14 @@ tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctree(const tinyxml2::XMLE return geom; } -tinyxml2::XMLElement* tesseract_urdf::writeOctree(const tesseract_geometry::Octree::ConstPtr& octree, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeOctree(const tesseract_geometry::Octree::ConstPtr& octree, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (octree == nullptr) std::throw_with_nested(std::runtime_error("Octree is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("octree"); + tinyxml2::XMLElement* xml_element = doc.NewElement(OCTREE_ELEMENT_NAME); std::string filepath = trailingSlash(package_path) + noLeadingSlash(filename); @@ -91,3 +92,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeOctree(const tesseract_geometry::Octr return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/origin.cpp b/tesseract_urdf/src/origin.cpp index 88c13c24377..1fd972ad195 100644 --- a/tesseract_urdf/src/origin.cpp +++ b/tesseract_urdf/src/origin.cpp @@ -37,7 +37,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_element, int /*version*/) +namespace tesseract_urdf +{ +Eigen::Isometry3d parseOrigin(const tinyxml2::XMLElement* xml_element) { Eigen::Isometry3d origin = Eigen::Isometry3d::Identity(); @@ -124,9 +126,9 @@ Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_el return origin; } -tinyxml2::XMLElement* tesseract_urdf::writeOrigin(const Eigen::Isometry3d& origin, tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeOrigin(const Eigen::Isometry3d& origin, tinyxml2::XMLDocument& doc) { - tinyxml2::XMLElement* xml_element = doc.NewElement("origin"); + tinyxml2::XMLElement* xml_element = doc.NewElement(ORIGIN_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); // Format and write the translation @@ -157,3 +159,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeOrigin(const Eigen::Isometry3d& origi return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/point_cloud.cpp b/tesseract_urdf/src/point_cloud.cpp index 408ff256368..17193ec7cbe 100644 --- a/tesseract_urdf/src/point_cloud.cpp +++ b/tesseract_urdf/src/point_cloud.cpp @@ -40,11 +40,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_geometry::Octree::Ptr tesseract_urdf::parsePointCloud(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - tesseract_geometry::OctreeSubType shape_type, - bool prune, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Octree::Ptr parsePointCloud(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + tesseract_geometry::OctreeSubType shape_type, + bool prune) { std::string filename; if (tesseract_common::QueryStringAttribute(xml_element, "filename", filename) != tinyxml2::XML_SUCCESS) @@ -79,3 +80,5 @@ tesseract_geometry::Octree::Ptr tesseract_urdf::parsePointCloud(const tinyxml2:: return geom; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/safety_controller.cpp b/tesseract_urdf/src/safety_controller.cpp index 01e170f1073..de8fde7dfcc 100644 --- a/tesseract_urdf/src/safety_controller.cpp +++ b/tesseract_urdf/src/safety_controller.cpp @@ -36,8 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_scene_graph::JointSafety::Ptr tesseract_urdf::parseSafetyController(const tinyxml2::XMLElement* xml_element, - int /*version*/) +namespace tesseract_urdf +{ +tesseract_scene_graph::JointSafety::Ptr parseSafetyController(const tinyxml2::XMLElement* xml_element) { auto s = std::make_shared(); if (xml_element->QueryDoubleAttribute("k_velocity", &(s->k_velocity)) != tinyxml2::XML_SUCCESS) @@ -72,13 +73,12 @@ tesseract_scene_graph::JointSafety::Ptr tesseract_urdf::parseSafetyController(co return s; } -tinyxml2::XMLElement* -tesseract_urdf::writeSafetyController(const std::shared_ptr& safety, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeSafetyController(const std::shared_ptr& safety, + tinyxml2::XMLDocument& doc) { if (safety == nullptr) std::throw_with_nested(std::runtime_error("Safety Controller is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("safety"); + tinyxml2::XMLElement* xml_element = doc.NewElement(SAFETY_CONTROLLER_ELEMENT_NAME); xml_element->SetAttribute("k_velocity", toString(safety->k_velocity).c_str()); @@ -89,3 +89,5 @@ tesseract_urdf::writeSafetyController(const std::shared_ptr #include -std::vector -tesseract_urdf::parseSDFMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int /*version*/) +namespace tesseract_urdf +{ +std::vector parseSDFMesh(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual) { std::vector meshes; @@ -93,14 +93,14 @@ tesseract_urdf::parseSDFMesh(const tinyxml2::XMLElement* xml_element, return meshes; } -tinyxml2::XMLElement* tesseract_urdf::writeSDFMesh(const std::shared_ptr& sdf_mesh, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename) +tinyxml2::XMLElement* writeSDFMesh(const std::shared_ptr& sdf_mesh, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& filename) { if (sdf_mesh == nullptr) std::throw_with_nested(std::runtime_error("SDF Mesh is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("sdf_mesh"); + tinyxml2::XMLElement* xml_element = doc.NewElement(SDF_MESH_ELEMENT_NAME); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); try @@ -121,3 +121,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeSDFMesh(const std::shared_ptr #include -tesseract_geometry::Sphere::Ptr tesseract_urdf::parseSphere(const tinyxml2::XMLElement* xml_element, int /*version*/) +namespace tesseract_urdf +{ +tesseract_geometry::Sphere::Ptr parseSphere(const tinyxml2::XMLElement* xml_element) { double radius{ 0 }; if (xml_element->QueryDoubleAttribute("radius", &(radius)) != tinyxml2::XML_SUCCESS || !(radius > 0)) @@ -44,14 +46,16 @@ tesseract_geometry::Sphere::Ptr tesseract_urdf::parseSphere(const tinyxml2::XMLE return std::make_shared(radius); } -tinyxml2::XMLElement* tesseract_urdf::writeSphere(const std::shared_ptr& sphere, - tinyxml2::XMLDocument& doc) +tinyxml2::XMLElement* writeSphere(const std::shared_ptr& sphere, + tinyxml2::XMLDocument& doc) { if (sphere == nullptr) std::throw_with_nested(std::runtime_error("Sphere is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("sphere"); + tinyxml2::XMLElement* xml_element = doc.NewElement(SPHERE_ELEMENT_NAME); xml_element->SetAttribute("radius", toString(sphere->getRadius()).c_str()); return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/urdf_parser.cpp b/tesseract_urdf/src/urdf_parser.cpp index 587e0a51cd9..eba7a04e5aa 100644 --- a/tesseract_urdf/src/urdf_parser.cpp +++ b/tesseract_urdf/src/urdf_parser.cpp @@ -45,6 +45,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +static const char* ROBOT_ELEMENT_NAME = "robot"; + namespace tesseract_urdf { std::unique_ptr parseURDFString(const std::string& urdf_xml_string, @@ -54,7 +56,7 @@ std::unique_ptr parseURDFString(const std::st if (xml_doc.Parse(urdf_xml_string.c_str()) != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("URDF: Failed to parse urdf string!")); - tinyxml2::XMLElement* robot = xml_doc.FirstChildElement("robot"); + tinyxml2::XMLElement* robot = xml_doc.FirstChildElement(ROBOT_ELEMENT_NAME); if (robot == nullptr) std::throw_with_nested(std::runtime_error("URDF: Missing element 'robot'!")); @@ -69,29 +71,22 @@ std::unique_ptr parseURDFString(const std::st std::runtime_error("URDF: Failed parsing attribute 'version' for robot '" + robot_name + "'!")); if (urdf_version != 1) - std::throw_with_nested( - std::runtime_error("URDF: 'version' for robot '" + robot_name + "' is set to `" + std::to_string(urdf_version) + - "', this is not supported, please set it to 1.0. If you want to use a different tesseract " - "URDF parsing version use `tesseract_version=\"2\"`")); - - int tesseract_urdf_version = 1; - auto tesseract_version_status = robot->QueryIntAttribute("tesseract_version", &tesseract_urdf_version); - if (tesseract_version_status != tinyxml2::XML_NO_ATTRIBUTE && tesseract_version_status != tinyxml2::XML_SUCCESS) - std::throw_with_nested( - std::runtime_error("URDF: Failed parsing attribute 'tesseract_version' for robot '" + robot_name + "'!")); + std::throw_with_nested(std::runtime_error("URDF: 'version' for robot '" + robot_name + "' is set to `" + + std::to_string(urdf_version) + + "', this is not supported, please set it to 1.0.")); auto sg = std::make_unique(); sg->setName(robot_name); std::unordered_map available_materials; - for (tinyxml2::XMLElement* material = robot->FirstChildElement("material"); material != nullptr; - material = material->NextSiblingElement("material")) + for (tinyxml2::XMLElement* material = robot->FirstChildElement(MATERIAL_ELEMENT_NAME); material != nullptr; + material = material->NextSiblingElement(MATERIAL_ELEMENT_NAME)) { tesseract_scene_graph::Material::Ptr m = nullptr; std::unordered_map empty_material; try { - m = parseMaterial(material, empty_material, true, tesseract_urdf_version); + m = parseMaterial(material, empty_material, true); } catch (...) { @@ -102,13 +97,13 @@ std::unique_ptr parseURDFString(const std::st available_materials[m->getName()] = m; } - for (tinyxml2::XMLElement* link = robot->FirstChildElement("link"); link != nullptr; - link = link->NextSiblingElement("link")) + for (tinyxml2::XMLElement* link = robot->FirstChildElement(LINK_ELEMENT_NAME); link != nullptr; + link = link->NextSiblingElement(LINK_ELEMENT_NAME)) { tesseract_scene_graph::Link::Ptr l = nullptr; try { - l = parseLink(link, locator, available_materials, tesseract_urdf_version); + l = parseLink(link, locator, available_materials); } catch (...) { @@ -129,14 +124,13 @@ std::unique_ptr parseURDFString(const std::st if (sg->getLinks().empty()) std::throw_with_nested(std::runtime_error("URDF: Error no links were found for robot '" + robot_name + "'!")); - for (tinyxml2::XMLElement* joint = robot->FirstChildElement("joint"); joint != nullptr; - joint = joint->NextSiblingElement("join" - "t")) + for (tinyxml2::XMLElement* joint = robot->FirstChildElement(JOINT_ELEMENT_NAME); joint != nullptr; + joint = joint->NextSiblingElement(JOINT_ELEMENT_NAME)) { tesseract_scene_graph::Joint::Ptr j = nullptr; try { - j = parseJoint(joint, tesseract_urdf_version); + j = parseJoint(joint); } catch (...) { @@ -220,7 +214,7 @@ void writeURDFFile(const std::shared_ptrSetAttribute("name", sg->getName().c_str()); // version? doc.InsertEndChild(xml_robot); diff --git a/tesseract_urdf/src/visual.cpp b/tesseract_urdf/src/visual.cpp index 1f7b0e6540a..47e76da0c58 100644 --- a/tesseract_urdf/src/visual.cpp +++ b/tesseract_urdf/src/visual.cpp @@ -41,11 +41,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +namespace tesseract_urdf +{ tesseract_scene_graph::Visual::Ptr -tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - std::unordered_map& available_materials, - int version) +parseVisual(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + std::unordered_map& available_materials) { // get name std::string visual_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -57,7 +58,7 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, { try { - visual_origin = parseOrigin(origin, version); + visual_origin = parseOrigin(origin); } catch (...) { @@ -72,7 +73,7 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, { try { - visual_material = parseMaterial(material, available_materials, true, version); + visual_material = parseMaterial(material, available_materials, true); } catch (...) { @@ -88,7 +89,7 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, tesseract_geometry::Geometry::Ptr geom; try { - geom = parseGeometry(geometry, locator, true, version); + geom = parseGeometry(geometry, locator, true); } catch (...) { @@ -104,16 +105,16 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, return visual; } -tinyxml2::XMLElement* tesseract_urdf::writeVisual(const std::shared_ptr& visual, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& link_name, - const int id = -1) +tinyxml2::XMLElement* writeVisual(const std::shared_ptr& visual, + tinyxml2::XMLDocument& doc, + const std::string& package_path, + const std::string& link_name, + const int id = -1) { if (visual == nullptr) std::throw_with_nested(std::runtime_error("Visual is nullptr and cannot be converted to XML")); - tinyxml2::XMLElement* xml_element = doc.NewElement("visual"); + tinyxml2::XMLElement* xml_element = doc.NewElement(VISUAL_ELEMENT_NAME); if (!visual->name.empty()) xml_element->SetAttribute("name", visual->name.c_str()); @@ -160,3 +161,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeVisual(const std::shared_ptr Date: Fri, 13 Dec 2024 16:05:03 -0600 Subject: [PATCH 2/3] Updated unit tests --- .../test/tesseract_urdf_box_unit.cpp | 24 ++- .../test/tesseract_urdf_calibration_unit.cpp | 12 +- .../test/tesseract_urdf_capsule_unit.cpp | 49 +++--- .../test/tesseract_urdf_collision_unit.cpp | 12 +- .../test/tesseract_urdf_common_unit.h | 41 ++--- .../test/tesseract_urdf_cone_unit.cpp | 49 +++--- .../test/tesseract_urdf_convex_mesh_unit.cpp | 164 +++++++++++++----- .../test/tesseract_urdf_cylinder_unit.cpp | 29 ++-- .../test/tesseract_urdf_dynamics_unit.cpp | 24 +-- .../tesseract_urdf_extra_delimeters_unit.cpp | 2 +- .../test/tesseract_urdf_geometry_unit.cpp | 68 ++++---- .../test/tesseract_urdf_inertial_unit.cpp | 80 ++++----- .../test/tesseract_urdf_joint_unit.cpp | 84 ++++++--- .../test/tesseract_urdf_limits_unit.cpp | 42 +++-- .../test/tesseract_urdf_link_unit.cpp | 113 ++++++++---- .../test/tesseract_urdf_material_unit.cpp | 92 +++++++--- .../tesseract_urdf_mesh_material_unit.cpp | 4 +- .../test/tesseract_urdf_mesh_unit.cpp | 18 +- .../test/tesseract_urdf_mimic_unit.cpp | 21 ++- .../test/tesseract_urdf_octree_unit.cpp | 100 +++++------ .../test/tesseract_urdf_origin_unit.cpp | 36 ++-- .../tesseract_urdf_safety_controller_unit.cpp | 14 +- .../test/tesseract_urdf_sdf_mesh_unit.cpp | 46 ++--- .../test/tesseract_urdf_sphere_unit.cpp | 17 +- .../test/tesseract_urdf_visual_unit.cpp | 40 +++-- 25 files changed, 732 insertions(+), 449 deletions(-) diff --git a/tesseract_urdf/test/tesseract_urdf_box_unit.cpp b/tesseract_urdf/test/tesseract_urdf_box_unit.cpp index 15d7fb9fefe..db61588979c 100644 --- a/tesseract_urdf/test/tesseract_urdf_box_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_box_unit.cpp @@ -13,7 +13,8 @@ TEST(TesseractURDFUnit, parse_box) // NOLINT { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_TRUE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); EXPECT_NEAR(geom->getX(), 1, 1e-8); EXPECT_NEAR(geom->getY(), 2, 1e-8); EXPECT_NEAR(geom->getZ(), 3, 1e-8); @@ -22,7 +23,8 @@ TEST(TesseractURDFUnit, parse_box) // NOLINT { // https://github.com/ros-industrial-consortium/tesseract_ros/issues/67 std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_TRUE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); EXPECT_NEAR(geom->getX(), 0.5, 1e-8); EXPECT_NEAR(geom->getY(), 0.25, 1e-8); EXPECT_NEAR(geom->getZ(), 0.75, 1e-8); @@ -31,37 +33,43 @@ TEST(TesseractURDFUnit, parse_box) // NOLINT { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } { std::string str = ""; tesseract_geometry::Box::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseBox, str, "box", 2)); + EXPECT_FALSE( + runTest(geom, &tesseract_urdf::parseBox, str, tesseract_urdf::BOX_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp b/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp index 4f0f7dd9658..29144f62714 100644 --- a/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp @@ -14,7 +14,7 @@ TEST(TesseractURDFUnit, parse_calibration) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); EXPECT_NEAR(elem->rising, 1, 1e-8); EXPECT_NEAR(elem->falling, 2, 1e-8); } @@ -23,7 +23,7 @@ TEST(TesseractURDFUnit, parse_calibration) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); EXPECT_NEAR(elem->rising, 1, 1e-8); EXPECT_NEAR(elem->falling, 0, 1e-8); } @@ -32,7 +32,7 @@ TEST(TesseractURDFUnit, parse_calibration) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); EXPECT_NEAR(elem->rising, 0, 1e-8); EXPECT_NEAR(elem->falling, 2, 1e-8); } @@ -41,21 +41,21 @@ TEST(TesseractURDFUnit, parse_calibration) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); } { std::string str = ""; tesseract_scene_graph::JointCalibration::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCalibration, str, "calibration", 2)); + elem, &tesseract_urdf::parseCalibration, str, tesseract_urdf::CALIBRATION_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp b/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp index 4565dd93a88..e7da8f2434d 100644 --- a/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp @@ -11,70 +11,79 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_capsule) // NOLINT { { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 1, 1e-8); EXPECT_NEAR(geom->getLength(), 2, 1e-8); } { // https://github.com/ros-industrial-consortium/tesseract_ros/issues/67 - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 0.25, 1e-8); EXPECT_NEAR(geom->getLength(), 0.5, 1e-8); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } // TODO: I would expect this to fail but tinyxml2 still parses it so need to create an issue. // { - // std::string str = R"()"; + // std::string str = R"()"; // tesseract_geometry::Capsule::Ptr geom; - // auto status = runTest(geom, str, "capsule"); + // auto status = runTest(geom, str, tesseract_urdf::CAPSULE_ELEMENT_NAME); // EXPECT_FALSE(*status); // EXPECT_FALSE(status->message().empty()); // } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } { - std::string str = ""; + std::string str = ""; tesseract_geometry::Capsule::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCapsule, str, "capsule", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCapsule, str, tesseract_urdf::CAPSULE_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp index 668da446887..fc1087c2cf4 100644 --- a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp @@ -23,7 +23,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_FALSE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } @@ -36,7 +36,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } @@ -49,7 +49,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); EXPECT_TRUE(elem->geometry->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH); @@ -65,7 +65,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem == nullptr); } @@ -77,7 +77,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem == nullptr); } @@ -86,7 +86,7 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; tesseract_scene_graph::Collision::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); + elem, &tesseract_urdf::parseCollision, str, tesseract_urdf::COLLISION_ELEMENT_NAME, resource_locator)); EXPECT_TRUE(elem == nullptr); } } diff --git a/tesseract_urdf/test/tesseract_urdf_common_unit.h b/tesseract_urdf/test/tesseract_urdf_common_unit.h index fc32790de86..e6abc50b1c0 100644 --- a/tesseract_urdf/test/tesseract_urdf_common_unit.h +++ b/tesseract_urdf/test/tesseract_urdf_common_unit.h @@ -19,10 +19,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP template bool runTest(ElementType& type, - std::function func, + std::function func, const std::string& xml_string, - const std::string& element_name, - int version) + const std::string& element_name) { tinyxml2::XMLDocument xml_doc; EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS); @@ -32,7 +31,7 @@ bool runTest(ElementType& type, try { - type = func(element, version); + type = func(element); } catch (const std::exception& e) { @@ -46,12 +45,10 @@ bool runTest(ElementType& type, template bool runTest( ElementType& type, - std::function - func, + std::function func, const std::string& xml_string, const std::string& element_name, const tesseract_common::ResourceLocator& locator, - int version, bool visual) { tinyxml2::XMLDocument xml_doc; @@ -62,7 +59,7 @@ bool runTest( try { - type = func(element, locator, visual, version); + type = func(element, locator, visual); } catch (const std::exception& e) { @@ -74,13 +71,11 @@ bool runTest( } template -bool runTest( - ElementType& type, - std::function func, - const std::string& xml_string, - const std::string& element_name, - const tesseract_common::ResourceLocator& locator, - int version) +bool runTest(ElementType& type, + std::function func, + const std::string& xml_string, + const std::string& element_name, + const tesseract_common::ResourceLocator& locator) { tinyxml2::XMLDocument xml_doc; EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS); @@ -90,7 +85,7 @@ bool runTest( try { - type = func(element, locator, version); + type = func(element, locator); } catch (const std::exception& e) { @@ -105,13 +100,11 @@ template bool runTest(ElementType& type, std::function&, - const int)> func, + std::unordered_map&)> func, const std::string& xml_string, const std::string& element_name, const tesseract_common::ResourceLocator& locator, - std::unordered_map& available_materials, - int version) + std::unordered_map& available_materials) { tinyxml2::XMLDocument xml_doc; EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS); @@ -121,7 +114,7 @@ bool runTest(ElementType& type, try { - type = func(element, locator, available_materials, version); + type = func(element, locator, available_materials); } catch (const std::exception& e) { @@ -136,12 +129,10 @@ template bool runTest(ElementType& type, std::function&, - bool, - const int)> func, + bool)> func, const std::string& xml_string, const std::string& element_name, std::unordered_map& available_materials, - int version, const bool visual) { tinyxml2::XMLDocument xml_doc; @@ -152,7 +143,7 @@ bool runTest(ElementType& type, try { - type = func(element, available_materials, visual, version); + type = func(element, available_materials, visual); } catch (const std::exception& e) { diff --git a/tesseract_urdf/test/tesseract_urdf_cone_unit.cpp b/tesseract_urdf/test/tesseract_urdf_cone_unit.cpp index 0cd399b9ec1..5f4e9ecd4f7 100644 --- a/tesseract_urdf/test/tesseract_urdf_cone_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_cone_unit.cpp @@ -11,70 +11,79 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_cone) // NOLINT { { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 1, 1e-8); EXPECT_NEAR(geom->getLength(), 2, 1e-8); } { // https://github.com/ros-industrial-consortium/tesseract_ros/issues/67 - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 0.25, 1e-8); EXPECT_NEAR(geom->getLength(), 0.5, 1e-8); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } // TODO: I would expect this to fail but tinyxml2 still parses it so need to create an issue. // { - // std::string str = R"()"; + // std::string str = R"()"; // tesseract_geometry::Cone::Ptr geom; - // auto status = runTest(geom, str, "cone"); + // auto status = runTest(geom, str, tesseract_urdf::CONE_ELEMENT_NAME); // EXPECT_FALSE(*status); // EXPECT_FALSE(status->message().empty()); // } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } { - std::string str = R"()"; + std::string str = R"()"; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } { - std::string str = ""; + std::string str = ""; tesseract_geometry::Cone::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCone, str, "cone", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCone, str, tesseract_urdf::CONE_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp index 0f59d8d60e4..093eb199277 100644 --- a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp @@ -25,10 +25,14 @@ TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT tesseract_common::GeneralResourceLocator resource_locator; { std::string str = - R"()"; + R"()"; std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 6); EXPECT_TRUE(geom[0]->getVertexCount() == 8); @@ -39,10 +43,10 @@ TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT { std::string str = - R"()"; + R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseConvexMesh, str, tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 12); EXPECT_TRUE(geom[0]->getVertexCount() == 8); @@ -53,10 +57,14 @@ TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT { std::string str = - R"()"; + R"()"; std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() >= 6); // Because we are converting due to numerical variance you could end up // with additional faces. @@ -65,18 +73,26 @@ TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT { std::string str = - R"()"; + R"()"; std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.size() == 2); } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 6); EXPECT_TRUE(geom[0]->getVertexCount() == 8); @@ -86,90 +102,142 @@ TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } { - std::string str = ""; + std::string str = ""; std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseConvexMesh, + str, + tesseract_urdf::CONVEX_MESH_ELEMENT_NAME, + resource_locator, + false)); EXPECT_TRUE(geom.empty()); } } @@ -188,7 +256,7 @@ TEST(TesseractURDFUnit, write_convex_mesh) // NOLINT EXPECT_EQ(0, writeTest( convex_mesh, &tesseract_urdf::writeConvexMesh, text, getTempPkgPath(), std::string("convex0.ply"))); - EXPECT_EQ(text, R"()"); + EXPECT_EQ(text, R"()"); } { diff --git a/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp b/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp index 8f0df432950..5f8899d3fd2 100644 --- a/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp @@ -13,7 +13,8 @@ TEST(TesseractURDFUnit, parse_cylinder) // NOLINT { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 1, 1e-8); EXPECT_NEAR(geom->getLength(), 2, 1e-8); } @@ -21,7 +22,8 @@ TEST(TesseractURDFUnit, parse_cylinder) // NOLINT { // https://github.com/ros-industrial-consortium/tesseract_ros/issues/67 std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 0.25, 1e-8); EXPECT_NEAR(geom->getLength(), 0.5, 1e-8); } @@ -29,32 +31,36 @@ TEST(TesseractURDFUnit, parse_cylinder) // NOLINT { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } // TODO: I would expect this to fail but tinyxml2 still parses it so need to create an issue. // { // std::string str = R"()"; // tesseract_geometry::Cylinder::Ptr geom; - // auto status = runTest(geom, str, "cylinder", 2); + // auto status = runTest(geom, str, tesseract_urdf::CYLINDER_ELEMENT_NAME); // EXPECT_FALSE(*status); // EXPECT_FALSE(status->message().empty()); // } @@ -62,19 +68,22 @@ TEST(TesseractURDFUnit, parse_cylinder) // NOLINT { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } { std::string str = ""; tesseract_geometry::Cylinder::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseCylinder, str, "cylinder", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseCylinder, str, tesseract_urdf::CYLINDER_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp b/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp index d3ee8c2b309..23e118d1662 100644 --- a/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp @@ -13,8 +13,8 @@ TEST(TesseractURDFUnit, parse_dynamics) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_TRUE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); EXPECT_NEAR(elem->damping, 1, 1e-8); EXPECT_NEAR(elem->friction, 2, 1e-8); } @@ -22,8 +22,8 @@ TEST(TesseractURDFUnit, parse_dynamics) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_TRUE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); EXPECT_NEAR(elem->damping, 1, 1e-8); EXPECT_NEAR(elem->friction, 0, 1e-8); } @@ -31,8 +31,8 @@ TEST(TesseractURDFUnit, parse_dynamics) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_TRUE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); EXPECT_NEAR(elem->damping, 0, 1e-8); EXPECT_NEAR(elem->friction, 2, 1e-8); } @@ -40,22 +40,22 @@ TEST(TesseractURDFUnit, parse_dynamics) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); } { std::string str = ""; tesseract_scene_graph::JointDynamics::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseDynamics, str, "dynamics", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseDynamics, str, tesseract_urdf::DYNAMICS_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_extra_delimeters_unit.cpp b/tesseract_urdf/test/tesseract_urdf_extra_delimeters_unit.cpp index b00baea2ba0..f112560cc33 100644 --- a/tesseract_urdf/test/tesseract_urdf_extra_delimeters_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_extra_delimeters_unit.cpp @@ -12,7 +12,7 @@ TEST(TesseractURDFUnit, parse_extra_delimeters) // NOLINT { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin")); EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8)); EXPECT_TRUE(origin.matrix().col(0).head(3).isApprox(Eigen::Vector3d(1, 0, 0), 1e-8)); EXPECT_TRUE(origin.matrix().col(1).head(3).isApprox(Eigen::Vector3d(0, -1, 0), 1e-8)); diff --git a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp index e4ff692031e..1020940ec16 100644 --- a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp @@ -19,7 +19,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::BOX); } @@ -29,7 +29,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SPHERE); } @@ -39,49 +39,49 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CYLINDER); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CONE); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CAPSULE); } { std::string str = R"( - - - + + + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::OCTREE); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CONVEX_MESH); } @@ -91,17 +91,17 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::MESH); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SDF_MESH); } @@ -111,7 +111,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -120,7 +120,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -130,7 +130,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -140,7 +140,7 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -150,49 +150,49 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } { std::string str = R"( - - - + + + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -202,17 +202,17 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } { std::string str = R"( - + )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, &tesseract_urdf::parseGeometry, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(elem == nullptr); } } diff --git a/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp b/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp index a1e0ce06c4b..58be9f26d38 100644 --- a/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp @@ -16,8 +16,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_TRUE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); EXPECT_NEAR(elem->mass, 1, 1e-8); EXPECT_NEAR(elem->ixx, 1, 1e-8); EXPECT_NEAR(elem->ixy, 2, 1e-8); @@ -33,8 +33,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_TRUE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); EXPECT_NEAR(elem->mass, 1, 1e-8); EXPECT_NEAR(elem->ixx, 1, 1e-8); EXPECT_NEAR(elem->ixy, 2, 1e-8); @@ -51,8 +51,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -60,8 +60,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -69,8 +69,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -79,8 +79,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -89,8 +89,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -99,8 +99,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -109,8 +109,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -119,8 +119,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -129,8 +129,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -139,8 +139,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -149,8 +149,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -159,8 +159,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -169,8 +169,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -179,8 +179,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -189,8 +189,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -199,8 +199,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -209,8 +209,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } { @@ -219,8 +219,8 @@ TEST(TesseractURDFUnit, parse_inertial) // NOLINT )"; tesseract_scene_graph::Inertial::Ptr elem; - EXPECT_FALSE( - runTest(elem, &tesseract_urdf::parseInertial, str, "inertial", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseInertial, str, tesseract_urdf::INERTIAL_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp index 82fdb39447a..62668d72312 100644 --- a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp @@ -22,7 +22,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::FLOATING); EXPECT_FALSE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -48,7 +49,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE); EXPECT_FALSE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -70,7 +72,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -92,7 +95,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::CONTINUOUS); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -113,7 +117,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::CONTINUOUS); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -135,7 +140,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::FIXED); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -157,7 +163,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::PRISMATIC); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -179,7 +186,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::REVOLUTE); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -201,7 +209,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); EXPECT_TRUE(elem->getName() == "my_joint"); EXPECT_TRUE(elem->type == tesseract_scene_graph::JointType::PLANAR); EXPECT_TRUE(elem->parent_to_joint_origin_transform.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -221,7 +230,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -230,7 +240,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -238,7 +249,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -246,7 +258,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -257,7 +270,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -268,7 +282,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -279,7 +294,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -290,7 +306,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -301,7 +318,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -316,7 +334,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -331,7 +350,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -346,7 +366,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -361,7 +382,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -376,7 +398,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -391,7 +414,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -407,7 +431,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -422,7 +447,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -434,7 +460,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } { @@ -444,7 +471,8 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT )"; tesseract_scene_graph::Joint::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseJoint, str, "joint", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseJoint, str, tesseract_urdf::JOINT_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp b/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp index 099141cfb37..05b87840b15 100644 --- a/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp @@ -15,7 +15,8 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); EXPECT_NEAR(elem->lower, 1, 1e-8); EXPECT_NEAR(elem->upper, 2, 1e-8); EXPECT_NEAR(elem->effort, 3, 1e-8); @@ -25,7 +26,8 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); EXPECT_NEAR(elem->lower, 0, 1e-8); EXPECT_NEAR(elem->upper, 2, 1e-8); EXPECT_NEAR(elem->effort, 3, 1e-8); @@ -35,7 +37,8 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); EXPECT_NEAR(elem->lower, 1, 1e-8); EXPECT_NEAR(elem->upper, 0, 1e-8); EXPECT_NEAR(elem->effort, 3, 1e-8); @@ -45,7 +48,8 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); EXPECT_NEAR(elem->lower, 0, 1e-8); EXPECT_NEAR(elem->upper, 0, 1e-8); EXPECT_NEAR(elem->effort, 3, 1e-8); @@ -55,7 +59,8 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); EXPECT_NEAR(elem->lower, 0, 1e-8); EXPECT_NEAR(elem->upper, 0, 1e-8); EXPECT_NEAR(elem->effort, 3, 1e-8); @@ -66,55 +71,64 @@ TEST(TesseractURDFUnit, parse_limits) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } { std::string str = ""; tesseract_scene_graph::JointLimits::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseLimits, str, "limit", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseLimits, str, tesseract_urdf::LIMITS_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp index 3c8aaae12b6..8ca6a71c3da 100644 --- a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp @@ -39,8 +39,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial != nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -88,8 +92,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial != nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -117,8 +125,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -159,8 +171,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -182,8 +198,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -212,8 +232,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -232,8 +256,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -258,8 +286,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -271,8 +303,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT std::unordered_map empty_available_materials; std::string str = R"()"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -293,8 +329,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } { @@ -311,8 +351,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } { @@ -330,8 +374,12 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } { @@ -345,15 +393,20 @@ TEST(TesseractURDFUnit, parse_link) // NOLINT )"; tesseract_scene_graph::Link::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseLink, str, "link", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseLink, + str, + tesseract_urdf::LINK_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } } TEST(TesseractURDFUnit, write_link) // NOLINT { { // Trigger id adjustments and inertial - tesseract_scene_graph::Link::Ptr link = std::make_shared("link"); + tesseract_scene_graph::Link::Ptr link = + std::make_shared(tesseract_urdf::LINK_ELEMENT_NAME); link->inertial = std::make_shared(); tesseract_scene_graph::Collision::Ptr collision = std::make_shared(); @@ -378,7 +431,8 @@ TEST(TesseractURDFUnit, write_link) // NOLINT } { // Trigger nullptr collision - tesseract_scene_graph::Link::Ptr link = std::make_shared("link"); + tesseract_scene_graph::Link::Ptr link = + std::make_shared(tesseract_urdf::LINK_ELEMENT_NAME); link->inertial = std::make_shared(); link->collision.push_back(nullptr); @@ -397,7 +451,8 @@ TEST(TesseractURDFUnit, write_link) // NOLINT } { // Trigger nullptr visual - tesseract_scene_graph::Link::Ptr link = std::make_shared("link"); + tesseract_scene_graph::Link::Ptr link = + std::make_shared(tesseract_urdf::LINK_ELEMENT_NAME); link->inertial = std::make_shared(); tesseract_scene_graph::Collision::Ptr collision = std::make_shared(); diff --git a/tesseract_urdf/test/tesseract_urdf_material_unit.cpp b/tesseract_urdf/test/tesseract_urdf_material_unit.cpp index 62573d8fe78..6128676f09f 100644 --- a/tesseract_urdf/test/tesseract_urdf_material_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_material_unit.cpp @@ -26,8 +26,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("extra="0 0 0"/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); EXPECT_TRUE(elem->getName() == "test_material"); EXPECT_TRUE(elem->color.isApprox(Eigen::Vector4d(1, .5, .5, 1), 1e-8)); EXPECT_TRUE(elem->texture_filename == tesseract_common::getTempPath() + "texture.txt"); @@ -42,8 +46,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); EXPECT_TRUE(elem->getName() == "test_material"); EXPECT_TRUE(elem->color.isApprox(Eigen::Vector4d(1, .5, .5, 1), 1e-8)); EXPECT_TRUE(elem->texture_filename.empty()); @@ -57,7 +65,7 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT std::string str = R"()"; tesseract_scene_graph::Material::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", available_materials, 2, true)); + elem, &tesseract_urdf::parseMaterial, str, tesseract_urdf::MATERIAL_ELEMENT_NAME, available_materials, true)); EXPECT_TRUE(elem->getName() == "test_material"); EXPECT_TRUE(elem->color.isApprox(Eigen::Vector4d(1, .5, .5, 1), 1e-8)); EXPECT_TRUE(elem->texture_filename == tesseract_common::getTempPath() + "texture.txt"); @@ -73,8 +81,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -86,8 +98,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -98,7 +114,7 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT std::string str = R"()"; tesseract_scene_graph::Material::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", available_materials, 2, true)); + elem, &tesseract_urdf::parseMaterial, str, tesseract_urdf::MATERIAL_ELEMENT_NAME, available_materials, true)); } { @@ -108,8 +124,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT std::string str = R"()"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -123,8 +143,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -138,8 +162,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -153,8 +181,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -168,8 +200,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -183,8 +219,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } { @@ -198,8 +238,12 @@ TEST(TesseractURDFUnit, parse_material) // NOLINT m->texture_filename + R"("/> )"; tesseract_scene_graph::Material::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseMaterial, str, "material", empty_available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME, + empty_available_materials, + true)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp index 57ea6710b32..bb3165d6e5a 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp @@ -17,7 +17,7 @@ TEST(TesseractURDFUnit, parse_mesh_material_dae) // NOLINT std::string str = R"()"; std::vector meshes; EXPECT_TRUE(runTest>( - meshes, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + meshes, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(meshes.size() == 4); auto& mesh0 = meshes[1]; auto& mesh1 = meshes[2]; @@ -91,7 +91,7 @@ TEST(TesseractURDFUnit, parse_mesh_material_gltf2) // NOLINT std::string str = R"()"; std::vector meshes; EXPECT_TRUE(runTest>( - meshes, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + meshes, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(meshes.size() == 4); auto& mesh0 = meshes[0]; auto& mesh1 = meshes[1]; diff --git a/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp index 290b610dbf0..866efac7579 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp @@ -28,7 +28,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -41,7 +41,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -54,7 +54,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -62,7 +62,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -70,7 +70,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -78,7 +78,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -86,7 +86,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -94,7 +94,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } @@ -102,7 +102,7 @@ TEST(TesseractURDFUnit, parse_mesh) // NOLINT std::string str = ""; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseMesh, str, tesseract_urdf::MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.empty()); } } diff --git a/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp index 7410b96def9..9013b10186f 100644 --- a/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp @@ -13,7 +13,8 @@ TEST(TesseractURDFUnit, parse_mimic) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); EXPECT_TRUE(elem->joint_name == "joint_1"); EXPECT_NEAR(elem->multiplier, 1, 1e-8); EXPECT_NEAR(elem->offset, 2, 1e-8); @@ -22,7 +23,8 @@ TEST(TesseractURDFUnit, parse_mimic) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); EXPECT_TRUE(elem->joint_name == "joint_1"); EXPECT_NEAR(elem->multiplier, 1, 1e-8); EXPECT_NEAR(elem->offset, 0, 1e-8); @@ -31,7 +33,8 @@ TEST(TesseractURDFUnit, parse_mimic) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); EXPECT_TRUE(elem->joint_name == "joint_1"); EXPECT_NEAR(elem->multiplier, 1, 1e-8); EXPECT_NEAR(elem->offset, 2, 1e-8); @@ -40,7 +43,8 @@ TEST(TesseractURDFUnit, parse_mimic) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_TRUE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_TRUE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); EXPECT_TRUE(elem->joint_name == "joint_1"); EXPECT_NEAR(elem->multiplier, 1, 1e-8); EXPECT_NEAR(elem->offset, 0, 1e-8); @@ -49,19 +53,22 @@ TEST(TesseractURDFUnit, parse_mimic) // NOLINT { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); } { std::string str = ""; tesseract_scene_graph::JointMimic::Ptr elem; - EXPECT_FALSE(runTest(elem, &tesseract_urdf::parseMimic, str, "mimic", 2)); + EXPECT_FALSE(runTest( + elem, &tesseract_urdf::parseMimic, str, tesseract_urdf::MIMIC_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp index aecfd81199c..602c51880e1 100644 --- a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp @@ -32,36 +32,36 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT // } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_INSIDE); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); @@ -69,12 +69,12 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT #ifdef TESSERACT_PARSE_POINT_CLOUDS { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 1000); @@ -82,12 +82,12 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 496); @@ -95,77 +95,77 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } #endif { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"( - - )"; + std::string str = R"( + + )"; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } { - std::string str = ""; + std::string str = ""; tesseract_geometry::Octree::Ptr geom; EXPECT_FALSE(runTest( - geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); + geom, &tesseract_urdf::parseOctomap, str, tesseract_urdf::OCTOMAP_ELEMENT_NAME, resource_locator, true)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_origin_unit.cpp b/tesseract_urdf/test/tesseract_urdf_origin_unit.cpp index 9aa45d6cb7a..716ec1be1a8 100644 --- a/tesseract_urdf/test/tesseract_urdf_origin_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_origin_unit.cpp @@ -32,14 +32,16 @@ TEST(TesseractURDFUnit, parse_origin) // NOLINT { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } @@ -49,7 +51,8 @@ TEST(TesseractURDFUnit, parse_origin) // NOLINT Eigen::Isometry3d check = Eigen::Isometry3d::Identity(); check.linear() << 0.6435823, -0.5794841, 0.5000000, 0.7558624, 0.5838996, -0.2961981, -0.1203077, 0.5685591, 0.8137977; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(check, 1e-6)); } @@ -58,14 +61,16 @@ TEST(TesseractURDFUnit, parse_origin) // NOLINT Eigen::Isometry3d origin; Eigen::Isometry3d check = Eigen::Isometry3d::Identity(); check.linear() = fromRPY(0.3490659, 0.5235988, 0.7330383).toRotationMatrix(); - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(check, 1e-6)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8)); EXPECT_TRUE(origin.matrix().col(0).head(3).isApprox(Eigen::Vector3d(1, 0, 0), 1e-8)); EXPECT_TRUE(origin.matrix().col(1).head(3).isApprox(Eigen::Vector3d(0, -1, 0), 1e-8)); @@ -80,7 +85,8 @@ TEST(TesseractURDFUnit, parse_origin) // NOLINT { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8)); EXPECT_TRUE(origin.rotation().isApprox(Eigen::Matrix3d::Identity(), 1e-8)); } @@ -88,39 +94,45 @@ TEST(TesseractURDFUnit, parse_origin) // NOLINT { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_TRUE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_TRUE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); EXPECT_TRUE(origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_FALSE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_FALSE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_FALSE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_FALSE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_FALSE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_FALSE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); } { std::string str = R"()"; Eigen::Isometry3d origin; - EXPECT_FALSE(runTest(origin, &tesseract_urdf::parseOrigin, str, "origin", 2)); + EXPECT_FALSE( + runTest(origin, &tesseract_urdf::parseOrigin, str, tesseract_urdf::ORIGIN_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp b/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp index 5696f19ab69..a809d7491c0 100644 --- a/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp @@ -15,7 +15,7 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); EXPECT_NEAR(elem->soft_lower_limit, 1, 1e-8); EXPECT_NEAR(elem->soft_upper_limit, 2, 1e-8); EXPECT_NEAR(elem->k_position, 3, 1e-8); @@ -26,7 +26,7 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); EXPECT_NEAR(elem->soft_lower_limit, 0, 1e-8); EXPECT_NEAR(elem->soft_upper_limit, 2, 1e-8); EXPECT_NEAR(elem->k_position, 3, 1e-8); @@ -37,7 +37,7 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); EXPECT_NEAR(elem->soft_lower_limit, 1, 1e-8); EXPECT_NEAR(elem->soft_upper_limit, 0, 1e-8); EXPECT_NEAR(elem->k_position, 3, 1e-8); @@ -48,7 +48,7 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); EXPECT_NEAR(elem->soft_lower_limit, 1, 1e-8); EXPECT_NEAR(elem->soft_upper_limit, 2, 1e-8); EXPECT_NEAR(elem->k_position, 0, 1e-8); @@ -59,7 +59,7 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); EXPECT_NEAR(elem->soft_lower_limit, 0, 1e-8); EXPECT_NEAR(elem->soft_upper_limit, 0, 1e-8); EXPECT_NEAR(elem->k_position, 0, 1e-8); @@ -70,14 +70,14 @@ TEST(TesseractURDFUnit, parse_safety_controller) // NOLINT std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_scene_graph::JointSafety::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseSafetyController, str, "safety_controller", 2)); + elem, &tesseract_urdf::parseSafetyController, str, tesseract_urdf::SAFETY_CONTROLLER_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp index a0fe2578046..47ec08e954b 100644 --- a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp @@ -25,10 +25,10 @@ TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT tesseract_common::GeneralResourceLocator resource_locator; { std::string str = - R"()"; + R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -38,10 +38,10 @@ TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -51,10 +51,10 @@ TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, false)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, false)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 42); @@ -64,52 +64,56 @@ TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = + R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } { - std::string str = R"()"; + std::string str = R"()"; std::vector geom; EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseSDFMesh, str, "sdf_mesh", resource_locator, 2, true)); + geom, &tesseract_urdf::parseSDFMesh, str, tesseract_urdf::SDF_MESH_ELEMENT_NAME, resource_locator, true)); } } @@ -127,7 +131,7 @@ TEST(TesseractURDFUnit, write_sdf_mesh) // NOLINT EXPECT_EQ(0, writeTest( sdf_mesh, &tesseract_urdf::writeSDFMesh, text, getTempPkgPath(), std::string("sdf0.ply"))); - EXPECT_EQ(text, R"()"); + EXPECT_EQ(text, R"()"); } { diff --git a/tesseract_urdf/test/tesseract_urdf_sphere_unit.cpp b/tesseract_urdf/test/tesseract_urdf_sphere_unit.cpp index d136ebfdb35..20a6df7fc78 100644 --- a/tesseract_urdf/test/tesseract_urdf_sphere_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_sphere_unit.cpp @@ -13,34 +13,38 @@ TEST(TesseractURDFUnit, parse_sphere) // NOLINT { std::string str = R"()"; tesseract_geometry::Sphere::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseSphere, str, "sphere", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseSphere, str, tesseract_urdf::SPHERE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 1, 1e-8); } { // https://github.com/ros-industrial-consortium/tesseract_ros/issues/67 std::string str = R"()"; tesseract_geometry::Sphere::Ptr geom; - EXPECT_TRUE(runTest(geom, &tesseract_urdf::parseSphere, str, "sphere", 2)); + EXPECT_TRUE(runTest( + geom, &tesseract_urdf::parseSphere, str, tesseract_urdf::SPHERE_ELEMENT_NAME)); EXPECT_NEAR(geom->getRadius(), 0.25, 1e-8); } { std::string str = R"()"; tesseract_geometry::Sphere::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseSphere, str, "sphere", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseSphere, str, tesseract_urdf::SPHERE_ELEMENT_NAME)); } { std::string str = R"()"; tesseract_geometry::Sphere::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseSphere, str, "sphere", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseSphere, str, tesseract_urdf::SPHERE_ELEMENT_NAME)); } // TODO: I would expect this to fail but tinyxml2 still parses it so need to create an issue. // { // std::string str = R"()"; // tesseract_geometry::Sphere::Ptr geom; - // auto status = runTest(geom, str, "sphere", 2); + // auto status = runTest(geom, str, tesseract_urdf::SPHERE_ELEMENT_NAME); // EXPECT_FALSE(*status); // EXPECT_FALSE(status->message().empty()); // } @@ -48,7 +52,8 @@ TEST(TesseractURDFUnit, parse_sphere) // NOLINT { std::string str = R"()"; tesseract_geometry::Sphere::Ptr geom; - EXPECT_FALSE(runTest(geom, &tesseract_urdf::parseSphere, str, "sphere", 2)); + EXPECT_FALSE(runTest( + geom, &tesseract_urdf::parseSphere, str, tesseract_urdf::SPHERE_ELEMENT_NAME)); } } diff --git a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp index 53df03e9be7..4531c401477 100644 --- a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp @@ -26,8 +26,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; tesseract_scene_graph::Visual::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseVisual, + str, + tesseract_urdf::VISUAL_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->material != nullptr); EXPECT_FALSE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -44,8 +48,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; tesseract_scene_graph::Visual::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseVisual, + str, + tesseract_urdf::VISUAL_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->material != nullptr); EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -59,8 +67,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; tesseract_scene_graph::Visual::Ptr elem; - EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseVisual, + str, + tesseract_urdf::VISUAL_ELEMENT_NAME, + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->material != nullptr); EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); @@ -74,8 +86,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; tesseract_scene_graph::Visual::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseVisual, + str, + tesseract_urdf::VISUAL_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } { @@ -86,8 +102,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; tesseract_scene_graph::Visual::Ptr elem; - EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseVisual, + str, + tesseract_urdf::VISUAL_ELEMENT_NAME, + resource_locator, + empty_available_materials)); } } From eb08453b1cfd5b140dcf7e5621f004e4e7c821ae Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 13 Dec 2024 19:09:38 -0600 Subject: [PATCH 3/3] Added NOLINT to URDF element string name definitions --- tesseract_urdf/include/tesseract_urdf/box.h | 2 +- tesseract_urdf/include/tesseract_urdf/calibration.h | 2 +- tesseract_urdf/include/tesseract_urdf/capsule.h | 2 +- tesseract_urdf/include/tesseract_urdf/collision.h | 2 +- tesseract_urdf/include/tesseract_urdf/cone.h | 2 +- tesseract_urdf/include/tesseract_urdf/convex_mesh.h | 2 +- tesseract_urdf/include/tesseract_urdf/cylinder.h | 2 +- tesseract_urdf/include/tesseract_urdf/dynamics.h | 2 +- tesseract_urdf/include/tesseract_urdf/geometry.h | 2 +- tesseract_urdf/include/tesseract_urdf/inertial.h | 2 +- tesseract_urdf/include/tesseract_urdf/joint.h | 2 +- tesseract_urdf/include/tesseract_urdf/limits.h | 2 +- tesseract_urdf/include/tesseract_urdf/link.h | 2 +- tesseract_urdf/include/tesseract_urdf/material.h | 2 +- tesseract_urdf/include/tesseract_urdf/mesh.h | 2 +- tesseract_urdf/include/tesseract_urdf/mimic.h | 2 +- tesseract_urdf/include/tesseract_urdf/octomap.h | 2 +- tesseract_urdf/include/tesseract_urdf/octree.h | 2 +- tesseract_urdf/include/tesseract_urdf/origin.h | 2 +- tesseract_urdf/include/tesseract_urdf/point_cloud.h | 2 +- tesseract_urdf/include/tesseract_urdf/safety_controller.h | 2 +- tesseract_urdf/include/tesseract_urdf/sdf_mesh.h | 2 +- tesseract_urdf/include/tesseract_urdf/sphere.h | 2 +- tesseract_urdf/include/tesseract_urdf/visual.h | 2 +- 24 files changed, 24 insertions(+), 24 deletions(-) diff --git a/tesseract_urdf/include/tesseract_urdf/box.h b/tesseract_urdf/include/tesseract_urdf/box.h index 6ce5715770b..c9254a37b15 100644 --- a/tesseract_urdf/include/tesseract_urdf/box.h +++ b/tesseract_urdf/include/tesseract_urdf/box.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* BOX_ELEMENT_NAME = "box"; +static const char* BOX_ELEMENT_NAME = "box"; // NOLINT /** * @brief Parse a xml box element diff --git a/tesseract_urdf/include/tesseract_urdf/calibration.h b/tesseract_urdf/include/tesseract_urdf/calibration.h index dcf99a00203..dc2233f7283 100644 --- a/tesseract_urdf/include/tesseract_urdf/calibration.h +++ b/tesseract_urdf/include/tesseract_urdf/calibration.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* CALIBRATION_ELEMENT_NAME = "calibration"; +static const char* CALIBRATION_ELEMENT_NAME = "calibration"; // NOLINT /** * @brief Parse a xml calibration element diff --git a/tesseract_urdf/include/tesseract_urdf/capsule.h b/tesseract_urdf/include/tesseract_urdf/capsule.h index d86337fce5e..0db2b6d99d8 100644 --- a/tesseract_urdf/include/tesseract_urdf/capsule.h +++ b/tesseract_urdf/include/tesseract_urdf/capsule.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* CAPSULE_ELEMENT_NAME = "tesseract:capsule"; +static const char* CAPSULE_ELEMENT_NAME = "tesseract:capsule"; // NOLINT /** * @brief Parse a xml capsule element diff --git a/tesseract_urdf/include/tesseract_urdf/collision.h b/tesseract_urdf/include/tesseract_urdf/collision.h index c6a917362a7..d74775d65ec 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -43,7 +43,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* COLLISION_ELEMENT_NAME = "collision"; +static const char* COLLISION_ELEMENT_NAME = "collision"; // NOLINT /** * @brief Parse xml element collision diff --git a/tesseract_urdf/include/tesseract_urdf/cone.h b/tesseract_urdf/include/tesseract_urdf/cone.h index 00ea755ecf2..8ca02ea9191 100644 --- a/tesseract_urdf/include/tesseract_urdf/cone.h +++ b/tesseract_urdf/include/tesseract_urdf/cone.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* CONE_ELEMENT_NAME = "tesseract:cone"; +static const char* CONE_ELEMENT_NAME = "tesseract:cone"; // NOLINT /** * @brief Parse a xml cone element diff --git a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h index 16c3b61c7c7..76481b80af4 100644 --- a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h @@ -43,7 +43,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* CONVEX_MESH_ELEMENT_NAME = "tesseract:convex_mesh"; +static const char* CONVEX_MESH_ELEMENT_NAME = "tesseract:convex_mesh"; // NOLINT /** * @brief Parse xml element convex_mesh diff --git a/tesseract_urdf/include/tesseract_urdf/cylinder.h b/tesseract_urdf/include/tesseract_urdf/cylinder.h index bf60bd2d14d..54163dfbc62 100644 --- a/tesseract_urdf/include/tesseract_urdf/cylinder.h +++ b/tesseract_urdf/include/tesseract_urdf/cylinder.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* CYLINDER_ELEMENT_NAME = "cylinder"; +static const char* CYLINDER_ELEMENT_NAME = "cylinder"; // NOLINT /** * @brief Parse a xml cylinder element diff --git a/tesseract_urdf/include/tesseract_urdf/dynamics.h b/tesseract_urdf/include/tesseract_urdf/dynamics.h index dc9c40da764..2fb00dde851 100644 --- a/tesseract_urdf/include/tesseract_urdf/dynamics.h +++ b/tesseract_urdf/include/tesseract_urdf/dynamics.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* DYNAMICS_ELEMENT_NAME = "dynamics"; +static const char* DYNAMICS_ELEMENT_NAME = "dynamics"; // NOLINT /** * @brief Parse a xml dynamics element diff --git a/tesseract_urdf/include/tesseract_urdf/geometry.h b/tesseract_urdf/include/tesseract_urdf/geometry.h index 0d8d1e7e047..58029b94758 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -43,7 +43,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* GEOMETRY_ELEMENT_NAME = "geometry"; +static const char* GEOMETRY_ELEMENT_NAME = "geometry"; // NOLINT /** * @brief Parse xml element geometry diff --git a/tesseract_urdf/include/tesseract_urdf/inertial.h b/tesseract_urdf/include/tesseract_urdf/inertial.h index 6e853454958..4779d6f7289 100644 --- a/tesseract_urdf/include/tesseract_urdf/inertial.h +++ b/tesseract_urdf/include/tesseract_urdf/inertial.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* INERTIAL_ELEMENT_NAME = "inertial"; +static const char* INERTIAL_ELEMENT_NAME = "inertial"; // NOLINT /** * @brief Parse xml element inertial diff --git a/tesseract_urdf/include/tesseract_urdf/joint.h b/tesseract_urdf/include/tesseract_urdf/joint.h index 998e70d3741..1cb4182774a 100644 --- a/tesseract_urdf/include/tesseract_urdf/joint.h +++ b/tesseract_urdf/include/tesseract_urdf/joint.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* JOINT_ELEMENT_NAME = "joint"; +static const char* JOINT_ELEMENT_NAME = "joint"; // NOLINT /** * @brief Parse xml element joint diff --git a/tesseract_urdf/include/tesseract_urdf/limits.h b/tesseract_urdf/include/tesseract_urdf/limits.h index 05d397ec01f..1a15bc69f74 100644 --- a/tesseract_urdf/include/tesseract_urdf/limits.h +++ b/tesseract_urdf/include/tesseract_urdf/limits.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* LIMITS_ELEMENT_NAME = "limit"; +static const char* LIMITS_ELEMENT_NAME = "limit"; // NOLINT /** * @brief Parse xml element limits diff --git a/tesseract_urdf/include/tesseract_urdf/link.h b/tesseract_urdf/include/tesseract_urdf/link.h index eaa8436488e..6fe40a7193d 100644 --- a/tesseract_urdf/include/tesseract_urdf/link.h +++ b/tesseract_urdf/include/tesseract_urdf/link.h @@ -43,7 +43,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* LINK_ELEMENT_NAME = "link"; +static const char* LINK_ELEMENT_NAME = "link"; // NOLINT /** * @brief Parse xml element link diff --git a/tesseract_urdf/include/tesseract_urdf/material.h b/tesseract_urdf/include/tesseract_urdf/material.h index 30b8ce37e38..1821fc56931 100644 --- a/tesseract_urdf/include/tesseract_urdf/material.h +++ b/tesseract_urdf/include/tesseract_urdf/material.h @@ -42,7 +42,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* MATERIAL_ELEMENT_NAME = "material"; +static const char* MATERIAL_ELEMENT_NAME = "material"; // NOLINT /** * @brief Parse xml element material diff --git a/tesseract_urdf/include/tesseract_urdf/mesh.h b/tesseract_urdf/include/tesseract_urdf/mesh.h index e05e61be081..6830bbc8c14 100644 --- a/tesseract_urdf/include/tesseract_urdf/mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/mesh.h @@ -43,7 +43,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* MESH_ELEMENT_NAME = "mesh"; +static const char* MESH_ELEMENT_NAME = "mesh"; // NOLINT /** * @brief Parse xml element mesh diff --git a/tesseract_urdf/include/tesseract_urdf/mimic.h b/tesseract_urdf/include/tesseract_urdf/mimic.h index 2192750e165..a7571a7a182 100644 --- a/tesseract_urdf/include/tesseract_urdf/mimic.h +++ b/tesseract_urdf/include/tesseract_urdf/mimic.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* MIMIC_ELEMENT_NAME = "mimic"; +static const char* MIMIC_ELEMENT_NAME = "mimic"; // NOLINT /** * @brief Parse xml element mimic diff --git a/tesseract_urdf/include/tesseract_urdf/octomap.h b/tesseract_urdf/include/tesseract_urdf/octomap.h index af467786d03..2d1e7f2474a 100644 --- a/tesseract_urdf/include/tesseract_urdf/octomap.h +++ b/tesseract_urdf/include/tesseract_urdf/octomap.h @@ -42,7 +42,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* OCTOMAP_ELEMENT_NAME = "tesseract:octomap"; +static const char* OCTOMAP_ELEMENT_NAME = "tesseract:octomap"; // NOLINT /** * @brief Parse xml element octomap diff --git a/tesseract_urdf/include/tesseract_urdf/octree.h b/tesseract_urdf/include/tesseract_urdf/octree.h index 3846df80f8f..843869914cc 100644 --- a/tesseract_urdf/include/tesseract_urdf/octree.h +++ b/tesseract_urdf/include/tesseract_urdf/octree.h @@ -42,7 +42,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* OCTREE_ELEMENT_NAME = "tesseract:octree"; +static const char* OCTREE_ELEMENT_NAME = "tesseract:octree"; // NOLINT /** * @brief Parse xml element octree diff --git a/tesseract_urdf/include/tesseract_urdf/origin.h b/tesseract_urdf/include/tesseract_urdf/origin.h index 443756306eb..05f986507a2 100644 --- a/tesseract_urdf/include/tesseract_urdf/origin.h +++ b/tesseract_urdf/include/tesseract_urdf/origin.h @@ -39,7 +39,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* ORIGIN_ELEMENT_NAME = "origin"; +static const char* ORIGIN_ELEMENT_NAME = "origin"; // NOLINT /** * @brief Parse xml element origin diff --git a/tesseract_urdf/include/tesseract_urdf/point_cloud.h b/tesseract_urdf/include/tesseract_urdf/point_cloud.h index 118b00cd43c..312c9de07c7 100644 --- a/tesseract_urdf/include/tesseract_urdf/point_cloud.h +++ b/tesseract_urdf/include/tesseract_urdf/point_cloud.h @@ -42,7 +42,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* POINT_CLOUD_ELEMENT_NAME = "tesseract:point_cloud"; +static const char* POINT_CLOUD_ELEMENT_NAME = "tesseract:point_cloud"; // NOLINT /** * @brief Parse xml element point_cloud diff --git a/tesseract_urdf/include/tesseract_urdf/safety_controller.h b/tesseract_urdf/include/tesseract_urdf/safety_controller.h index 4fa54567c65..323d72d6b4a 100644 --- a/tesseract_urdf/include/tesseract_urdf/safety_controller.h +++ b/tesseract_urdf/include/tesseract_urdf/safety_controller.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* SAFETY_CONTROLLER_ELEMENT_NAME = "safety_controller"; +static const char* SAFETY_CONTROLLER_ELEMENT_NAME = "safety_controller"; // NOLINT /** * @brief Parse xml element safety_controller diff --git a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h index caa4899f723..a1f501b27f4 100644 --- a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h @@ -42,7 +42,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* SDF_MESH_ELEMENT_NAME = "tesseract:sdf_mesh"; +static const char* SDF_MESH_ELEMENT_NAME = "tesseract:sdf_mesh"; // NOLINT /** * @brief Parse xml element sdf_mesh diff --git a/tesseract_urdf/include/tesseract_urdf/sphere.h b/tesseract_urdf/include/tesseract_urdf/sphere.h index d85992bfcaa..d31aa48e44b 100644 --- a/tesseract_urdf/include/tesseract_urdf/sphere.h +++ b/tesseract_urdf/include/tesseract_urdf/sphere.h @@ -41,7 +41,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* SPHERE_ELEMENT_NAME = "sphere"; +static const char* SPHERE_ELEMENT_NAME = "sphere"; // NOLINT /** * @brief Parse a xml sphere element diff --git a/tesseract_urdf/include/tesseract_urdf/visual.h b/tesseract_urdf/include/tesseract_urdf/visual.h index b1ea80721d4..41c76fc625d 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -44,7 +44,7 @@ class XMLDocument; namespace tesseract_urdf { -static const char* VISUAL_ELEMENT_NAME = "visual"; +static const char* VISUAL_ELEMENT_NAME = "visual"; // NOLINT /** * @brief Parse xml element visual