diff --git a/tesseract_support/urdf/abb_irb2400.urdf b/tesseract_support/urdf/abb_irb2400.urdf index cff1f3d3550..86c030adc9b 100644 --- a/tesseract_support/urdf/abb_irb2400.urdf +++ b/tesseract_support/urdf/abb_irb2400.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/abb_irb2400_external_positioner.urdf b/tesseract_support/urdf/abb_irb2400_external_positioner.urdf index e4f223ec38e..95ea5fd644d 100644 --- a/tesseract_support/urdf/abb_irb2400_external_positioner.urdf +++ b/tesseract_support/urdf/abb_irb2400_external_positioner.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/abb_irb2400_on_positioner.urdf b/tesseract_support/urdf/abb_irb2400_on_positioner.urdf index aa9d3ef33d8..a53d1b02ce0 100644 --- a/tesseract_support/urdf/abb_irb2400_on_positioner.urdf +++ b/tesseract_support/urdf/abb_irb2400_on_positioner.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/boxbot.urdf b/tesseract_support/urdf/boxbot.urdf index e50877d3fb1..25f0c3024d5 100644 --- a/tesseract_support/urdf/boxbot.urdf +++ b/tesseract_support/urdf/boxbot.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/car_seat_demo.urdf b/tesseract_support/urdf/car_seat_demo.urdf index b77f159f1c8..7e35f50ff5e 100644 --- a/tesseract_support/urdf/car_seat_demo.urdf +++ b/tesseract_support/urdf/car_seat_demo.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/car_seat_demo.xacro b/tesseract_support/urdf/car_seat_demo.xacro index 5f24546f10a..0404f6cbba7 100644 --- a/tesseract_support/urdf/car_seat_demo.xacro +++ b/tesseract_support/urdf/car_seat_demo.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/gantry_robot_underslung/gudel_fp6.xacro b/tesseract_support/urdf/gantry_robot_underslung/gudel_fp6.xacro index eb9087a29b0..ae3779cae1b 100644 --- a/tesseract_support/urdf/gantry_robot_underslung/gudel_fp6.xacro +++ b/tesseract_support/urdf/gantry_robot_underslung/gudel_fp6.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/iiwa7.urdf b/tesseract_support/urdf/iiwa7.urdf index d92997ac155..5669087eda4 100644 --- a/tesseract_support/urdf/iiwa7.urdf +++ b/tesseract_support/urdf/iiwa7.urdf @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820.urdf b/tesseract_support/urdf/lbr_iiwa_14_r820.urdf index 5e34bbb58ee..7ee6b22434e 100644 --- a/tesseract_support/urdf/lbr_iiwa_14_r820.urdf +++ b/tesseract_support/urdf/lbr_iiwa_14_r820.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820.xacro b/tesseract_support/urdf/lbr_iiwa_14_r820.xacro index c2776232f41..de1d19143ce 100644 --- a/tesseract_support/urdf/lbr_iiwa_14_r820.xacro +++ b/tesseract_support/urdf/lbr_iiwa_14_r820.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/online_planning_example.urdf b/tesseract_support/urdf/online_planning_example.urdf index b43aaf3f896..e11581d9b2a 100644 --- a/tesseract_support/urdf/online_planning_example.urdf +++ b/tesseract_support/urdf/online_planning_example.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/online_planning_example.xacro b/tesseract_support/urdf/online_planning_example.xacro index bed6bafd68e..7cf39114c14 100644 --- a/tesseract_support/urdf/online_planning_example.xacro +++ b/tesseract_support/urdf/online_planning_example.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/pick_and_place_plan.urdf b/tesseract_support/urdf/pick_and_place_plan.urdf index a08350dbd96..09951850fd6 100644 --- a/tesseract_support/urdf/pick_and_place_plan.urdf +++ b/tesseract_support/urdf/pick_and_place_plan.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/pick_and_place_plan.xacro b/tesseract_support/urdf/pick_and_place_plan.xacro index c3c3cd8a040..0d49c48684c 100644 --- a/tesseract_support/urdf/pick_and_place_plan.xacro +++ b/tesseract_support/urdf/pick_and_place_plan.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/puzzle_piece_workcell.urdf b/tesseract_support/urdf/puzzle_piece_workcell.urdf index 98c821ca259..4d310e44f2f 100644 --- a/tesseract_support/urdf/puzzle_piece_workcell.urdf +++ b/tesseract_support/urdf/puzzle_piece_workcell.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/puzzle_piece_workcell.xacro b/tesseract_support/urdf/puzzle_piece_workcell.xacro index a15e32df584..6e7267e1127 100644 --- a/tesseract_support/urdf/puzzle_piece_workcell.xacro +++ b/tesseract_support/urdf/puzzle_piece_workcell.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/scene_graph_example.urdf b/tesseract_support/urdf/scene_graph_example.urdf index 2ecb3bf604f..3e1d766c2fe 100644 --- a/tesseract_support/urdf/scene_graph_example.urdf +++ b/tesseract_support/urdf/scene_graph_example.urdf @@ -3,7 +3,7 @@ - + diff --git a/tesseract_support/urdf/scene_graph_example.xacro b/tesseract_support/urdf/scene_graph_example.xacro index 27c34da2a6f..90b6ff75282 100644 --- a/tesseract_support/urdf/scene_graph_example.xacro +++ b/tesseract_support/urdf/scene_graph_example.xacro @@ -1,5 +1,5 @@ - + diff --git a/tesseract_support/urdf/sia20d.xacro b/tesseract_support/urdf/sia20d.xacro index 567e5bd451a..b0b90c45142 100644 --- a/tesseract_support/urdf/sia20d.xacro +++ b/tesseract_support/urdf/sia20d.xacro @@ -1,6 +1,6 @@ - + diff --git a/tesseract_urdf/CMakeLists.txt b/tesseract_urdf/CMakeLists.txt index 9e2432eda1f..c6bc909c6f0 100644 --- a/tesseract_urdf/CMakeLists.txt +++ b/tesseract_urdf/CMakeLists.txt @@ -50,7 +50,6 @@ add_library( src/capsule.cpp src/collision.cpp src/cone.cpp - src/convex_mesh.cpp src/cylinder.cpp src/dynamics.cpp src/geometry.cpp diff --git a/tesseract_urdf/include/tesseract_urdf/box.h b/tesseract_urdf/include/tesseract_urdf/box.h index 26df475c380..33ba05457c4 100644 --- a/tesseract_urdf/include/tesseract_urdf/box.h +++ b/tesseract_urdf/include/tesseract_urdf/box.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,12 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..4cef5694ab7 100644 --- a/tesseract_urdf/include/tesseract_urdf/calibration.h +++ b/tesseract_urdf/include/tesseract_urdf/calibration.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..9eaf7678166 100644 --- a/tesseract_urdf/include/tesseract_urdf/capsule.h +++ b/tesseract_urdf/include/tesseract_urdf/capsule.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,12 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..35f6424dc67 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -29,7 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,16 +43,18 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view COLLISION_ELEMENT_NAME = "collision"; + /** * @brief Parse xml element collision * @param xml_element The xml element * @param locator The Tesseract resource locator - * @param version The version number + * @param make_convex_meshes Flag to indicate if the meshes should be converted to convex hulls * @return A Collision object */ std::shared_ptr parseCollision(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - int version); + bool make_convex_meshes); /** * @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..9967467c9d7 100644 --- a/tesseract_urdf/include/tesseract_urdf/cone.h +++ b/tesseract_urdf/include/tesseract_urdf/cone.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,12 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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 deleted file mode 100644 index 64d10dc69ef..00000000000 --- a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file convex_mesh.h - * @brief Parse convex_mesh from xml string - * - * @author Levi Armstrong - * @date September 1, 2019 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_URDF_CONVEX_MESH_H -#define TESSERACT_URDF_CONVEX_MESH_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_urdf -{ -/** - * @brief Parse xml element convex_mesh - * @param xml_element The xml element - * @param locator The Tesseract resource - * @param visual Indicate if it visual - * @param version The version number - * @return A Tesseract Geometry ConvexMesh - */ -std::vector> -parseConvexMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version); - -/** - * @brief writeConvexMesh Write convex mesh to URDF XML. This is non-standard URDF / tesseract-exclusive - * @param mesh Mesh to be written - * @param doc XML document to which xml will belong - * @param package_path ///. If set, geometry will be saved relative to the package. If not - * set, geometry will be saved with absolute paths. - * @param filename Desired file location. If package_path is set, this should be relative to the package. If not, - * this should be an absolute path. - * @return XML element representing the convex mesh object in URDF format. - */ -tinyxml2::XMLElement* writeConvexMesh(const std::shared_ptr& mesh, - tinyxml2::XMLDocument& doc, - const std::string& package_path, - const std::string& filename); - -} // namespace tesseract_urdf - -#endif // TESSERACT_URDF_CONVEX_MESH_H diff --git a/tesseract_urdf/include/tesseract_urdf/cylinder.h b/tesseract_urdf/include/tesseract_urdf/cylinder.h index 6053a5768ca..30d959cf44e 100644 --- a/tesseract_urdf/include/tesseract_urdf/cylinder.h +++ b/tesseract_urdf/include/tesseract_urdf/cylinder.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,12 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..4d2ff1d2de6 100644 --- a/tesseract_urdf/include/tesseract_urdf/dynamics.h +++ b/tesseract_urdf/include/tesseract_urdf/dynamics.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..ee9f453c7d4 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -29,7 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,18 +43,20 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view GEOMETRY_ELEMENT_NAME = "geometry"; + /** * @brief Parse xml element geometry * @param xml_element The xml element * @param locator The Tesseract resource locator * @param visual Indicate if visual - * @param version The version number + * @param make_convex_meshes Flag to indicate if the meshes should be converted to convex hulls * @return A Tesseract Geometry */ std::shared_ptr parseGeometry(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual, - int version); + bool make_convex_meshes); /** * @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..6d143c19e2f 100644 --- a/tesseract_urdf/include/tesseract_urdf/inertial.h +++ b/tesseract_urdf/include/tesseract_urdf/inertial.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..9f3d67869ec 100644 --- a/tesseract_urdf/include/tesseract_urdf/joint.h +++ b/tesseract_urdf/include/tesseract_urdf/joint.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..08021388548 100644 --- a/tesseract_urdf/include/tesseract_urdf/limits.h +++ b/tesseract_urdf/include/tesseract_urdf/limits.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..0cf7f75602a 100644 --- a/tesseract_urdf/include/tesseract_urdf/link.h +++ b/tesseract_urdf/include/tesseract_urdf/link.h @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,19 +44,21 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view LINK_ELEMENT_NAME = "link"; + /** * @brief Parse xml element link * @param xml_element The xml element * @param locator The Tesseract resource locator * @param available_materials The current available materials - * @param version The version number + * @param make_convex_meshes Flag to indicate if the meshes should be converted to convex hulls * @return A Tesseract Link */ std::shared_ptr parseLink(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - std::unordered_map>& available_materials, - int version); + bool make_convex_meshes, + 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..33adf488028 100644 --- a/tesseract_urdf/include/tesseract_urdf/material.h +++ b/tesseract_urdf/include/tesseract_urdf/material.h @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view MATERIAL_ELEMENT_NAME = "material"; + /** * @brief Parse xml element material * @param xml_element The xml element @@ -53,8 +56,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..4691bcf7057 100644 --- a/tesseract_urdf/include/tesseract_urdf/mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/mesh.h @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,18 +44,21 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view MESH_ELEMENT_NAME = "mesh"; + /** * @brief Parse xml element mesh * @param xml_element The xml element * @param locator The Tesseract resource locator * @param visual Indicate if visual - * @param version The version number + * @param make_convex Flag to indicate if the mesh should be converted to a convex hull * @return A vector of Tesseract Meshes */ -std::vector> parseMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version); +std::vector> +parseMesh(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual, + bool make_convex); /** * @brief writeMesh Write a mesh to URDF XML and PLY file @@ -66,7 +70,7 @@ std::vector> parseMesh(const tinyxml2: * should be an absolute path * @return XML element representing the mesh object in URDF format. */ -tinyxml2::XMLElement* writeMesh(const std::shared_ptr& mesh, +tinyxml2::XMLElement* writeMesh(const std::shared_ptr& mesh, tinyxml2::XMLDocument& doc, const std::string& package_path, const std::string& filename); diff --git a/tesseract_urdf/include/tesseract_urdf/mimic.h b/tesseract_urdf/include/tesseract_urdf/mimic.h index fc11c4a3f57..9f9853826d3 100644 --- a/tesseract_urdf/include/tesseract_urdf/mimic.h +++ b/tesseract_urdf/include/tesseract_urdf/mimic.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,13 +42,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..3abc6df7823 100644 --- a/tesseract_urdf/include/tesseract_urdf/octomap.h +++ b/tesseract_urdf/include/tesseract_urdf/octomap.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view OCTOMAP_ELEMENT_NAME = "tesseract:octomap"; + /** * @brief Parse xml element octomap * @param xml_element The xml element @@ -52,8 +55,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..3d52309bc95 100644 --- a/tesseract_urdf/include/tesseract_urdf/octree.h +++ b/tesseract_urdf/include/tesseract_urdf/octree.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view OCTREE_ELEMENT_NAME = "tesseract:octree"; + /** * @brief Parse xml element octree * @param xml_element The xml element @@ -53,8 +56,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..13a818d70e1 100644 --- a/tesseract_urdf/include/tesseract_urdf/origin.h +++ b/tesseract_urdf/include/tesseract_urdf/origin.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -39,13 +40,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..a392a3e01de 100644 --- a/tesseract_urdf/include/tesseract_urdf/point_cloud.h +++ b/tesseract_urdf/include/tesseract_urdf/point_cloud.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view POINT_CLOUD_ELEMENT_NAME = "tesseract:point_cloud"; + /** * @brief Parse xml element point_cloud * @param xml_element The xml element @@ -54,8 +57,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..e2f65350fc7 100644 --- a/tesseract_urdf/include/tesseract_urdf/safety_controller.h +++ b/tesseract_urdf/include/tesseract_urdf/safety_controller.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,14 +42,15 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..d5700cae010 100644 --- a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +43,8 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view SDF_MESH_ELEMENT_NAME = "tesseract:sdf_mesh"; + /** * @brief Parse xml element sdf_mesh * @param xml_element The xml element @@ -52,8 +55,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..099275391b0 100644 --- a/tesseract_urdf/include/tesseract_urdf/sphere.h +++ b/tesseract_urdf/include/tesseract_urdf/sphere.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,12 +42,14 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view 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..0a8d78ffef6 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -30,7 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -44,18 +44,18 @@ class XMLDocument; namespace tesseract_urdf { +static constexpr std::string_view VISUAL_ELEMENT_NAME = "visual"; + /** * @brief Parse xml element visual * @param xml_element The xml element * @param locator The Tesseract resource locator - * @param version The version number * @return A Visual object */ 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..851289781ed 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.data()); 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..d1580fdac59 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.data()); 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..585d2034fc0 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.data()); 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..7e7735e7adb 100644 --- a/tesseract_urdf/src/collision.cpp +++ b/tesseract_urdf/src/collision.cpp @@ -40,9 +40,11 @@ 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, + bool make_convex_meshes) { // get name std::string collision_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -54,7 +56,7 @@ tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyx { try { - collision_origin = parseOrigin(origin, version); + collision_origin = parseOrigin(origin); } catch (...) { @@ -70,7 +72,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, make_convex_meshes); } catch (...) { @@ -85,17 +87,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.data()); if (!collision->name.empty()) xml_element->SetAttribute("name", collision->name.c_str()); @@ -133,3 +134,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.data()); 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 deleted file mode 100644 index bba577d008e..00000000000 --- a/tesseract_urdf/src/convex_mesh.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/** - * @file convex_mesh.cpp - * @brief Parse convex_mesh from xml string - * - * @author Levi Armstrong - * @date September 1, 2019 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include - -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include -#include - -std::vector -tesseract_urdf::parseConvexMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int /*version*/) -{ - std::vector meshes; - - std::string filename; - if (tesseract_common::QueryStringAttribute(xml_element, "filename", filename) != tinyxml2::XML_SUCCESS) - std::throw_with_nested(std::runtime_error("ConvexMesh: Missing or failed parsing attribute 'filename'!")); - - std::string scale_string; - Eigen::Vector3d scale(1, 1, 1); - if (tesseract_common::QueryStringAttribute(xml_element, "scale", scale_string) == tinyxml2::XML_SUCCESS) - { - std::vector tokens; - boost::split(tokens, scale_string, boost::is_any_of(" "), boost::token_compress_on); - if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens)) - std::throw_with_nested(std::runtime_error("ConvexMesh: Failed parsing attribute 'scale'!")); - - double sx{ 0 }, sy{ 0 }, sz{ 0 }; - // No need to check return values because the tokens are verified above - tesseract_common::toNumeric(tokens[0], sx); - tesseract_common::toNumeric(tokens[1], sy); - tesseract_common::toNumeric(tokens[2], sz); - - if (!(sx > 0)) - std::throw_with_nested(std::runtime_error("ConvexMesh: Scale x must be greater than zero!")); - - if (!(sy > 0)) - std::throw_with_nested(std::runtime_error("ConvexMesh: Scale y must be greater than zero!")); - - if (!(sz > 0)) - std::throw_with_nested(std::runtime_error("ConvexMesh: Scale z must be greater than zero!")); - - scale = Eigen::Vector3d(sx, sy, sz); - } - - bool convert = false; - xml_element->QueryBoolAttribute("convert", &convert); - - if (visual) - meshes = tesseract_geometry::createMeshFromResource( - locator.locateResource(filename), scale, true, true, true, true, true); - else - { - if (!convert) - { - meshes = tesseract_geometry::createMeshFromResource( - locator.locateResource(filename), scale, false, false); - } - else - { - std::vector temp_meshes = - tesseract_geometry::createMeshFromResource( - locator.locateResource(filename), scale, true, false); - for (auto& mesh : temp_meshes) - { - auto convex_mesh = tesseract_collision::makeConvexMesh(*mesh); - convex_mesh->setCreationMethod(tesseract_geometry::ConvexMesh::CONVERTED); - meshes.push_back(convex_mesh); - } - } - } - - if (meshes.empty()) - std::throw_with_nested(std::runtime_error("ConvexMesh: Error importing meshes from filename: '" + filename + "'!")); - - return meshes; -} - -tinyxml2::XMLElement* tesseract_urdf::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"); - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - try - { - writeMeshToFile(mesh, trailingSlash(package_path) + noLeadingSlash(filename)); - } - catch (...) - { - std::throw_with_nested(std::runtime_error("Failed to write convex mesh to file: " + package_path + filename)); - } - - // Write the path to the xml element - xml_element->SetAttribute("filename", makeURDFFilePath(package_path, filename).c_str()); - - // Write the scale to the xml element - if (!mesh->getScale().isOnes()) - { - std::stringstream scale_string; - scale_string << mesh->getScale().format(eigen_format); - xml_element->SetAttribute("scale", scale_string.str().c_str()); - } - - xml_element->SetAttribute("convert", false); - - return xml_element; -} diff --git a/tesseract_urdf/src/cylinder.cpp b/tesseract_urdf/src/cylinder.cpp index f7a0b89c957..410d3b41619 100644 --- a/tesseract_urdf/src/cylinder.cpp +++ b/tesseract_urdf/src/cylinder.cpp @@ -35,8 +35,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #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.data()); 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..f666a29d827 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.data()); 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..08d483b496a 100644 --- a/tesseract_urdf/src/geometry.cpp +++ b/tesseract_urdf/src/geometry.cpp @@ -39,7 +39,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include #include #include #include @@ -47,10 +46,12 @@ 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, + bool make_convex_meshes) { 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,111 +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, make_convex_meshes); } 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 (geometry_type == "capsule") - { - tesseract_geometry::Capsule::Ptr capsule; - try - { - capsule = parseCapsule(geometry, version); - } - catch (...) - { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'capsule'!")); - } + if (meshes.size() > 1) + return std::make_shared(meshes); - return capsule; + return meshes.front(); } - if (geometry_type == "octomap") + // Custom Tesseract elements + if (geometry_type == CONE_ELEMENT_NAME) { - tesseract_geometry::Octree::Ptr octree; + tesseract_geometry::Cone::Ptr cone; try { - octree = parseOctomap(geometry, locator, visual, version); + cone = parseCone(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 'cone'!")); } - return octree; + return cone; } - if (geometry_type == "mesh") + if (geometry_type == CAPSULE_ELEMENT_NAME) { - std::vector meshes; + tesseract_geometry::Capsule::Ptr capsule; try { - meshes = parseMesh(geometry, locator, visual, version); + capsule = parseCapsule(geometry); } 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 'capsule'!")); } - if (version < 2 && !visual) - return tesseract_collision::makeConvexMesh(*meshes.front()); - - return meshes.front(); + return capsule; } - if (geometry_type == "convex_mesh") + if (geometry_type == OCTOMAP_ELEMENT_NAME) { - std::vector meshes; + tesseract_geometry::Octree::Ptr octree; try { - meshes = parseConvexMesh(geometry, locator, visual, version); + octree = parseOctomap(geometry, locator, visual); } catch (...) { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'convex_mesh'!")); + std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'octomap'!")); } - if (meshes.size() > 1) - return std::make_shared(meshes); - - return meshes.front(); + return octree; } - 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 +193,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.data()); tesseract_geometry::GeometryType type = geometry->getType(); @@ -309,8 +276,11 @@ tinyxml2::XMLElement* tesseract_urdf::writeGeometry(const std::shared_ptr(geometry), doc, package_path, filename + ".ply"); + tinyxml2::XMLElement* xml_mesh = + writeMesh(std::static_pointer_cast(geometry), + doc, + package_path, + filename + ".ply"); xml_element->InsertEndChild(xml_mesh); } catch (...) @@ -318,22 +288,6 @@ tinyxml2::XMLElement* tesseract_urdf::writeGeometry(const std::shared_ptr(geometry), - doc, - package_path, - filename + ".ply"); - xml_element->InsertEndChild(xml_convex_mesh); - } - catch (...) - { - std::throw_with_nested(std::runtime_error("Could not write geometry marked as convex mesh!")); - } - } else if (type == tesseract_geometry::GeometryType::SDF_MESH) { try @@ -367,3 +321,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.data()); 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..b5cb9d491ed 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.data()); // 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.data()); // 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, + bool make_convex_meshes, + std::unordered_map& available_materials) { std::string link_name; if (tesseract_common::QueryStringAttribute(xml_element, "name", link_name) != tinyxml2::XML_SUCCESS) @@ -59,7 +61,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, { try { - l->inertial = parseInertial(inertial, version); + l->inertial = parseInertial(inertial); } catch (...) { @@ -75,7 +77,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 +94,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, make_convex_meshes); } catch (...) { @@ -106,13 +108,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.data()); // Set name xml_element->SetAttribute("name", link->getName().c_str()); @@ -163,3 +165,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.data()); 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 TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include #include #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, + bool make_convex) { - std::vector meshes; - std::string filename; if (tesseract_common::QueryStringAttribute(xml_element, "filename", filename) != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Mesh: Missing or failed parsing attribute 'filename'!")); @@ -79,6 +80,8 @@ std::vector tesseract_urdf::parseMesh(const tinyx scale = Eigen::Vector3d(sx, sy, sz); } + std::vector meshes; + if (visual) meshes = tesseract_geometry::createMeshFromResource( locator.locateResource(filename), scale, true, true, true, true, true); @@ -89,17 +92,50 @@ std::vector tesseract_urdf::parseMesh(const tinyx if (meshes.empty()) std::throw_with_nested(std::runtime_error("Mesh: Error importing meshes from filename: '" + filename + "'!")); - return meshes; + bool make_convex_override = false; + auto make_convex_override_status = xml_element->QueryBoolAttribute("tesseract:make_convex", &make_convex_override); + if (make_convex_override_status != tinyxml2::XML_NO_ATTRIBUTE) + { + // Make convex override attribute is specified + // Check that it was loaded successfully + if (make_convex_override_status != tinyxml2::XML_SUCCESS) + std::throw_with_nested(std::runtime_error("Mesh: Failed to parse attribute 'tesseract:make_convex'")); + + // Override the global make_convex flag with the value from the attribute + make_convex = make_convex_override; + } + + if (make_convex) + { + std::vector convex_meshes; + convex_meshes.reserve(meshes.size()); + + for (const auto& mesh : meshes) + { + tesseract_geometry::ConvexMesh::Ptr convex_mesh = tesseract_collision::makeConvexMesh(*mesh); + convex_mesh->setCreationMethod(tesseract_geometry::ConvexMesh::CONVERTED); + convex_meshes.push_back(convex_mesh); + } + + return convex_meshes; + } + + // Convert to base class for output + std::vector output; + output.reserve(meshes.size()); + std::copy(meshes.begin(), meshes.end(), std::back_inserter(output)); + + return output; } -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.data()); Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); try @@ -119,5 +155,15 @@ tinyxml2::XMLElement* tesseract_urdf::writeMesh(const std::shared_ptrSetAttribute("scale", scale_string.str().c_str()); } + // If the mesh is actually a convex mesh, set the `tesseract:make_convex` attribute true. + // The geometry itself is already convex, so telling Tesseract to make it convex won't change the geometry. + // However, it will make sure it gets added to the environment as a `ConvexMesh` shape instead of a `Mesh` shape. + if (std::dynamic_pointer_cast(mesh)) + { + xml_element->SetAttribute("tesseract:make_convex", true); + } + return xml_element; } + +} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/mimic.cpp b/tesseract_urdf/src/mimic.cpp index b36bec0a71d..2e155664fc0 100644 --- a/tesseract_urdf/src/mimic.cpp +++ b/tesseract_urdf/src/mimic.cpp @@ -37,8 +37,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #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.data()); 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.data()); 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.data()); 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.data()); 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.data()); 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..b897bdc726c 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.data()); 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..64811e43594 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.data()); 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.data()); 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.data()); 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 514c8e3931a..05a4da8d52b 100644 --- a/tesseract_urdf/src/urdf_parser.cpp +++ b/tesseract_urdf/src/urdf_parser.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -45,6 +46,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +static constexpr std::string_view ROBOT_ELEMENT_NAME = "robot"; + namespace tesseract_urdf { std::unique_ptr parseURDFString(const std::string& urdf_xml_string, @@ -54,7 +57,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.data()); if (robot == nullptr) std::throw_with_nested(std::runtime_error("URDF: Missing element 'robot'!")); @@ -69,29 +72,47 @@ 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.")); + + // Check for global attribute for converting meshes to convex hulls + bool make_convex = false; + auto make_convex_status = robot->QueryBoolAttribute("tesseract:make_convex", &make_convex); + switch (make_convex_status) + { + case tinyxml2::XML_SUCCESS: + break; + case tinyxml2::XML_NO_ATTRIBUTE: + { + const std::string message = "URDF: missing boolean attribute 'tesseract:make_convex'. This attribute indicates " + "whether Tesseract should globally convert all collision mesh geometries into convex " + "hulls. Previous versions of Tesseract performed this conversion automatically " + "(i.e., 'tesseract:make_convex=\"true\"'. If you want to perform collision checking " + "with detailed meshes instead of convex hulls, set " + "'tesseract:make_convex=\"false\"'. This global attribute can be overriden on a " + "per-mesh basis by specifying the 'tesseract:make_convex' attribute in the 'mesh' " + "element (e.g., ."; + std::throw_with_nested(std::runtime_error(message)); + } + default: + std::throw_with_nested(std::runtime_error("URDF: Failed to parse boolean attribute 'tesseract:make_convex' for " + "robot '" + + robot_name + "'")); + } 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.data()); material != nullptr; + material = material->NextSiblingElement(MATERIAL_ELEMENT_NAME.data())) { 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 +123,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.data()); link != nullptr; + link = link->NextSiblingElement(LINK_ELEMENT_NAME.data())) { tesseract_scene_graph::Link::Ptr l = nullptr; try { - l = parseLink(link, locator, available_materials, tesseract_urdf_version); + l = parseLink(link, locator, make_convex, available_materials); } catch (...) { @@ -129,14 +150,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.data()); joint != nullptr; + joint = joint->NextSiblingElement(JOINT_ELEMENT_NAME.data())) { tesseract_scene_graph::Joint::Ptr j = nullptr; try { - j = parseJoint(joint, tesseract_urdf_version); + j = parseJoint(joint); } catch (...) { @@ -220,8 +240,9 @@ void writeURDFFile(const std::shared_ptrSetAttribute("name", sg->getName().c_str()); + xml_robot->SetAttribute("tesseract:make_convex", false); // version? doc.InsertEndChild(xml_robot); diff --git a/tesseract_urdf/src/visual.cpp b/tesseract_urdf/src/visual.cpp index 1f7b0e6540a..d95ed28a969 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,9 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, tesseract_geometry::Geometry::Ptr geom; try { - geom = parseGeometry(geometry, locator, true, version); + // Set `make_convex_meshes` argument to false for visual geometry + // Note: mesh elements can still override + geom = parseGeometry(geometry, locator, true, false); } catch (...) { @@ -104,16 +107,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.data()); if (!visual->name.empty()) xml_element->SetAttribute("name", visual->name.c_str()); @@ -160,3 +163,5 @@ tinyxml2::XMLElement* tesseract_urdf::writeVisual(const std::shared_ptr)"; 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp b/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp index 4f0f7dd9658..5a716d455d2 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp b/tesseract_urdf/test/tesseract_urdf_capsule_unit.cpp index 4565dd93a88..ae3404b231a 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.data())); 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.data())); 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.data())); } { - 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.data())); } { - 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.data())); } { - 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.data())); } // 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.data()); // 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.data())); } { - 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.data())); } { - 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp index 668da446887..8116986a430 100644 --- a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp @@ -13,6 +13,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_collision) // NOLINT { tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = false; + const auto parse_collision_fn = [&](const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator) { + return tesseract_urdf::parseCollision(xml_element, locator, global_make_convex); + }; { std::string str = R"( @@ -23,7 +28,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), resource_locator)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_FALSE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } @@ -36,7 +41,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), resource_locator)); EXPECT_TRUE(elem->geometry != nullptr); EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } @@ -49,7 +54,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), 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 +70,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), resource_locator)); EXPECT_TRUE(elem == nullptr); } @@ -77,7 +82,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), resource_locator)); EXPECT_TRUE(elem == nullptr); } @@ -86,7 +91,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, parse_collision_fn, str, tesseract_urdf::COLLISION_ELEMENT_NAME.data(), 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..5262ecbea0f 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.data())); 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.data())); 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.data())); } { - 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.data())); } { - 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.data())); } { - 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.data())); } // 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.data()); // 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.data())); } { - 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.data())); } { - 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp deleted file mode 100644 index b4e8c53e0b2..00000000000 --- a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include "tesseract_urdf_common_unit.h" - -static std::string getTempPkgPath() -{ - std::string tmp = tesseract_common::getTempPath(); - std::string tmppkg = tmp + "tmppkg"; - if (!std::filesystem::is_directory(tmppkg) || !std::filesystem::exists(tmppkg)) - { - std::filesystem::create_directory(tmppkg); - } - return tmppkg; -} - -TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT -{ - tesseract_common::GeneralResourceLocator resource_locator; - { - std::string str = - R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.size() == 1); - EXPECT_TRUE(geom[0]->getFaceCount() == 6); - EXPECT_TRUE(geom[0]->getVertexCount() == 8); - EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); - } - - { - std::string str = - R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, true)); - EXPECT_TRUE(geom.size() == 1); - EXPECT_TRUE(geom[0]->getFaceCount() == 12); - EXPECT_TRUE(geom[0]->getVertexCount() == 8); - EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); - } - - { - std::string str = - R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, 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. - EXPECT_TRUE(geom[0]->getVertexCount() == 8); - } - - { - std::string str = - R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.size() == 2); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.size() == 1); - EXPECT_TRUE(geom[0]->getFaceCount() == 6); - EXPECT_TRUE(geom[0]->getVertexCount() == 8); - EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5); - EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } - - { - std::string str = ""; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseConvexMesh, str, "convex_mesh", resource_locator, 2, false)); - EXPECT_TRUE(geom.empty()); - } -} - -TEST(TesseractURDFUnit, write_convex_mesh) // NOLINT -{ - { - tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0), - Eigen::Vector3d(1, 0, 0), - Eigen::Vector3d(0, 1, 0) }; - Eigen::VectorXi indices(4); - indices << 3, 0, 1, 2; - tesseract_geometry::ConvexMesh::Ptr convex_mesh = std::make_shared( - std::make_shared(vertices), std::make_shared(indices)); - std::string text; - EXPECT_EQ(0, - writeTest( - convex_mesh, &tesseract_urdf::writeConvexMesh, text, getTempPkgPath(), std::string("convex0.ply"))); - EXPECT_EQ(text, R"()"); - } - - { - tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0), - Eigen::Vector3d(1, 0, 0), - Eigen::Vector3d(0, 1, 0) }; - Eigen::VectorXi indices(4); - indices << 3, 0, 1, 2; - tesseract_geometry::ConvexMesh::Ptr convex_mesh = std::make_shared( - std::make_shared(vertices), std::make_shared(indices)); - std::string text; - EXPECT_EQ( - 1, - writeTest( - convex_mesh, &tesseract_urdf::writeConvexMesh, text, tesseract_common::getTempPath(), std::string(""))); - EXPECT_EQ(text, ""); - } - - { - tesseract_geometry::ConvexMesh::Ptr convex_mesh = nullptr; - std::string text; - EXPECT_EQ(1, - writeTest(convex_mesh, - &tesseract_urdf::writeConvexMesh, - text, - tesseract_common::getTempPath(), - std::string("convex1.ply"))); - EXPECT_EQ(text, ""); - } -} diff --git a/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp b/tesseract_urdf/test/tesseract_urdf_cylinder_unit.cpp index 8f0df432950..8b61f61e305 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.data())); 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.data())); EXPECT_NEAR(geom->getRadius(), 0.25, 1e-8); EXPECT_NEAR(geom->getLength(), 0.5, 1e-8); } @@ -29,52 +31,58 @@ 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } // 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); - // EXPECT_FALSE(*status); - // EXPECT_FALSE(status->message().empty()); + // auto status = runTest(geom, str, + // tesseract_urdf::CYLINDER_ELEMENT_NAME.data()); EXPECT_FALSE(*status); EXPECT_FALSE(status->message().empty()); // } { 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp b/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp index d3ee8c2b309..c78b11194aa 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } } 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..70c15e8ebfe 100644 --- a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp @@ -12,6 +12,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_geometry) // NOLINT { + const bool global_make_convex = false; + const auto parse_geometry_fn = [&](const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + const bool visual) { + return tesseract_urdf::parseGeometry(xml_element, locator, visual, global_make_convex); + }; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"( @@ -19,7 +26,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::BOX); } @@ -29,7 +36,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SPHERE); } @@ -39,69 +46,59 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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)); - EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CONVEX_MESH); - } - { std::string str = R"( )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_TRUE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); + elem, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SDF_MESH); } @@ -111,7 +108,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -120,7 +117,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -130,7 +127,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -140,7 +137,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -150,49 +147,39 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; tesseract_geometry::Geometry::Ptr elem; EXPECT_FALSE(runTest( - elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } @@ -202,17 +189,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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), 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, parse_geometry_fn, str, tesseract_urdf::GEOMETRY_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(elem == nullptr); } } @@ -303,23 +290,6 @@ TEST(TesseractURDFUnit, write_geometry) // NOLINT EXPECT_NE(text, ""); } - { // convex_mesh - tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0), - Eigen::Vector3d(1, 0, 0), - Eigen::Vector3d(0, 1, 0) }; - Eigen::VectorXi indices(4); - indices << 3, 0, 1, 2; - tesseract_geometry::Geometry::Ptr geometry = std::make_shared( - std::make_shared(vertices), std::make_shared(indices)); - - std::string text; - EXPECT_EQ( - 0, - writeTest( - geometry, &tesseract_urdf::writeGeometry, text, tesseract_common::getTempPath(), std::string("geom1"))); - EXPECT_NE(text, ""); - } - { // sdf_mesh tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1, 0, 0), diff --git a/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp b/tesseract_urdf/test/tesseract_urdf_inertial_unit.cpp index a1e0ce06c4b..cfc4a8e5925 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.data())); 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.data())); 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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp index 82fdb39447a..e74e33a6f89 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } { @@ -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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp b/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp index 099141cfb37..20dce4501fe 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp index 3c8aaae12b6..152a0df4708 100644 --- a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp @@ -13,6 +13,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_link) // NOLINT { tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = false; + const auto parse_link_fn = + [&](const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + std::unordered_map>& available_materials) { + return tesseract_urdf::parseLink(xml_element, locator, global_make_convex, available_materials); + }; { std::unordered_map empty_available_materials; @@ -39,8 +46,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial != nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -88,8 +99,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial != nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -117,8 +132,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -159,8 +178,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -182,8 +205,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 1); @@ -212,8 +239,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.size() == 2); @@ -232,8 +263,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -258,8 +293,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -271,8 +310,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); EXPECT_TRUE(elem->getName() == "my_link"); EXPECT_TRUE(elem->inertial == nullptr); EXPECT_TRUE(elem->visual.empty()); @@ -293,8 +336,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); } { @@ -311,8 +358,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); } { @@ -330,8 +381,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + resource_locator, + empty_available_materials)); } { @@ -345,15 +400,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, + parse_link_fn, + str, + tesseract_urdf::LINK_ELEMENT_NAME.data(), + 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.data()); link->inertial = std::make_shared(); tesseract_scene_graph::Collision::Ptr collision = std::make_shared(); @@ -378,7 +438,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.data()); link->inertial = std::make_shared(); link->collision.push_back(nullptr); @@ -397,7 +458,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.data()); 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..8ef25922d5b 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.data(), + 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.data(), + 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()); @@ -56,8 +64,12 @@ 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)); + EXPECT_TRUE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME.data(), + 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 +85,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.data(), + empty_available_materials, + true)); } { @@ -86,8 +102,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.data(), + empty_available_materials, + true)); } { @@ -97,8 +117,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", available_materials, 2, true)); + EXPECT_FALSE(runTest(elem, + &tesseract_urdf::parseMaterial, + str, + tesseract_urdf::MATERIAL_ELEMENT_NAME.data(), + available_materials, + true)); } { @@ -108,8 +132,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.data(), + empty_available_materials, + true)); } { @@ -123,8 +151,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.data(), + empty_available_materials, + true)); } { @@ -138,8 +170,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.data(), + empty_available_materials, + true)); } { @@ -153,8 +189,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.data(), + empty_available_materials, + true)); } { @@ -168,8 +208,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.data(), + empty_available_materials, + true)); } { @@ -183,8 +227,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.data(), + empty_available_materials, + true)); } { @@ -198,8 +246,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.data(), + 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..33b5142be5c 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp @@ -13,11 +13,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP TEST(TesseractURDFUnit, parse_mesh_material_dae) // NOLINT { tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = false; + const auto parse_mesh_fn = + [&](const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual) { + return tesseract_urdf::parseMesh(xml_element, locator, visual, global_make_convex); + }; + { std::string str = R"()"; - std::vector meshes; - EXPECT_TRUE(runTest>( - meshes, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector meshes; + EXPECT_TRUE(runTest>( + meshes, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(meshes.size() == 4); auto& mesh0 = meshes[1]; auto& mesh1 = meshes[2]; @@ -87,11 +93,17 @@ TEST(TesseractURDFUnit, parse_mesh_material_dae) // NOLINT TEST(TesseractURDFUnit, parse_mesh_material_gltf2) // NOLINT { tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = false; + const auto parse_mesh_fn = + [&](const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual) { + return tesseract_urdf::parseMesh(xml_element, locator, visual, global_make_convex); + }; + { std::string str = R"()"; - std::vector meshes; - EXPECT_TRUE(runTest>( - meshes, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector meshes; + EXPECT_TRUE(runTest>( + meshes, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), 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 bc85e1210f1..76c5b212fc6 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp @@ -6,6 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include "tesseract_urdf_common_unit.h" @@ -23,15 +24,21 @@ static std::string getTempPkgPath() TEST(TesseractURDFUnit, parse_mesh) // NOLINT { tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = false; + const auto parse_mesh_fn = + [&](const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual) { + return tesseract_urdf::parseMesh(xml_element, locator, visual, global_make_convex); + }; + { std::string str = R"()"; - std::vector geom; - EXPECT_TRUE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.size() == 1); - EXPECT_TRUE(geom[0]->getFaceCount() == 80); - EXPECT_TRUE(geom[0]->getVertexCount() == 240); + EXPECT_EQ(geom[0]->getFaceCount(), 80); + EXPECT_EQ(geom[0]->getVertexCount(), 240); EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); @@ -39,70 +46,110 @@ 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)); + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_EQ(geom[0]->getFaceCount(), 80); + EXPECT_EQ(geom[0]->getVertexCount(), 240); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + // Make convex override false (correct) + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_EQ(geom[0]->getFaceCount(), 80); + EXPECT_EQ(geom[0]->getVertexCount(), 240); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + // Make convex override true (correct) + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.size() == 1); - EXPECT_TRUE(geom[0]->getFaceCount() == 80); - EXPECT_TRUE(geom[0]->getVertexCount() == 240); + EXPECT_LE(geom[0]->getFaceCount(), 80); + EXPECT_LE(geom[0]->getVertexCount(), 240); EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5); EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); } + // Make convex (incorrect) + { + std::string str = + R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); + EXPECT_TRUE(geom.empty()); + } + { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = R"()"; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } { std::string str = ""; - std::vector geom; - EXPECT_FALSE(runTest>( - geom, &tesseract_urdf::parseMesh, str, "mesh", resource_locator, 2, true)); + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, true)); EXPECT_TRUE(geom.empty()); } } @@ -148,3 +195,201 @@ TEST(TesseractURDFUnit, write_mesh) // NOLINT EXPECT_EQ(text, ""); } } + +TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT +{ + tesseract_common::GeneralResourceLocator resource_locator; + const bool global_make_convex = true; + const auto parse_mesh_fn = + [&](const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, bool visual) { + return tesseract_urdf::parseMesh(xml_element, locator, visual, global_make_convex); + }; + + { + std::string str = R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_EQ(geom[0]->getFaceCount(), 7); + EXPECT_EQ(geom[0]->getVertexCount(), 8); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + // Global and local override set to make convex mesh + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_EQ(geom[0]->getFaceCount(), 7); + EXPECT_EQ(geom[0]->getVertexCount(), 8); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + // Global and local override set to make convex mesh + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_EQ(geom[0]->getFaceCount(), 12); + EXPECT_EQ(geom[0]->getVertexCount(), 8); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 2, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), 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. + EXPECT_TRUE(geom[0]->getVertexCount() == 8); + } + + { + std::string str = + R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.size() == 2); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_TRUE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.size() == 1); + EXPECT_TRUE(geom[0]->getFaceCount() == 7); + EXPECT_TRUE(geom[0]->getVertexCount() == 8); + EXPECT_NEAR(geom[0]->getScale()[0], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[1], 1, 1e-5); + EXPECT_NEAR(geom[0]->getScale()[2], 1, 1e-5); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = R"()"; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } + + { + std::string str = ""; + std::vector geom; + EXPECT_FALSE(runTest>( + geom, parse_mesh_fn, str, tesseract_urdf::MESH_ELEMENT_NAME.data(), resource_locator, false)); + EXPECT_TRUE(geom.empty()); + } +} + +TEST(TesseractURDFUnit, write_convex_mesh) // NOLINT +{ + { + // Create an arbitrary mesh denoted specifically as a convex mesh type + tesseract_common::VectorVector3d vertices = { Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(0, 1, 0) }; + Eigen::VectorXi indices(4); + indices << 3, 0, 1, 2; + auto convex_mesh = std::make_shared( + std::make_shared(vertices), std::make_shared(indices)); + + // Write the convex mesh into a string + // Since the input type is specifically a ConvexMesh, the tesseract:make_convex` attribute should be present to + // indicate that mesh should be made convex and produce a `ConvexMesh` + std::string text; + EXPECT_EQ(0, + writeTest( + convex_mesh, &tesseract_urdf::writeMesh, text, getTempPkgPath(), std::string("convex0.ply"))); + EXPECT_EQ(text, R"()"); + } +} diff --git a/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp index 7410b96def9..2e089d976a3 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp index aecfd81199c..b46e7e241be 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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..ee68690fce2 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } { 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp b/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp index 5696f19ab69..7acfe263001 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); 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.data())); } { 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp index 25de3a4271b..c5161e01f1c 100644 --- a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp @@ -25,10 +25,14 @@ 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)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + resource_locator, + true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -38,10 +42,14 @@ 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)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + resource_locator, + true)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 240); @@ -51,10 +59,14 @@ 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)); + EXPECT_TRUE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + resource_locator, + false)); EXPECT_TRUE(geom.size() == 1); EXPECT_TRUE(geom[0]->getFaceCount() == 80); EXPECT_TRUE(geom[0]->getVertexCount() == 42); @@ -64,52 +76,84 @@ 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + 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)); + EXPECT_FALSE(runTest>(geom, + &tesseract_urdf::parseSDFMesh, + str, + tesseract_urdf::SDF_MESH_ELEMENT_NAME.data(), + resource_locator, + true)); } } @@ -127,7 +171,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..2be240e33b3 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.data())); 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.data())); 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.data())); } { 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.data())); } // 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.data()); // 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.data())); } } diff --git a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp index 5e6be4327cf..d2fde22f8b9 100644 --- a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp @@ -16,7 +16,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT tesseract_common::GeneralResourceLocator resource_locator; { std::string str = - R"( + R"( @@ -41,7 +41,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -66,7 +66,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -89,7 +89,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -112,7 +112,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -131,7 +131,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -151,7 +151,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -170,7 +170,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -190,8 +190,8 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( - + R"( + @@ -209,7 +209,7 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT { std::string str = - R"( + R"( @@ -231,6 +231,12 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT EXPECT_ANY_THROW(tesseract_urdf::parseURDFString(str, resource_locator)); // NOLINT } + // Missing tesseract:make_convex attribute + { + std::string str = R"()"; + EXPECT_ANY_THROW(tesseract_urdf::parseURDFString(str, resource_locator)); // NOLINT + } + { const std::string path = resource_locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); @@ -244,7 +250,7 @@ TEST(TesseractURDFUnit, parse_urdf_with_available_materials) // NOLINT tesseract_common::GeneralResourceLocator resource_locator; { std::string str = - R"( + R"( @@ -281,7 +287,7 @@ TEST(TesseractURDFUnit, parse_urdf_with_available_materials) // NOLINT { std::string str = - R"( + R"( @@ -325,7 +331,7 @@ TEST(TesseractURDFUnit, parse_urdf_with_available_materials) // NOLINT { std::string str = - R"( + R"( @@ -362,7 +368,7 @@ TEST(TesseractURDFUnit, parse_urdf_with_available_materials) // NOLINT { std::string str = - R"( + R"( diff --git a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp index 53df03e9be7..b515c455cda 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.data(), + 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.data(), + 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.data(), + 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.data(), + 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.data(), + resource_locator, + empty_available_materials)); } }