From 729d339cc754f19ba48174600f2f6a9e520772c9 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 21 Mar 2023 17:00:42 -0500 Subject: [PATCH] Update tesseract_collision benchmarks (#868) --- .../test_suite/benchmarks/benchmark_utils.hpp | 2 +- .../test_suite/benchmarks/primatives_benchmarks.hpp | 13 +++++++------ .../benchmarks/bullet_discrete_bvh_benchmarks.cpp | 8 ++++---- .../bullet_discrete_simple_benchmarks.cpp | 8 ++++---- .../test/benchmarks/collision_profile.cpp | 2 ++ .../test/benchmarks/fcl_discrete_bvh_benchmarks.cpp | 8 ++++---- 6 files changed, 22 insertions(+), 19 deletions(-) diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/benchmark_utils.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/benchmark_utils.hpp index 282eb102a43..2d639ee75f1 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/benchmark_utils.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/benchmark_utils.hpp @@ -38,5 +38,5 @@ inline tesseract_geometry::Geometry::Ptr CreateUnitPrimative(const tesseract_geo break; } return geom; -}; +} #endif diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/primatives_benchmarks.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/primatives_benchmarks.hpp index 708c4b977ee..389dd266ad7 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/primatives_benchmarks.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/primatives_benchmarks.hpp @@ -56,7 +56,7 @@ static void BM_CLONE(benchmark::State& state, DiscreteBenchmarkInfo info, std::s { benchmark::DoNotOptimize(clone = info.contact_manager_->clone()); } -}; +} /** @brief Benchmark that checks the contactTest function in discrete contact managers*/ static void BM_CONTACT_TEST(benchmark::State& state, DiscreteBenchmarkInfo info) // NOLINT @@ -73,7 +73,7 @@ static void BM_CONTACT_TEST(benchmark::State& state, DiscreteBenchmarkInfo info) result.clear(); info.contact_manager_->contactTest(result, ContactRequest(info.contact_test_type_)); } -}; +} /** @brief Benchmark that checks how long it takes to select a random object so that number can be subtracted from other * benchmarks if that is important*/ @@ -84,7 +84,7 @@ static void BM_SELECT_RANDOM_OBJECT(benchmark::State& state, int num_obj) { benchmark::DoNotOptimize(selected_link = rand() % num_obj); // NOLINT } -}; +} /** @brief Benchmark that checks the setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& * pose) method in discrete contact managers*/ @@ -111,7 +111,7 @@ static void BM_SET_COLLISION_OBJECTS_TRANSFORM_SINGLE(benchmark::State& state, std::size_t selected_link = static_cast(rand()) % num_obj; info.contact_manager_->setCollisionObjectsTransform(active_obj[selected_link], info.obj2_poses[0]); } -}; +} /** @brief Benchmark that checks the setCollisionObjectsTransform(const std::vector& names, const tesseract_common::VectorIsometry3d& poses) method in discrete contact managers. Moves only a single random link*/ @@ -139,7 +139,7 @@ static void BM_SET_COLLISION_OBJECTS_TRANSFORM_VECTOR(benchmark::State& state, selected_links[0] = active_obj[static_cast(rand()) % num_obj]; info.contact_manager_->setCollisionObjectsTransform(selected_links, info.obj2_poses); } -}; +} /** @brief Benchmark that checks the setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) * method in discrete contact managers. Moves only a single random link*/ @@ -163,10 +163,11 @@ static void BM_SET_COLLISION_OBJECTS_TRANSFORM_MAP(benchmark::State& state, { // Including this seems necessary to insure that a distribution of links is used rather than always searching for // the same one. It might be worth it to manually time these as well if it's really important + selected_link.clear(); // Need to clear or this grows and is not releastic to compare to vector version selected_link[active_obj[static_cast(rand()) % num_obj]] = info.obj2_poses[0]; info.contact_manager_->setCollisionObjectsTransform(selected_link); } -}; +} } // namespace test_suite } // namespace tesseract_collision diff --git a/tesseract_collision/test/benchmarks/bullet_discrete_bvh_benchmarks.cpp b/tesseract_collision/test/benchmarks/bullet_discrete_bvh_benchmarks.cpp index a1060026b06..ead79e6dae4 100644 --- a/tesseract_collision/test/benchmarks/bullet_discrete_bvh_benchmarks.cpp +++ b/tesseract_collision/test/benchmarks/bullet_discrete_bvh_benchmarks.cpp @@ -234,7 +234,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -244,7 +244,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_MULTILINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -270,7 +270,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -280,7 +280,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_SINGLELINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { diff --git a/tesseract_collision/test/benchmarks/bullet_discrete_simple_benchmarks.cpp b/tesseract_collision/test/benchmarks/bullet_discrete_simple_benchmarks.cpp index 9c27b310524..779d268eb1b 100644 --- a/tesseract_collision/test/benchmarks/bullet_discrete_simple_benchmarks.cpp +++ b/tesseract_collision/test/benchmarks/bullet_discrete_simple_benchmarks.cpp @@ -233,7 +233,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -243,7 +243,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_MULTILINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -269,7 +269,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -279,7 +279,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_SINGLELINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { diff --git a/tesseract_collision/test/benchmarks/collision_profile.cpp b/tesseract_collision/test/benchmarks/collision_profile.cpp index 301fe5c3177..55fec54b87c 100644 --- a/tesseract_collision/test/benchmarks/collision_profile.cpp +++ b/tesseract_collision/test/benchmarks/collision_profile.cpp @@ -12,6 +12,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + static const std::size_t DIM = 10; using namespace tesseract_collision; diff --git a/tesseract_collision/test/benchmarks/fcl_discrete_bvh_benchmarks.cpp b/tesseract_collision/test/benchmarks/fcl_discrete_bvh_benchmarks.cpp index c694fe68bb5..57a3c2cd892 100644 --- a/tesseract_collision/test/benchmarks/fcl_discrete_bvh_benchmarks.cpp +++ b/tesseract_collision/test/benchmarks/fcl_discrete_bvh_benchmarks.cpp @@ -243,7 +243,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -253,7 +253,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_MULTILINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } // These last two took 45s and 120s. Too long to run in CI, and impractical for our purposes anyway. if (RUN_QUICK) @@ -285,7 +285,7 @@ int main(int argc, char** argv) edge_size, tesseract_geometry::GeometryType::CONVEX_MESH) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) { @@ -295,7 +295,7 @@ int main(int argc, char** argv) benchmark::RegisterBenchmark( name.c_str(), BM_LARGE_DATASET_SINGLELINK_FUNC, clone, edge_size, tesseract_geometry::GeometryType::SPHERE) ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond); + ->Unit(benchmark::TimeUnit::kNanosecond); } for (const auto& edge_size : edge_sizes) {