diff --git a/tesseract_urdf/CMakeLists.txt b/tesseract_urdf/CMakeLists.txt index 1a6f1c358f5..7a84308d57b 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/collision.h b/tesseract_urdf/include/tesseract_urdf/collision.h index d74775d65ec..547b20614d6 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -49,11 +49,12 @@ static const char* COLLISION_ELEMENT_NAME = "collision"; // NOLINT * @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); + const tesseract_common::ResourceLocator& locator, + bool make_convex_meshes); /** * @brief writeCollision Write collision object to URDF XML 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 76481b80af4..00000000000 --- a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h +++ /dev/null @@ -1,76 +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 -{ -static const char* CONVEX_MESH_ELEMENT_NAME = "tesseract:convex_mesh"; // NOLINT - -/** - * @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); - -/** - * @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/geometry.h b/tesseract_urdf/include/tesseract_urdf/geometry.h index 58029b94758..18e8bdb45e2 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -50,12 +50,13 @@ static const char* GEOMETRY_ELEMENT_NAME = "geometry"; // NOLINT * @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); + bool visual, + bool make_convex_meshes); /** * @brief writeGeometry Write geometry to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/link.h b/tesseract_urdf/include/tesseract_urdf/link.h index 6fe40a7193d..ce05f65853f 100644 --- a/tesseract_urdf/include/tesseract_urdf/link.h +++ b/tesseract_urdf/include/tesseract_urdf/link.h @@ -50,12 +50,13 @@ static const char* LINK_ELEMENT_NAME = "link"; // NOLINT * @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, + bool make_convex_meshes, std::unordered_map>& available_materials); /** diff --git a/tesseract_urdf/include/tesseract_urdf/mesh.h b/tesseract_urdf/include/tesseract_urdf/mesh.h index 6830bbc8c14..049041d07e0 100644 --- a/tesseract_urdf/include/tesseract_urdf/mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/mesh.h @@ -50,12 +50,14 @@ static const char* MESH_ELEMENT_NAME = "mesh"; // NOLINT * @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); +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 diff --git a/tesseract_urdf/include/tesseract_urdf/visual.h b/tesseract_urdf/include/tesseract_urdf/visual.h index 41c76fc625d..481696c0238 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -50,7 +50,6 @@ static const char* VISUAL_ELEMENT_NAME = "visual"; // NOLINT * @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 diff --git a/tesseract_urdf/src/collision.cpp b/tesseract_urdf/src/collision.cpp index ecea7b0b4ae..c3a5b1a25bb 100644 --- a/tesseract_urdf/src/collision.cpp +++ b/tesseract_urdf/src/collision.cpp @@ -43,7 +43,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_urdf { tesseract_scene_graph::Collision::Ptr parseCollision(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator) + const tesseract_common::ResourceLocator& locator, + bool make_convex_meshes) { // get name std::string collision_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -71,7 +72,7 @@ tesseract_scene_graph::Collision::Ptr parseCollision(const tinyxml2::XMLElement* tesseract_geometry::Geometry::Ptr geom; try { - geom = parseGeometry(geometry, locator, false); + geom = parseGeometry(geometry, locator, false, make_convex_meshes); } catch (...) { diff --git a/tesseract_urdf/src/convex_mesh.cpp b/tesseract_urdf/src/convex_mesh.cpp deleted file mode 100644 index c6487b31772..00000000000 --- a/tesseract_urdf/src/convex_mesh.cpp +++ /dev/null @@ -1,150 +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 - -namespace tesseract_urdf -{ -std::vector parseConvexMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual) -{ - 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* 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_ELEMENT_NAME); - 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; -} - -} // namespace tesseract_urdf diff --git a/tesseract_urdf/src/geometry.cpp b/tesseract_urdf/src/geometry.cpp index f71df8dc918..7fd2d7e63ab 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 @@ -51,7 +50,8 @@ namespace tesseract_urdf { tesseract_geometry::Geometry::Ptr parseGeometry(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - bool visual) + bool visual, + bool make_convex_meshes) { const tinyxml2::XMLElement* geometry = xml_element->FirstChildElement(); if (geometry == nullptr) @@ -110,10 +110,10 @@ tesseract_geometry::Geometry::Ptr parseGeometry(const tinyxml2::XMLElement* xml_ if (geometry_type == MESH_ELEMENT_NAME) { - std::vector meshes; + std::vector meshes; try { - meshes = parseMesh(geometry, locator, visual); + meshes = parseMesh(geometry, locator, visual, make_convex_meshes); } catch (...) { @@ -172,24 +172,6 @@ tesseract_geometry::Geometry::Ptr parseGeometry(const tinyxml2::XMLElement* xml_ return octree; } - if (geometry_type == CONVEX_MESH_ELEMENT_NAME) - { - std::vector meshes; - try - { - meshes = parseConvexMesh(geometry, locator, visual); - } - catch (...) - { - std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'convex_mesh'!")); - } - - if (meshes.size() > 1) - return std::make_shared(meshes); - - return meshes.front(); - } - if (geometry_type == SDF_MESH_ELEMENT_NAME) { std::vector meshes; @@ -303,22 +285,6 @@ tinyxml2::XMLElement* 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 diff --git a/tesseract_urdf/src/link.cpp b/tesseract_urdf/src/link.cpp index 09e7a08cf64..96606caede6 100644 --- a/tesseract_urdf/src/link.cpp +++ b/tesseract_urdf/src/link.cpp @@ -46,6 +46,7 @@ namespace tesseract_urdf tesseract_scene_graph::Link::Ptr parseLink(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, + bool make_convex_meshes, std::unordered_map& available_materials) { std::string link_name; @@ -93,7 +94,7 @@ parseLink(const tinyxml2::XMLElement* xml_element, tesseract_scene_graph::Collision::Ptr temp_collision; try { - temp_collision = parseCollision(collision, locator); + temp_collision = parseCollision(collision, locator, make_convex_meshes); } catch (...) { diff --git a/tesseract_urdf/src/mesh.cpp b/tesseract_urdf/src/mesh.cpp index 79e9ec90caf..4ac3e147593 100644 --- a/tesseract_urdf/src/mesh.cpp +++ b/tesseract_urdf/src/mesh.cpp @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include @@ -43,12 +44,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_urdf { -std::vector parseMesh(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual) +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'!")); @@ -80,6 +80,8 @@ std::vector parseMesh(const tinyxml2::XMLElement* scale = Eigen::Vector3d(sx, sy, sz); } + std::vector meshes; + if (visual) meshes = tesseract_geometry::createMeshFromResource( locator.locateResource(filename), scale, true, true, true, true, true); @@ -90,7 +92,40 @@ std::vector parseMesh(const tinyxml2::XMLElement* 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("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* writeMesh(const std::shared_ptr& mesh, diff --git a/tesseract_urdf/src/urdf_parser.cpp b/tesseract_urdf/src/urdf_parser.cpp index eba7a04e5aa..6412072c495 100644 --- a/tesseract_urdf/src/urdf_parser.cpp +++ b/tesseract_urdf/src/urdf_parser.cpp @@ -75,6 +75,12 @@ std::unique_ptr parseURDFString(const std::st 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; + auto make_convex_status = robot->QueryBoolAttribute("tesseract:make_convex", &make_convex); + if (make_convex_status != tinyxml2::XML_NO_ATTRIBUTE && make_convex_status != tinyxml2::XML_SUCCESS) + std::throw_with_nested("URDF: Failed to parse attribute 'tesseract:make_convex' for robot '" + robot_name + "'"); + auto sg = std::make_unique(); sg->setName(robot_name); @@ -103,7 +109,7 @@ std::unique_ptr parseURDFString(const std::st tesseract_scene_graph::Link::Ptr l = nullptr; try { - l = parseLink(link, locator, available_materials); + l = parseLink(link, locator, make_convex, available_materials); } catch (...) { diff --git a/tesseract_urdf/src/visual.cpp b/tesseract_urdf/src/visual.cpp index 47e76da0c58..12c13c5bc4e 100644 --- a/tesseract_urdf/src/visual.cpp +++ b/tesseract_urdf/src/visual.cpp @@ -89,7 +89,9 @@ parseVisual(const tinyxml2::XMLElement* xml_element, tesseract_geometry::Geometry::Ptr geom; try { - geom = parseGeometry(geometry, locator, true); + // Set `make_convex_meshes` argument to false for visual geometry + // Note: mesh elements can still override + geom = parseGeometry(geometry, locator, true, false); } catch (...) {