Added application for performing convex decomposition

tesseract_collision/vhacd/CMakeLists.txt

@@ -23,6 +23,31 @@ target_include_directories(
   ${PROJECT_NAME}_vhacd_convex_decomposition PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                     "$<INSTALL_INTERFACE:include>")
 
+if(NOT MSVC)
+  find_package(Threads REQUIRED)
+  # Create target for creating convex decompositions from meshes
+  add_executable(create_convex_decomposition src/create_convex_decomposition.cpp)
+  target_link_libraries(
+    create_convex_decomposition
+    PUBLIC ${PROJECT_NAME}_vhacd_convex_decomposition
+           Boost::boost
+           Boost::program_options
+           Eigen3::Eigen
+           tesseract::tesseract_common
+           tesseract::tesseract_geometry
+           console_bridge::console_bridge
+           Threads::Threads)
+  target_compile_options(create_convex_decomposition PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}
+                                                             ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
+  target_compile_definitions(create_convex_decomposition PRIVATE ${TESSERACT_COMPILE_DEFINITIONS})
+  target_cxx_version(create_convex_decomposition PRIVATE VERSION ${TESSERACT_CXX_VERSION})
+  target_clang_tidy(create_convex_decomposition ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
+
+  list(APPEND PACKAGE_LIBRARIES create_convex_decomposition)
+
+  install_targets(TARGETS create_convex_decomposition COMPONENT vhacd)
+endif()
+
 # Mark cpp header files for installation
 install(
   DIRECTORY include/${PROJECT_NAME}

tesseract_collision/vhacd/src/create_convex_decomposition.cpp

@@ -0,0 +1,126 @@
+/**
+ * @file Create a convex decomposition of a mesh
+ * @brief This takes an input file and generates a convex decomposition
+ *
+ * @author Michael Ripperger
+ * @date Nov 22, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, 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/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+#include <boost/program_options.hpp>
+#include <iostream>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_collision/core/common.h>
+#include <tesseract_collision/vhacd/convex_decomposition_vhacd.h>
+
+namespace
+{
+const size_t ERROR_IN_COMMAND_LINE = 1;
+const size_t SUCCESS = 0;
+const size_t ERROR_UNHANDLED_EXCEPTION = 2;
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  std::string input;
+  std::string output;
+  tesseract_collision::VHACDParameters params;
+
+  // clang-format off
+  namespace po = boost::program_options;
+  po::options_description desc("Options");
+  desc.add_options()("help,h", "Print help messages")
+      ("input,i", po::value<std::string>(&input)->required(), "File path to mesh used to create a convex hull.")
+      ("output,o", po::value<std::string>(&output)->required(), "File path to save the generated convex hull as a ply.")
+      ("max_convex_hulls,c", po::value<unsigned>(&(params.max_convex_hulls)), "Maximum number of convex hulls")
+      ("resolution,r", po::value<unsigned>(&(params.resolution)), "Number of voxels to use to represent the shape")
+      ("min_volume_percent_error,v", po::value<double>(&(params.minimum_volume_percent_error_allowed)), "If the voxels are within this threshold percentage of the volume of the hull, we consider this a close enough approximation")
+      ("max_recursion_depth,d", po::value<unsigned>(&(params.max_recursion_depth)), "Maximum recursion depth for convex decomposition improvement")
+      ("shrinkwrap,s", po::value<bool>(&(params.shrinkwrap)), "Shrinkwrap the voxel positions to the source mesh on output")
+      ("max_num_vertices,n", po::value<unsigned>(&(params.max_num_vertices_per_ch)), "Maximum number of vertices per convex hull")
+      ("min_edge_length,e", po::value<unsigned>(&(params.min_edge_length)), "Once a voxel patch has an edge length of less than this value in all 3 dimensions, stop recursing")
+      ("find_best_plane,p", po::value<bool>(&(params.find_best_plane)), "Flag for attempting to split planes along best location (experimental)");
+  // clang-format on
+
+  po::variables_map vm;
+  try
+  {
+    po::store(po::parse_command_line(argc, argv, desc), vm);  // can throw
+
+    /** --help option */
+    if (vm.count("help") != 0U)
+    {
+      std::cout << "Basic Command Line Parameter App" << std::endl << desc << std::endl;
+      return SUCCESS;
+    }
+
+    po::notify(vm);  // throws on error, so do after help in case
+                     // there are any problems
+  }
+  catch (po::error& e)
+  {
+    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
+    std::cerr << desc << std::endl;
+    return ERROR_IN_COMMAND_LINE;
+  }
+
+  std::ifstream file(input, std::ios::binary | std::ios::ate);
+  std::streamsize size = file.tellg();
+  if (size < 0)
+  {
+    CONSOLE_BRIDGE_logError("Failed to locate input file!");
+    return ERROR_UNHANDLED_EXCEPTION;
+  }
+
+  tesseract_common::VectorVector3d mesh_vertices;
+  Eigen::VectorXi mesh_faces;
+  int num_faces = tesseract_collision::loadSimplePlyFile(input, mesh_vertices, mesh_faces, true);
+  if (num_faces < 0)
+  {
+    CONSOLE_BRIDGE_logError("Failed to read mesh from file!");
+    return ERROR_UNHANDLED_EXCEPTION;
+  }
+
+  tesseract_collision::ConvexDecompositionVHACD convex_decomp(params);
+  std::vector<tesseract_geometry::ConvexMesh::Ptr> convex_hulls = convex_decomp.compute(mesh_vertices, mesh_faces);
+
+  if (convex_hulls.empty())
+  {
+    CONSOLE_BRIDGE_logError("Failed to create convex decomposition!");
+    return ERROR_UNHANDLED_EXCEPTION;
+  }
+
+  for (std::size_t i = 0; i < convex_hulls.size(); ++i)
+  {
+    auto ch = convex_hulls[i];
+    if (!tesseract_collision::writeSimplePlyFile(
+            std::to_string(i) + "_" + output, *(ch->getVertices()), *(ch->getFaces()), ch->getFaceCount()))
+    {
+      CONSOLE_BRIDGE_logError("Failed to write convex hull to file!");
+      return ERROR_UNHANDLED_EXCEPTION;
+    }
+  }
+
+  return 0;
+}