fixup

tesseract_collision/bullet/cmake/bullet-extras.cmake

@@ -0,0 +1 @@
+find_bullet()

tesseract_collision/core/cmake/core-extras.cmake

@@ -0,0 +1,24 @@
+if(NOT TARGET console_bridge::console_bridge)
+  add_library(console_bridge::console_bridge INTERFACE IMPORTED)
+  set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
+                                                                  ${console_bridge_INCLUDE_DIRS})
+  set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_LINK_LIBRARIES ${console_bridge_LIBRARIES})
+else()
+  get_target_property(CHECK_INCLUDE_DIRECTORIES console_bridge::console_bridge INTERFACE_INCLUDE_DIRECTORIES)
+  if(NOT ${CHECK_INCLUDE_DIRECTORIES})
+    set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
+                                                                    ${console_bridge_INCLUDE_DIRS})
+  endif()
+endif()
+
+# These targets are necessary for 16.04 builds. Remove when Kinetic support is dropped
+if(NOT TARGET octomap)
+  add_library(octomap INTERFACE IMPORTED)
+  set_target_properties(octomap PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${OCTOMAP_INCLUDE_DIRS}")
+  set_target_properties(octomap PROPERTIES INTERFACE_LINK_LIBRARIES "${OCTOMAP_LIBRARIES}")
+endif()
+if(NOT TARGET octomath)
+  add_library(octomath INTERFACE IMPORTED)
+  set_target_properties(octomath PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${OCTOMAP_INCLUDE_DIRS}")
+  set_target_properties(octomath PROPERTIES INTERFACE_LINK_LIBRARIES "${OCTOMAP_LIBRARIES}")
+endif()

tesseract_collision/test_suite/CMakeLists.txt

@@ -0,0 +1,51 @@
+find_package(tesseract_support REQUIRED)
+# Create test suite interface
+add_library(${PROJECT_NAME}_test_suite INTERFACE)
+target_link_libraries(
+  ${PROJECT_NAME}_test_suite
+  INTERFACE Eigen3::Eigen
+            tesseract::tesseract_support
+            tesseract::tesseract_geometry
+            ${PROJECT_NAME}_core)
+target_compile_options(${PROJECT_NAME}_test_suite INTERFACE ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
+target_compile_definitions(${PROJECT_NAME}_test_suite INTERFACE ${TESSERACT_COMPILE_DEFINITIONS})
+target_cxx_version(${PROJECT_NAME}_test_suite INTERFACE VERSION ${TESSERACT_CXX_VERSION})
+target_clang_tidy(${PROJECT_NAME}_test_suite ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
+target_include_directories(${PROJECT_NAME}_test_suite INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                                                "$<INSTALL_INTERFACE:include>")
+
+# Create test suite benchmarks interface
+add_library(${PROJECT_NAME}_test_suite_benchmarks INTERFACE)
+target_link_libraries(${PROJECT_NAME}_test_suite_benchmarks INTERFACE Eigen3::Eigen tesseract::tesseract_support
+                                                                      tesseract::tesseract_geometry)
+target_compile_options(${PROJECT_NAME}_test_suite_benchmarks INTERFACE ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
+target_cxx_version(${PROJECT_NAME}_test_suite_benchmarks INTERFACE VERSION ${TESSERACT_CXX_VERSION})
+target_clang_tidy(${PROJECT_NAME}_test_suite_benchmarks ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
+target_include_directories(
+  ${PROJECT_NAME}_test_suite_benchmarks INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                                  "$<INSTALL_INTERFACE:include>")
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
+  FILES_MATCHING
+  PATTERN "*.h"
+  PATTERN "*.hpp"
+  PATTERN "*.inl"
+  PATTERN ".svn" EXCLUDE)
+
+configure_component(
+  COMPONENT test_suite
+  NAMESPACE tesseract
+  TARGETS ${PROJECT_NAME}_test_suite ${PROJECT_NAME}_test_suite_benchmarks
+  DEPENDENCIES tesseract_support "tesseract_collision COMPONENTS core")
+
+if(TESSERACT_PACKAGE)
+  cpack_component(
+    COMPONENT test_suite
+    VERSION ${pkg_extracted_version}
+    DESCRIPTION "Tesseract Collision test_suite components"
+    COMPONENT_DEPENDS core
+    LINUX_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-support"
+    WINDOWS_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-support")
+endif()

tesseract_collision/test_suite/include/tesseract_collision/test_suite/benchmarks/benchmark_utils.hpp

@@ -0,0 +1,42 @@
+#ifndef TESSERACT_COLLISION_BENCHMARK_UTILS_HPP
+#define TESSERACT_COLLISION_BENCHMARK_UTILS_HPP
+
+#include <benchmark/benchmark.h>
+#include <Eigen/Eigen>
+#include <console_bridge/console.h>
+
+using namespace tesseract_collision;
+using namespace test_suite;
+using namespace tesseract_geometry;
+
+inline tesseract_geometry::Geometry::Ptr CreateUnitPrimative(const tesseract_geometry::GeometryType type,
+                                                             const double scale = 1.0)
+{
+  tesseract_geometry::Geometry::Ptr geom;
+  switch (type)
+  {
+    case tesseract_geometry::GeometryType::BOX:
+      geom = std::make_shared<tesseract_geometry::Box>(scale, scale, scale);
+      break;
+    case tesseract_geometry::GeometryType::CONE:
+      geom = std::make_shared<tesseract_geometry::Cone>(scale, scale);
+      break;
+    case tesseract_geometry::GeometryType::PLANE:
+      geom = std::make_shared<tesseract_geometry::Plane>(scale, scale, scale, scale);
+      break;
+    case tesseract_geometry::GeometryType::SPHERE:
+      geom = std::make_shared<tesseract_geometry::Sphere>(scale);
+      break;
+    case tesseract_geometry::GeometryType::CAPSULE:
+      geom = std::make_shared<tesseract_geometry::Capsule>(scale, scale);
+      break;
+    case tesseract_geometry::GeometryType::CYLINDER:
+      geom = std::make_shared<tesseract_geometry::Cylinder>(scale, scale);
+      break;
+    default:
+      CONSOLE_BRIDGE_logError("Invalid Geometry Type. Can only create primatives");
+      break;
+  }
+  return geom;
+}
+#endif

tesseract_collision/test_suite/include/tesseract_collision/test_suite/benchmarks/large_dataset_benchmarks.hpp

@@ -0,0 +1,200 @@
+#ifndef TESSERACT_COLLISION_LARGE_DATASET_BENCHMARKS_HPP
+#define TESSERACT_COLLISION_LARGE_DATASET_BENCHMARKS_HPP
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <chrono>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_collision/bullet/convex_hull_utils.h>
+#include <tesseract_collision/core/discrete_contact_manager.h>
+#include <tesseract_collision/core/common.h>
+#include <tesseract_geometry/geometries.h>
+
+namespace tesseract_collision
+{
+namespace test_suite
+{
+/** @brief Benchmark that checks collisions between a lot of objects. In this case it is a grid of spheres - each as its
+ * own link*/
+static void BM_LARGE_DATASET_MULTILINK(benchmark::State& state,
+                                       DiscreteContactManager::Ptr checker,  // NOLINT
+                                       int edge_size,
+                                       tesseract_geometry::GeometryType type)
+{
+  // Add Meshed Sphere to checker
+  CollisionShapePtr sphere;
+
+  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
+  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
+  loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true);
+
+  switch (type)
+  {
+    case tesseract_geometry::GeometryType::CONVEX_MESH:
+    {
+      auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
+      sphere = makeConvexMesh(*mesh);
+      break;
+    }
+    case tesseract_geometry::GeometryType::MESH:
+    {
+      sphere = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
+      break;
+    }
+    case tesseract_geometry::GeometryType::SPHERE:
+    {
+      sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
+      break;
+    }
+    default:
+    {
+      throw(std::runtime_error("Invalid geometry type"));
+      break;
+    }
+  }
+
+  double delta = 0.55;
+
+  std::vector<std::string> link_names;
+  tesseract_common::TransformMap location;
+  for (int x = 0; x < edge_size; ++x)
+  {
+    for (int y = 0; y < edge_size; ++y)
+    {
+      for (int z = 0; z < edge_size; ++z)
+      {
+        CollisionShapesConst obj3_shapes;
+        tesseract_common::VectorIsometry3d obj3_poses;
+        Eigen::Isometry3d sphere_pose;
+        sphere_pose.setIdentity();
+
+        obj3_shapes.push_back(CollisionShapePtr(sphere->clone()));
+        obj3_poses.push_back(sphere_pose);
+
+        link_names.push_back("sphere_link_" + std::to_string(x) + std::to_string(y) + std::to_string(z));
+
+        location[link_names.back()] = sphere_pose;
+        location[link_names.back()].translation() = Eigen::Vector3d(
+            static_cast<double>(x) * delta, static_cast<double>(y) * delta, static_cast<double>(z) * delta);
+        checker->addCollisionObject(link_names.back(), 0, obj3_shapes, obj3_poses);
+      }
+    }
+  }
+
+  // Check if they are in collision
+  checker->setActiveCollisionObjects(link_names);
+  checker->setCollisionMarginData(CollisionMarginData(0.1));
+  checker->setCollisionObjectsTransform(location);
+
+  ContactResultMap result;
+  ContactResultVector result_vector;
+
+  for (auto _ : state)  // NOLINT
+  {
+    result.clear();
+    result_vector.clear();
+    checker->contactTest(result, ContactTestType::ALL);
+    result.flattenMoveResults(result_vector);
+  }
+}
+
+/** @brief Benchmark that checks collisions between a lot of objects. In this case it is a grid of spheres in one link
+ * and a single sphere in another link*/
+static void BM_LARGE_DATASET_SINGLELINK(benchmark::State& state,
+                                        DiscreteContactManager::Ptr checker,  // NOLINT
+                                        int edge_size,
+                                        tesseract_geometry::GeometryType type)
+{
+  // Add Meshed Sphere to checker
+  CollisionShapePtr sphere;
+
+  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
+  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
+  loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true);
+
+  switch (type)
+  {
+    case tesseract_geometry::GeometryType::CONVEX_MESH:
+    {
+      auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
+      sphere = makeConvexMesh(*mesh);
+      break;
+    }
+    case tesseract_geometry::GeometryType::MESH:
+    {
+      sphere = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
+      break;
+    }
+    case tesseract_geometry::GeometryType::SPHERE:
+    {
+      sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
+      break;
+    }
+    default:
+    {
+      throw(std::runtime_error("Invalid geometry type"));
+      break;
+    }
+  }
+
+  // Add Grid of spheres
+  double delta = 0.55;
+
+  std::vector<std::string> link_names;
+  //  tesseract_common::TransformMap location;
+  CollisionShapesConst obj3_shapes;
+  tesseract_common::VectorIsometry3d obj3_poses;
+  for (int x = 0; x < edge_size; ++x)
+  {
+    for (int y = 0; y < edge_size; ++y)
+    {
+      for (int z = 0; z < edge_size; ++z)
+      {
+        Eigen::Isometry3d sphere_pose;
+        sphere_pose.setIdentity();
+        sphere_pose.translation() = Eigen::Vector3d(
+            static_cast<double>(x) * delta, static_cast<double>(y) * delta, static_cast<double>(z) * delta);
+
+        obj3_shapes.push_back(CollisionShapePtr(sphere->clone()));
+        obj3_poses.push_back(sphere_pose);
+      }
+    }
+  }
+  link_names.emplace_back("grid_link");
+  checker->addCollisionObject(link_names.back(), 0, obj3_shapes, obj3_poses);
+
+  // Add Single Sphere Link
+  Eigen::Isometry3d sphere_pose;
+  sphere_pose.setIdentity();
+  sphere_pose.translation() = Eigen::Vector3d(static_cast<double>(edge_size) / 2.0 * delta,
+                                              static_cast<double>(edge_size) / 2.0 * delta,
+                                              static_cast<double>(edge_size) / 2.0 * delta);
+  CollisionShapesConst single_shapes;
+  tesseract_common::VectorIsometry3d single_poses;
+  single_shapes.push_back(CollisionShapePtr(sphere->clone()));
+  single_poses.push_back(sphere_pose);
+  link_names.emplace_back("single_link");
+  checker->addCollisionObject(link_names.back(), 0, single_shapes, single_poses);
+
+  // Check if they are in collision
+  checker->setActiveCollisionObjects(link_names);
+  checker->setCollisionMarginData(CollisionMarginData(0.1));
+  //  checker->setCollisionObjectsTransform(location);
+
+  ContactResultMap result;
+  ContactResultVector result_vector;
+
+  for (auto _ : state)  // NOLINT
+  {
+    result.clear();
+    result_vector.clear();
+    checker->contactTest(result, ContactTestType::ALL);
+    result.flattenMoveResults(result_vector);
+  }
+}
+
+}  // namespace test_suite
+}  // namespace tesseract_collision
+
+#endif