diff --git a/tesseract_collision/examples/CMakeLists.txt b/tesseract_collision/examples/CMakeLists.txt index 20a263a3824..467b60a0169 100644 --- a/tesseract_collision/examples/CMakeLists.txt +++ b/tesseract_collision/examples/CMakeLists.txt @@ -1,10 +1,8 @@ -find_package(tesseract_support REQUIRED) add_executable(${PROJECT_NAME}_box_box_example box_box_example.cpp) target_link_libraries( ${PROJECT_NAME}_box_box_example ${PROJECT_NAME}_bullet ${PROJECT_NAME}_fcl - tesseract::tesseract_support tesseract::tesseract_geometry console_bridge::console_bridge ${Boost_LIBRARIES} diff --git a/tesseract_collision/examples/box_box_example.cpp b/tesseract_collision/examples/box_box_example.cpp index 5080ea2b55b..960b21b7531 100644 --- a/tesseract_collision/examples/box_box_example.cpp +++ b/tesseract_collision/examples/box_box_example.cpp @@ -6,6 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_collision; using namespace tesseract_geometry; @@ -57,11 +58,15 @@ int main(int /*argc*/, char** /*argv*/) // documentation:start:4: Add convex hull // Add second box to checker, but convert to convex hull mesh + tesseract_common::GeneralResourceLocator locator; CollisionShapePtr second_box; auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply", *mesh_vertices, *mesh_faces, true); + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/box_2m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true); auto mesh = std::make_shared(mesh_vertices, mesh_faces); second_box = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test/benchmarks/CMakeLists.txt b/tesseract_collision/test/benchmarks/CMakeLists.txt index c29402661d1..4e795d4a9d0 100644 --- a/tesseract_collision/test/benchmarks/CMakeLists.txt +++ b/tesseract_collision/test/benchmarks/CMakeLists.txt @@ -1,5 +1,4 @@ find_package(benchmark REQUIRED) -find_package(tesseract_support REQUIRED) find_package(tesseract_scene_graph REQUIRED) macro(add_benchmark benchmark_name benchmark_file) @@ -43,7 +42,6 @@ target_link_libraries( PRIVATE ${PROJECT_NAME}_bullet ${PROJECT_NAME}_fcl tesseract::tesseract_geometry - tesseract::tesseract_support ${Boost_LIBRARIES} ${OCTOMAP_LIBRARIES} ${LIBFCL_LIBRARIES}) diff --git a/tesseract_collision/test/benchmarks/collision_profile.cpp b/tesseract_collision/test/benchmarks/collision_profile.cpp index 3889601db62..d3b98cf2d6b 100644 --- a/tesseract_collision/test/benchmarks/collision_profile.cpp +++ b/tesseract_collision/test/benchmarks/collision_profile.cpp @@ -13,7 +13,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include - +#include static const std::size_t DIM = 10; using namespace tesseract_collision; @@ -31,8 +31,12 @@ void addCollisionObjects(DiscreteContactManager& checker, bool use_single_link, { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true); + + tesseract_common::GeneralResourceLocator locator; + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); @@ -98,8 +102,12 @@ void addCollisionObjects(ContinuousContactManager& checker, bool use_single_link { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true); + + tesseract_common::GeneralResourceLocator locator; + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/CMakeLists.txt b/tesseract_collision/test_suite/CMakeLists.txt index b444173e97c..c73799c607f 100644 --- a/tesseract_collision/test_suite/CMakeLists.txt +++ b/tesseract_collision/test_suite/CMakeLists.txt @@ -1,12 +1,7 @@ -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_link_libraries(${PROJECT_NAME}_test_suite INTERFACE Eigen3::Eigen 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}) @@ -16,8 +11,7 @@ target_include_directories(${PROJECT_NAME}_test_suite INTERFACE "$ #include #include +#include namespace tesseract_collision { @@ -27,7 +28,12 @@ static void BM_LARGE_DATASET_MULTILINK(benchmark::State& state, auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true); + + tesseract_common::GeneralResourceLocator locator; + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true); switch (type) { @@ -111,7 +117,12 @@ static void BM_LARGE_DATASET_SINGLELINK(benchmark::State& state, auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true); + + tesseract_common::GeneralResourceLocator locator; + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true); switch (type) { diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp index faaa3b9c7ac..467bd0543c2 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace tesseract_collision::test_suite { @@ -54,8 +55,11 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); // TODO: Need to figure out why this test not pass of bullet when using the box_2m.ply mesh - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box2_2m.ply", *mesh_vertices, *mesh_faces, true), + tesseract_common::GeneralResourceLocator locator; + EXPECT_GT(loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/box2_2m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp index 08a06e0a50b..e3774be28c3 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace tesseract_collision::test_suite { @@ -52,9 +53,13 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + tesseract_common::GeneralResourceLocator locator; + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp index 17aac5747ad..7cefc9c4c60 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -20,7 +21,8 @@ inline void addCollisionObjects(T& checker) ///////////////////////////////////////////////////////////////// // Add Octomap ///////////////////////////////////////////////////////////////// - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; + tesseract_common::GeneralResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/meshes/box_2m.bt")->getFilePath(); auto ot = std::make_shared(path); CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp index 92eceb404bb..ae29ef9f646 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp @@ -11,6 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -23,9 +24,13 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + tesseract_common::GeneralResourceLocator locator; + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp index 282ccb3cdaf..01c6e0606f5 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace tesseract_collision::test_suite { @@ -18,8 +19,12 @@ inline void addCollisionObjects(DiscreteContactManager& checker) auto vertices = std::make_shared(); auto faces = std::make_shared(); + tesseract_common::GeneralResourceLocator locator; int num_faces = - loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *vertices, *faces, true); + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *vertices, + *faces, + true); EXPECT_GT(num_faces, 0); sphere = std::make_shared(vertices, faces); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp index 7f25f27fa27..d3ef3d7dc2e 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp @@ -11,6 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -22,9 +23,13 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + tesseract_common::GeneralResourceLocator locator; + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp index 7e4575066ec..5cb40981130 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp @@ -11,6 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -21,7 +22,8 @@ inline void addCollisionObjects(DiscreteContactManager& checker) ///////////////////////////////////////////////////////////////// // Add Octomap ///////////////////////////////////////////////////////////////// - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; + tesseract_common::GeneralResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/meshes/box_2m.bt")->getFilePath(); auto ot = std::make_shared(path); CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); @@ -39,7 +41,9 @@ inline void addCollisionObjects(DiscreteContactManager& checker) // Add plane mesh to checker. ///////////////////////////////////////////////////////////////// CollisionShapePtr sphere = tesseract_geometry::createMeshFromPath( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/plane_4m.stl", Eigen::Vector3d(1, 1, 1), true)[0]; + locator.locateResource("package://tesseract_support/meshes/plane_4m.stl")->getFilePath(), + Eigen::Vector3d(1, 1, 1), + true)[0]; Eigen::Isometry3d sphere_pose; sphere_pose.setIdentity(); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp index 4a1d2188a85..e16ba22880d 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -20,7 +21,8 @@ inline void addCollisionObjects(T& checker) ///////////////////////////////////////////////////////////////// // Add Octomap ///////////////////////////////////////////////////////////////// - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; + tesseract_common::GeneralResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/meshes/box_2m.bt")->getFilePath(); auto ot = std::make_shared(path); CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp index 4f280c7e51c..f56938d696e 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp @@ -12,6 +12,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision::test_suite { @@ -22,7 +23,8 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex ///////////////////////////////////////////////////////////////// // Add Octomap ///////////////////////////////////////////////////////////////// - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/blender_monkey.bt"; + tesseract_common::GeneralResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/meshes/blender_monkey.bt")->getFilePath(); auto ot = std::make_shared(path); CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); @@ -46,9 +48,12 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp index 494a8f51f69..835a43fc4f0 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace tesseract_collision::test_suite { @@ -12,6 +13,8 @@ namespace detail { inline void addCollisionObjects(ContinuousContactManager& checker, bool use_convex_mesh = false) { + tesseract_common::GeneralResourceLocator locator; + //////////////////////// // Add sphere to checker //////////////////////// @@ -20,9 +23,12 @@ inline void addCollisionObjects(ContinuousContactManager& checker, bool use_conv { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); @@ -73,9 +79,12 @@ inline void addCollisionObjects(ContinuousContactManager& checker, bool use_conv { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere1 = makeConvexMesh(*mesh); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp index b10bf7a2eb2..cbc2c38e60c 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace tesseract_collision::test_suite { @@ -12,6 +13,8 @@ namespace detail { inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex_mesh = false) { + tesseract_common::GeneralResourceLocator locator; + //////////////////////// // Add sphere to checker //////////////////////// @@ -20,9 +23,12 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere = makeConvexMesh(*mesh); @@ -66,9 +72,12 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex { auto mesh_vertices = std::make_shared(); auto mesh_faces = std::make_shared(); - EXPECT_GT(loadSimplePlyFile( - std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true), - 0); + EXPECT_GT( + loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(), + *mesh_vertices, + *mesh_faces, + true), + 0); auto mesh = std::make_shared(mesh_vertices, mesh_faces); sphere1 = makeConvexMesh(*mesh); diff --git a/tesseract_environment/test/CMakeLists.txt b/tesseract_environment/test/CMakeLists.txt index d1b3dfe465d..f061f8185e9 100644 --- a/tesseract_environment/test/CMakeLists.txt +++ b/tesseract_environment/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_gtest() -find_package(tesseract_support REQUIRED) find_package(tesseract_collision REQUIRED COMPONENTS core bullet fcl) find_package(OpenMP REQUIRED) if(NOT TARGET OpenMP::OpenMP_CXX) @@ -17,7 +16,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME} OpenMP::OpenMP_CXX - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_collision_bullet tesseract::tesseract_collision_fcl @@ -38,12 +36,7 @@ add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_unit) add_executable(${PROJECT_NAME}_collision tesseract_environment_collision.cpp) -target_link_libraries( - ${PROJECT_NAME}_collision - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_collision PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_collision PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_collision PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -60,12 +53,7 @@ add_dependencies(${PROJECT_NAME}_collision ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_collision) add_executable(${PROJECT_NAME}_utils tesseract_environment_utils.cpp) -target_link_libraries( - ${PROJECT_NAME}_utils - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_utils PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_utils PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_utils PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -90,7 +78,6 @@ target_link_libraries( ${PROJECT_NAME} ${PROJECT_NAME}_commands tesseract::tesseract_common - tesseract::tesseract_support tesseract::tesseract_urdf) target_compile_options(${PROJECT_NAME}_serialization_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -107,12 +94,7 @@ add_dependencies(${PROJECT_NAME}_serialization_unit ${PROJECT_NAME}_commands) add_dependencies(run_tests ${PROJECT_NAME}_serialization_unit) add_executable(${PROJECT_NAME}_cache_unit tesseract_environment_cache_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_cache_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_cache_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_cache_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_cache_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_environment/test/benchmarks/CMakeLists.txt b/tesseract_environment/test/benchmarks/CMakeLists.txt index da577c7a3be..325f391c7d8 100644 --- a/tesseract_environment/test/benchmarks/CMakeLists.txt +++ b/tesseract_environment/test/benchmarks/CMakeLists.txt @@ -1,5 +1,4 @@ find_package(benchmark REQUIRED) -find_package(tesseract_support REQUIRED) macro(add_benchmark benchmark_name benchmark_file) add_executable(${benchmark_name} ${benchmark_file}) @@ -14,7 +13,6 @@ macro(add_benchmark benchmark_name benchmark_file) benchmark::benchmark ${PROJECT_NAME} tesseract::tesseract_urdf - tesseract::tesseract_support console_bridge ${Boost_LIBRARIES}) target_include_directories(${benchmark_name} PRIVATE "$") diff --git a/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp b/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp index f66ce2a015a..498de607e17 100644 --- a/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp @@ -14,7 +14,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -27,19 +27,19 @@ using namespace tesseract_environment; SceneGraph::Ptr getSceneGraph() { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + std::string url = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + tesseract_common::GeneralResourceLocator locator; + return tesseract_urdf::parseURDFFile(locator.locateResource(url)->getFilePath(), locator); } SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string url = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; + tesseract_common::GeneralResourceLocator locator; auto srdf = std::make_shared(); - srdf->initFile(scene_graph, path, locator); + srdf->initFile(scene_graph, locator.locateResource(url)->getFilePath(), locator); return srdf; } @@ -128,7 +128,7 @@ int main(int argc, char** argv) tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); auto srdf = getSRDFModel(*scene_graph); env->init(*scene_graph, srdf); - env->setResourceLocator(std::make_shared()); + env->setResourceLocator(std::make_shared()); // Add sphere to environment Link link_sphere("sphere_attached"); diff --git a/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp b/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp index 8a8b5e0c3d0..7f39e2c536c 100644 --- a/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp @@ -11,28 +11,27 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_scene_graph; using namespace tesseract_collision; using namespace tesseract_environment; using namespace tesseract_kinematics; -SceneGraph::Ptr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -tesseract_srdf::SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +tesseract_srdf::SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, + const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; auto srdf = std::make_shared(); - srdf->initFile(scene_graph, path, locator); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } @@ -88,8 +87,9 @@ static void BM_KINEMATIC_GROUP_COPY(benchmark::State& state, KinematicGroup::Ptr int main(int argc, char** argv) { - SceneGraph::Ptr scene_graph = getSceneGraph(); - auto srdf = getSRDFModel(*scene_graph); + tesseract_common::GeneralResourceLocator locator; + SceneGraph::Ptr scene_graph = getSceneGraph(locator); + auto srdf = getSRDFModel(*scene_graph, locator); Environment::Ptr env = std::make_shared(); env->init(*scene_graph, srdf); StateSolver::Ptr state_solver = env->getStateSolver(); diff --git a/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp index 4ee97d87c9d..2a9f6f53c72 100644 --- a/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp @@ -12,28 +12,24 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include #include using namespace tesseract_scene_graph; using namespace tesseract_srdf; using namespace tesseract_environment; -SceneGraph::Ptr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; auto srdf = std::make_shared(); - srdf->initFile(scene_graph, path, locator); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } @@ -113,11 +109,12 @@ static void BM_GET_JACOBIAN_MANIP(benchmark::State& state, int main(int argc, char** argv) { + tesseract_common::GeneralResourceLocator locator; auto env = std::make_shared(); - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); - auto srdf = getSRDFModel(*scene_graph); + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(locator); + auto srdf = getSRDFModel(*scene_graph, locator); env->init(*scene_graph, srdf); - env->setResourceLocator(std::make_shared()); + env->setResourceLocator(std::make_shared()); // Set the robot initial state std::vector joint_names; diff --git a/tesseract_environment/test/tesseract_environment_cache_unit.cpp b/tesseract_environment/test/tesseract_environment_cache_unit.cpp index 4f0d7aec426..f49da2ef1e4 100644 --- a/tesseract_environment/test/tesseract_environment_cache_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_cache_unit.cpp @@ -18,28 +18,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include - using namespace tesseract_scene_graph; using namespace tesseract_srdf; -using namespace tesseract_collision; using namespace tesseract_environment; -SceneGraph::UPtr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/boxbot.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/boxbot.srdf"; - auto srdf = std::make_unique(); - srdf->initFile(scene_graph, path, locator); + auto srdf = std::make_shared(); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } @@ -65,10 +59,11 @@ void addLink(Environment& env) TEST(TesseractEnvironmentCache, defaultEnvironmentCacheTest) // NOLINT { - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); auto env = std::make_shared(); diff --git a/tesseract_environment/test/tesseract_environment_collision.cpp b/tesseract_environment/test/tesseract_environment_collision.cpp index f4dc0aceb8c..e59a7e962f0 100644 --- a/tesseract_environment/test/tesseract_environment_collision.cpp +++ b/tesseract_environment/test/tesseract_environment_collision.cpp @@ -21,38 +21,35 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include - using namespace tesseract_scene_graph; using namespace tesseract_srdf; using namespace tesseract_collision; using namespace tesseract_environment; -SceneGraph::UPtr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; - auto srdf = std::make_unique(); - srdf->initFile(scene_graph, path, locator); + auto srdf = std::make_shared(); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } tesseract_environment::Environment::UPtr getEnvironment() { - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + + auto scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); auto env = std::make_unique(); diff --git a/tesseract_environment/test/tesseract_environment_serialization_unit.cpp b/tesseract_environment/test/tesseract_environment_serialization_unit.cpp index 9e98c9d0493..3258c6f1064 100644 --- a/tesseract_environment/test/tesseract_environment_serialization_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_serialization_unit.cpp @@ -45,36 +45,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include - using namespace tesseract_common; using namespace tesseract_environment; using namespace tesseract_scene_graph; using namespace tesseract_srdf; -SceneGraph::UPtr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::GeneralResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; auto srdf = std::make_shared(); - srdf->initFile(scene_graph, path, locator); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } + Environment::Ptr getEnvironment() { + tesseract_common::GeneralResourceLocator locator; auto env = std::make_shared(); - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); - auto srdf = getSRDFModel(*scene_graph); + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(locator); + auto srdf = getSRDFModel(*scene_graph, locator); env->init(*scene_graph, srdf); env->setResourceLocator(std::make_shared()); return env; @@ -165,13 +162,14 @@ TEST(EnvironmentCommandsSerializeUnit, AddTrajectoryLinkCommand) // NOLINT TEST(EnvironmentCommandsSerializeUnit, AddSceneGraphCommand) // NOLINT { + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::Joint joint; Joint joint_1("joint_1"); joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25; joint_1.parent_link_name = "world"; joint_1.child_link_name = "joint_a1"; joint_1.type = JointType::FIXED; - auto object = std::make_shared(*getSceneGraph(), joint, "prefix"); + auto object = std::make_shared(*getSceneGraph(locator), joint, "prefix"); testSerialization(*object, "AddSceneGraphCommand"); testSerializationDerivedClass(object, "AddSceneGraphCommand"); } diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp index e4d232699b8..00de706e59c 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -40,8 +40,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include - using namespace tesseract_scene_graph; using namespace tesseract_srdf; using namespace tesseract_collision; @@ -56,40 +54,37 @@ Eigen::Isometry3d tcpCallback(const tesseract_common::ManipulatorInfo& mi) throw std::runtime_error("TCPCallback failed to find tcp!"); } -SceneGraph::UPtr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; auto srdf = std::make_shared(); - srdf->initFile(scene_graph, path, locator); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } -std::string getSceneGraphString() +std::string getSceneGraphString(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf"; - std::ifstream f(path); + std::ifstream f(locator.locateResource(path)->getFilePath()); std::ostringstream ss; ss << f.rdbuf(); return ss.str(); } -std::string getSRDFModelString() +std::string getSRDFModelString(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; + std::string path = "package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf"; - std::ifstream f(path); + std::ifstream f(locator.locateResource(path)->getFilePath()); std::ostringstream ss; ss << f.rdbuf(); return ss.str(); @@ -169,6 +164,8 @@ enum class EnvironmentInitType Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitType::OBJECT) { + tesseract_common::GeneralResourceLocator locator; + auto env = std::make_shared(); EXPECT_TRUE(env != nullptr); EXPECT_EQ(0, env->getRevision()); @@ -184,7 +181,7 @@ Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitT { case EnvironmentInitType::OBJECT: { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); // Check to make sure all links are enabled @@ -194,30 +191,31 @@ Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitT EXPECT_TRUE(scene_graph->getLinkVisibility(link->getName())); } - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); success = env->init(*scene_graph, srdf); EXPECT_TRUE(env->getResourceLocator() == nullptr); - env->setResourceLocator(std::make_shared()); + env->setResourceLocator(std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } case EnvironmentInitType::STRING: { - std::string urdf_string = getSceneGraphString(); - std::string srdf_string = getSRDFModelString(); - success = - env->init(urdf_string, srdf_string, std::make_shared()); + std::string urdf_string = getSceneGraphString(locator); + std::string srdf_string = getSRDFModelString(locator); + success = env->init(urdf_string, srdf_string, std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } case EnvironmentInitType::FILEPATH: { - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); - success = env->init(urdf_path, srdf_path, std::make_shared()); + success = env->init(urdf_path, srdf_path, std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } @@ -353,6 +351,8 @@ Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitT Environment::Ptr getEnvironmentURDFOnly(EnvironmentInitType init_type) { + tesseract_common::GeneralResourceLocator locator; + auto env = std::make_shared(); EXPECT_TRUE(env != nullptr); EXPECT_EQ(0, env->getRevision()); @@ -367,7 +367,7 @@ Environment::Ptr getEnvironmentURDFOnly(EnvironmentInitType init_type) { case EnvironmentInitType::OBJECT: { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); // Check to make sure all links are enabled @@ -379,21 +379,22 @@ Environment::Ptr getEnvironmentURDFOnly(EnvironmentInitType init_type) success = env->init(*scene_graph); EXPECT_TRUE(env->getResourceLocator() == nullptr); - env->setResourceLocator(std::make_shared()); + env->setResourceLocator(std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } case EnvironmentInitType::STRING: { - std::string urdf_string = getSceneGraphString(); - success = env->init(urdf_string, std::make_shared()); + std::string urdf_string = getSceneGraphString(locator); + success = env->init(urdf_string, std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } case EnvironmentInitType::FILEPATH: { - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - success = env->init(urdf_path, std::make_shared()); + tesseract_common::fs::path urdf_path( + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + success = env->init(urdf_path, std::make_shared()); EXPECT_TRUE(env->getResourceLocator() != nullptr); break; } @@ -427,7 +428,7 @@ TEST(TesseractEnvironmentUnit, EnvInitURDFOnlyUnit) // NOLINT TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT { - auto rl = std::make_shared(); + auto rl = std::make_shared(); { auto env = std::make_shared(); EXPECT_TRUE(env != nullptr); @@ -472,7 +473,7 @@ TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT { // Test Empty URDF String with srdf auto env = std::make_shared(); std::string urdf_string; - std::string srdf_string = getSRDFModelString(); + std::string srdf_string = getSRDFModelString(*rl); EXPECT_FALSE(env->init(urdf_string, srdf_string, rl)); EXPECT_FALSE(env->isInitialized()); } @@ -480,14 +481,15 @@ TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT { // Test bad URDF file path with srdf auto env = std::make_shared(); tesseract_common::fs::path urdf_path("/usr/tmp/doesnotexist.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path srdf_path( + rl->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_FALSE(env->init(urdf_path, srdf_path, rl)); EXPECT_FALSE(env->isInitialized()); } { // Test URDF String with empty srdf auto env = std::make_shared(); - std::string urdf_string = getSceneGraphString(); + std::string urdf_string = getSceneGraphString(*rl); std::string srdf_string; EXPECT_FALSE(env->init(urdf_string, srdf_string, rl)); EXPECT_FALSE(env->isInitialized()); @@ -495,7 +497,8 @@ TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT { // Test URDF file path with bad srdf path auto env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + tesseract_common::fs::path urdf_path( + rl->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); tesseract_common::fs::path srdf_path("/usr/tmp/doesnotexist.srdf"); EXPECT_FALSE(env->init(urdf_path, srdf_path, rl)); EXPECT_FALSE(env->isInitialized()); @@ -1966,7 +1969,7 @@ TEST(TesseractEnvironmentUnit, EnvApplyCommandsStateSolverCompareUnit) // NOLIN { // This is testing commands that modify the connectivity of scene graph // It checks that the state solver are updated correctly - + tesseract_common::GeneralResourceLocator locator; { // Add new link no joint // Get the environment auto compare_env = getEnvironment(); @@ -2293,7 +2296,7 @@ TEST(TesseractEnvironmentUnit, EnvApplyCommandsStateSolverCompareUnit) // NOLIN { // Add SceneGraph case 2 auto compare_env = getEnvironment(); - auto subgraph = getSceneGraph(); + auto subgraph = getSceneGraph(locator); Commands commands{ std::make_shared(*subgraph, "prefix_") }; EXPECT_TRUE(compare_env->applyCommands(commands)); @@ -2306,7 +2309,7 @@ TEST(TesseractEnvironmentUnit, EnvApplyCommandsStateSolverCompareUnit) // NOLIN { // Add SceneGraph case 3 auto compare_env = getEnvironment(); - auto subgraph = getSceneGraph(); + auto subgraph = getSceneGraph(locator); Joint attach_joint("prefix_base_link_joint"); attach_joint.parent_link_name = "tool0"; attach_joint.child_link_name = "prefix_base_link"; diff --git a/tesseract_environment/test/tesseract_environment_utils.cpp b/tesseract_environment/test/tesseract_environment_utils.cpp index 10f070cc4b3..b4d9d5c6b82 100644 --- a/tesseract_environment/test/tesseract_environment_utils.cpp +++ b/tesseract_environment/test/tesseract_environment_utils.cpp @@ -18,28 +18,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include - using namespace tesseract_scene_graph; using namespace tesseract_srdf; using namespace tesseract_collision; using namespace tesseract_environment; -SceneGraph::UPtr getSceneGraph() +SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; - return tesseract_urdf::parseURDFFile(path, locator); + std::string path = "package://tesseract_support/urdf/boxbot.urdf"; + return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator); } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = "package://tesseract_support/urdf/boxbot.srdf"; - auto srdf = std::make_unique(); - srdf->initFile(scene_graph, path, locator); + auto srdf = std::make_shared(); + srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator); return srdf; } @@ -93,10 +88,11 @@ void checkIsAllowedFnOverride(std::unique_ptr manager) TEST(TesseractEnvironmentUtils, applyContactManagerConfigIsAllowed) // NOLINT { - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); auto env = std::make_shared(); @@ -117,10 +113,12 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigIsAllowed) // NOLINT TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLINT { - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + + auto scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); auto env = std::make_shared(); @@ -242,10 +240,12 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN TEST(TesseractEnvironmentUtils, checkTrajectoryState) // NOLINT { - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + + auto scene_graph = getSceneGraph(locator); EXPECT_TRUE(scene_graph != nullptr); - auto srdf = getSRDFModel(*scene_graph); + auto srdf = getSRDFModel(*scene_graph, locator); EXPECT_TRUE(srdf != nullptr); auto env = std::make_shared(); diff --git a/tesseract_geometry/examples/CMakeLists.txt b/tesseract_geometry/examples/CMakeLists.txt index 5c251e44cae..3ccd15c9883 100644 --- a/tesseract_geometry/examples/CMakeLists.txt +++ b/tesseract_geometry/examples/CMakeLists.txt @@ -1,11 +1,5 @@ -find_package(tesseract_support REQUIRED) - add_executable(${PROJECT_NAME}_create_geometries_example create_geometries_example.cpp) -target_link_libraries( - ${PROJECT_NAME}_create_geometries_example - ${PROJECT_NAME} - console_bridge::console_bridge - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_create_geometries_example ${PROJECT_NAME} console_bridge::console_bridge) target_compile_options(${PROJECT_NAME}_create_geometries_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_clang_tidy(${PROJECT_NAME}_create_geometries_example ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) @@ -14,11 +8,7 @@ add_dependencies(${PROJECT_NAME}_create_geometries_example ${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME}_create_geometries_example DESTINATION bin) add_executable(${PROJECT_NAME}_parse_mesh_example parse_mesh_example.cpp) -target_link_libraries( - ${PROJECT_NAME}_parse_mesh_example - ${PROJECT_NAME} - console_bridge::console_bridge - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_parse_mesh_example ${PROJECT_NAME} console_bridge::console_bridge) target_compile_options(${PROJECT_NAME}_parse_mesh_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_clang_tidy(${PROJECT_NAME}_parse_mesh_example ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) diff --git a/tesseract_geometry/examples/parse_mesh_example.cpp b/tesseract_geometry/examples/parse_mesh_example.cpp index 5372c72dcb7..cd9cd00ce82 100644 --- a/tesseract_geometry/examples/parse_mesh_example.cpp +++ b/tesseract_geometry/examples/parse_mesh_example.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include using namespace tesseract_geometry; @@ -8,8 +9,9 @@ using namespace tesseract_geometry; int main(int /*argc*/, char** /*argv*/) { // documentation:start:1: Create meshes - std::string mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.dae"; - std::vector meshes = createMeshFromPath(mesh_file); + tesseract_common::GeneralResourceLocator locator; + std::string mesh_file = "package://tesseract_support/meshes/sphere_p25m.dae"; + std::vector meshes = createMeshFromPath(locator.locateResource(mesh_file)->getFilePath()); // documentation:end:1: Create meshes // documentation:start:2: Print mesh information diff --git a/tesseract_geometry/test/CMakeLists.txt b/tesseract_geometry/test/CMakeLists.txt index 97417328dc8..7494c4d0694 100644 --- a/tesseract_geometry/test/CMakeLists.txt +++ b/tesseract_geometry/test/CMakeLists.txt @@ -1,13 +1,7 @@ find_gtest() -find_package(tesseract_support REQUIRED) add_executable(${PROJECT_NAME}_serialization_unit tesseract_geometry_serialization_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_serialization_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_serialization_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_serialization_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_serialization_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -24,12 +18,7 @@ add_dependencies(${PROJECT_NAME}_serialization_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_serialization_unit) add_executable(${PROJECT_NAME}_unit tesseract_geometry_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp index 75f00529c7a..2d289534b13 100644 --- a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp @@ -31,11 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include #include -#include using namespace tesseract_geometry; @@ -62,8 +62,8 @@ TEST(TesseractGeometrySerializeUnit, Cone) // NOLINT TEST(TesseractGeometrySerializeUnit, ConvexMesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); tesseract_common::testSerialization(*object.front(), "ConvexMesh"); @@ -73,8 +73,8 @@ TEST(TesseractGeometrySerializeUnit, ConvexMesh) // NOLINT TEST(TesseractGeometrySerializeUnit, CompoundConvexMesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); std::vector meshes; @@ -96,8 +96,8 @@ TEST(TesseractGeometrySerializeUnit, Cylinder) // NOLINT TEST(TesseractGeometrySerializeUnit, Mesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); tesseract_common::testSerialization(*object.front(), "Mesh"); @@ -107,8 +107,8 @@ TEST(TesseractGeometrySerializeUnit, Mesh) // NOLINT TEST(TesseractGeometrySerializeUnit, CompoundMesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); std::vector meshes; @@ -165,8 +165,8 @@ TEST(TesseractGeometrySerializeUnit, Plane) // NOLINT TEST(TesseractGeometrySerializeUnit, PolygonMesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); tesseract_common::testSerialization(*object.front(), "PolygonMesh"); @@ -175,8 +175,8 @@ TEST(TesseractGeometrySerializeUnit, PolygonMesh) // NOLINT TEST(TesseractGeometrySerializeUnit, SDFMesh) // NOLINT { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; + std::string path = "package://tesseract_support/meshes/sphere_p25m.stl"; auto object = tesseract_geometry::createMeshFromResource( locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); tesseract_common::testSerialization(*object.front(), "SDFMesh"); diff --git a/tesseract_geometry/test/tesseract_geometry_unit.cpp b/tesseract_geometry/test/tesseract_geometry_unit.cpp index e3a7a348954..8fb9e3df5ff 100644 --- a/tesseract_geometry/test/tesseract_geometry_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_unit.cpp @@ -505,19 +505,20 @@ TEST(TesseractGeometryUnit, LoadMeshUnit) // NOLINT { using namespace tesseract_geometry; - std::string mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; + tesseract_common::GeneralResourceLocator locator; + std::string mesh_file = locator.locateResource("package://tesseract_support/meshes/sphere_p25m.stl")->getFilePath(); std::vector meshes = createMeshFromPath(mesh_file); EXPECT_TRUE(meshes.size() == 1); EXPECT_TRUE(meshes[0]->getFaceCount() == 80); EXPECT_TRUE(meshes[0]->getVertexCount() == 42); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(); meshes = createMeshFromPath(mesh_file); EXPECT_TRUE(meshes.size() == 1); EXPECT_TRUE(meshes[0]->getFaceCount() == 80); EXPECT_TRUE(meshes[0]->getVertexCount() == 42); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.dae"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/sphere_p25m.dae")->getFilePath(); meshes = createMeshFromPath(mesh_file); EXPECT_TRUE(meshes.size() == 2); EXPECT_TRUE(meshes[0]->getFaceCount() == 80); @@ -525,25 +526,25 @@ TEST(TesseractGeometryUnit, LoadMeshUnit) // NOLINT EXPECT_TRUE(meshes[1]->getFaceCount() == 80); EXPECT_TRUE(meshes[1]->getVertexCount() == 42); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.dae"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/sphere_p25m.dae")->getFilePath(); meshes = createMeshFromPath(mesh_file, Eigen::Vector3d(1, 1, 1), false, true); EXPECT_TRUE(meshes.size() == 1); EXPECT_TRUE(meshes[0]->getFaceCount() == 2 * 80); EXPECT_TRUE(meshes[0]->getVertexCount() == 2 * 42); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/box_2m.ply")->getFilePath(); meshes = createMeshFromPath(mesh_file, Eigen::Vector3d(1, 1, 1), true, true); EXPECT_TRUE(meshes.size() == 1); EXPECT_TRUE(meshes[0]->getFaceCount() == 12); EXPECT_TRUE(meshes[0]->getVertexCount() == 8); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/box_2m.ply")->getFilePath(); meshes = createMeshFromPath(mesh_file, Eigen::Vector3d(1, 1, 1), true, true); EXPECT_TRUE(meshes.size() == 1); EXPECT_TRUE(meshes[0]->getFaceCount() == 12); EXPECT_TRUE(meshes[0]->getVertexCount() == 8); - mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply"; + mesh_file = locator.locateResource("package://tesseract_support/meshes/box_2m.ply")->getFilePath(); std::vector convex_meshes = createMeshFromPath(mesh_file, Eigen::Vector3d(1, 1, 1), false, false); EXPECT_TRUE(convex_meshes.size() == 1); @@ -557,7 +558,9 @@ TEST(TesseractGeometryUnit, LoadMeshWithMaterialGltf2Unit) // NOLINT { using namespace tesseract_geometry; - std::string mesh_file = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/tesseract_material_mesh.glb"; + tesseract_common::GeneralResourceLocator locator; + std::string mesh_file = + locator.locateResource("package://tesseract_support/meshes/tesseract_material_mesh.glb")->getFilePath(); std::vector meshes = createMeshFromPath(mesh_file, Eigen::Vector3d(1, 1, 1), true, true, true, true, true); ASSERT_TRUE(meshes.size() == 4); diff --git a/tesseract_kinematics/test/CMakeLists.txt b/tesseract_kinematics/test/CMakeLists.txt index a7aa63cdd84..ff01b53f239 100644 --- a/tesseract_kinematics/test/CMakeLists.txt +++ b/tesseract_kinematics/test/CMakeLists.txt @@ -1,5 +1,5 @@ find_gtest() -find_package(tesseract_support REQUIRED) + find_package(tesseract_urdf REQUIRED) find_package(LAPACK REQUIRED) # Required for ikfast @@ -10,7 +10,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_core ${PROJECT_NAME}_kdl - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph tesseract::tesseract_common) @@ -35,7 +34,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_kdl - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph tesseract::tesseract_common) @@ -61,7 +59,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_opw - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_opw_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -86,7 +83,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ikfast - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_ikfast_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -110,7 +106,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ikfast - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_ikfast_7dof_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -134,7 +129,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_opw - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_rop_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -159,7 +153,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_opw - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_rep_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -184,7 +177,6 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ur - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_ur_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} @@ -208,7 +200,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_core - tesseract::tesseract_support tesseract::tesseract_urdf tesseract::tesseract_scene_graph) target_compile_options(${PROJECT_NAME}_factory_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} diff --git a/tesseract_kinematics/test/ikfast_kinematics_7dof_unit.cpp b/tesseract_kinematics/test/ikfast_kinematics_7dof_unit.cpp index 1577417674c..5d2db17f7c2 100644 --- a/tesseract_kinematics/test/ikfast_kinematics_7dof_unit.cpp +++ b/tesseract_kinematics/test/ikfast_kinematics_7dof_unit.cpp @@ -48,7 +48,8 @@ TEST(TesseractKinematicsUnit, IKFastInvKin7DOF) // NOLINT Eigen::VectorXd seed = Eigen::VectorXd::Zero(7); // Setup test - auto scene_graph = getSceneGraphIIWA7(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphIIWA7(locator); std::string base_link_name = "link_0"; std::string tip_link_name = "ikfast_tcp_link"; std::vector joint_names{ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7" }; diff --git a/tesseract_kinematics/test/ikfast_kinematics_unit.cpp b/tesseract_kinematics/test/ikfast_kinematics_unit.cpp index 2bd2e73ca28..41713f1d9ef 100644 --- a/tesseract_kinematics/test/ikfast_kinematics_unit.cpp +++ b/tesseract_kinematics/test/ikfast_kinematics_unit.cpp @@ -48,7 +48,8 @@ TEST(TesseractKinematicsUnit, IKFastInvKin) // NOLINT Eigen::VectorXd seed = Eigen::VectorXd::Zero(6); // Setup test - auto scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphABB(locator); std::string base_link_name = "base_link"; std::string tip_link_name = "tool0"; std::vector joint_names{ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; diff --git a/tesseract_kinematics/test/kdl_kinematics_unit.cpp b/tesseract_kinematics/test/kdl_kinematics_unit.cpp index c86d81ba5df..17650adf8e8 100644 --- a/tesseract_kinematics/test/kdl_kinematics_unit.cpp +++ b/tesseract_kinematics/test/kdl_kinematics_unit.cpp @@ -15,7 +15,8 @@ using namespace tesseract_kinematics::test_suite; TEST(TesseractKinematicsUnit, KDLKinChainLMAInverseKinematicUnit) // NOLINT { - auto scene_graph = getSceneGraphIIWA(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphIIWA(locator); tesseract_kinematics::KDLInvKinChainLMA::Config config; tesseract_kinematics::KDLInvKinChainLMA derived_kin(*scene_graph, "base_link", "tool0", config); @@ -26,7 +27,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainLMAInverseKinematicUnit) // NOLINT TEST(TesseractKinematicsUnit, KDLKinChainNRInverseKinematicUnit) // NOLINT { - auto scene_graph = getSceneGraphIIWA(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphIIWA(locator); tesseract_kinematics::KDLInvKinChainNR::Config config; tesseract_kinematics::KDLInvKinChainNR derived_kin(*scene_graph, "base_link", "tool0", config); @@ -37,7 +39,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainNRInverseKinematicUnit) // NOLINT TEST(TesseractKinematicsUnit, KDLKinChainNR_JLInverseKinematicUnit) // NOLINT { - auto scene_graph = getSceneGraphIIWA(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphIIWA(locator); tesseract_kinematics::KDLInvKinChainNR_JL::Config config; tesseract_kinematics::KDLInvKinChainNR_JL derived_kin(*scene_graph, "base_link", "tool0", config); diff --git a/tesseract_kinematics/test/kinematics_core_unit.cpp b/tesseract_kinematics/test/kinematics_core_unit.cpp index d83005cede1..ecaf4e17429 100644 --- a/tesseract_kinematics/test/kinematics_core_unit.cpp +++ b/tesseract_kinematics/test/kinematics_core_unit.cpp @@ -254,7 +254,8 @@ TEST(TesseractKinematicsUnit, RedundantSolutionsUnit) // NOLINT TEST(TesseractKinematicsUnit, UtilsNearSingularityUnit) // NOLINT { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(locator); tesseract_kinematics::KDLFwdKinChain fwd_kin(*scene_graph, "base_link", "tool0"); @@ -279,7 +280,8 @@ TEST(TesseractKinematicsUnit, UtilsNearSingularityUnit) // NOLINT TEST(TesseractKinematicsUnit, UtilscalcManipulabilityUnit) // NOLINT { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(locator); tesseract_kinematics::KDLFwdKinChain fwd_kin(*scene_graph, "base_link", "tool0"); diff --git a/tesseract_kinematics/test/kinematics_factory_unit.cpp b/tesseract_kinematics/test/kinematics_factory_unit.cpp index 990b789890d..d505f87de2d 100644 --- a/tesseract_kinematics/test/kinematics_factory_unit.cpp +++ b/tesseract_kinematics/test/kinematics_factory_unit.cpp @@ -38,11 +38,12 @@ using namespace tesseract_kinematics; void runKinematicsFactoryTest(const tesseract_common::fs::path& config_path) { - tesseract_scene_graph::SceneGraph::UPtr iiwa_scene_graph = getSceneGraphIIWA(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr iiwa_scene_graph = getSceneGraphIIWA(locator); tesseract_scene_graph::KDLStateSolver iiwa_state_solver(*iiwa_scene_graph); tesseract_scene_graph::SceneState iiwa_scene_state = iiwa_state_solver.getState(); - tesseract_scene_graph::SceneGraph::UPtr abb_scene_graph = getSceneGraphABB(); + tesseract_scene_graph::SceneGraph::UPtr abb_scene_graph = getSceneGraphABB(locator); tesseract_scene_graph::KDLStateSolver abb_state_solver(*abb_scene_graph); tesseract_scene_graph::SceneState abb_scene_state = abb_state_solver.getState(); @@ -50,11 +51,11 @@ void runKinematicsFactoryTest(const tesseract_common::fs::path& config_path) tesseract_scene_graph::KDLStateSolver ur_state_solver(*ur_scene_graph); tesseract_scene_graph::SceneState ur_scene_state = ur_state_solver.getState(); - tesseract_scene_graph::SceneGraph::UPtr rop_scene_graph = getSceneGraphABBOnPositioner(); + tesseract_scene_graph::SceneGraph::UPtr rop_scene_graph = getSceneGraphABBOnPositioner(locator); tesseract_scene_graph::KDLStateSolver rop_state_solver(*rop_scene_graph); tesseract_scene_graph::SceneState rop_scene_state = rop_state_solver.getState(); - tesseract_scene_graph::SceneGraph::UPtr rep_scene_graph = getSceneGraphABBExternalPositioner(); + tesseract_scene_graph::SceneGraph::UPtr rep_scene_graph = getSceneGraphABBExternalPositioner(locator); tesseract_scene_graph::KDLStateSolver rep_state_solver(*rep_scene_graph); tesseract_scene_graph::SceneState rep_scene_state = rep_state_solver.getState(); @@ -260,7 +261,8 @@ TEST(TesseractKinematicsFactoryUnit, LoadKinematicsPluginInfoUnit) // NOLINT { using namespace tesseract_scene_graph; - tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); @@ -317,7 +319,8 @@ TEST(TesseractKinematicsFactoryUnit, LoadOPWKinematicsUnit) // NOLINT { using namespace tesseract_scene_graph; - tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); @@ -840,7 +843,9 @@ TEST(TesseractKinematicsFactoryUnit, LoadURKinematicsUnit) // NOLINT TEST(TesseractKinematicsFactoryUnit, LoadREPKinematicsUnit) // NOLINT { using namespace tesseract_scene_graph; - tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABBExternalPositioner(); + + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABBExternalPositioner(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); @@ -973,7 +978,9 @@ TEST(TesseractKinematicsFactoryUnit, LoadREPKinematicsUnit) // NOLINT TEST(TesseractKinematicsFactoryUnit, LoadROPKinematicsUnit) // NOLINT { using namespace tesseract_scene_graph; - tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABBOnPositioner(); + + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABBOnPositioner(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); @@ -1105,7 +1112,8 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT { using namespace tesseract_scene_graph; - tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphABB(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); diff --git a/tesseract_kinematics/test/kinematics_test_utils.h b/tesseract_kinematics/test/kinematics_test_utils.h index b70562724d2..fb26593fad9 100644 --- a/tesseract_kinematics/test/kinematics_test_utils.h +++ b/tesseract_kinematics/test/kinematics_test_utils.h @@ -48,47 +48,41 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include namespace tesseract_kinematics::test_suite { -inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA() +inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } -inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBExternalPositioner() +inline tesseract_scene_graph::SceneGraph::UPtr +getSceneGraphABBExternalPositioner(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400_external_positioner.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = + locator.locateResource("package://tesseract_support/urdf/abb_irb2400_external_positioner.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } -inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBOnPositioner() +inline tesseract_scene_graph::SceneGraph::UPtr +getSceneGraphABBOnPositioner(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400_on_positioner.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = + locator.locateResource("package://tesseract_support/urdf/abb_irb2400_on_positioner.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } -inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABB() +inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABB(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } -inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA7() +inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA7(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/iiwa7.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/urdf/iiwa7.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } @@ -968,7 +962,8 @@ inline void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactor const std::string& inv_factory_name, const std::string& fwd_factory_name) { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraphIIWA(); + tesseract_common::GeneralResourceLocator locator; + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraphIIWA(locator); std::string manip_name = "manip"; std::string base_link_name = "base_link"; std::string tip_link_name = "tool0"; diff --git a/tesseract_kinematics/test/opw_kinematics_unit.cpp b/tesseract_kinematics/test/opw_kinematics_unit.cpp index 9b9d0cffb4d..6a0545e42a8 100644 --- a/tesseract_kinematics/test/opw_kinematics_unit.cpp +++ b/tesseract_kinematics/test/opw_kinematics_unit.cpp @@ -66,7 +66,8 @@ TEST(TesseractKinematicsUnit, OPWInvKinUnit) // NOLINT Eigen::VectorXd seed = Eigen::VectorXd::Zero(6); // Setup test - auto scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphABB(locator); std::string manip_name = "manip"; std::string base_link_name = "base_link"; std::string tip_link_name = "tool0"; @@ -114,7 +115,8 @@ TEST(TesseractKinematicsUnit, OPWInvKinGroupUnit) // NOLINT Eigen::VectorXd seed = Eigen::VectorXd::Zero(6); // Setup test - auto scene_graph = getSceneGraphABB(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphABB(locator); std::string manip_name = "manip"; std::string base_link_name = "base_link"; std::string tip_link_name = "tool0"; diff --git a/tesseract_kinematics/test/rep_kinematics_unit.cpp b/tesseract_kinematics/test/rep_kinematics_unit.cpp index 0ac2e9b6327..1980b9b0494 100644 --- a/tesseract_kinematics/test/rep_kinematics_unit.cpp +++ b/tesseract_kinematics/test/rep_kinematics_unit.cpp @@ -102,7 +102,8 @@ InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneG TEST(TesseractKinematicsUnit, RobotWithExternalPositionerInverseKinematicUnit) // NOLINT { - auto scene_graph = getSceneGraphABBExternalPositioner(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphABBExternalPositioner(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); diff --git a/tesseract_kinematics/test/rop_kinematics_unit.cpp b/tesseract_kinematics/test/rop_kinematics_unit.cpp index 99f5cbfbc05..a23d2dfd625 100644 --- a/tesseract_kinematics/test/rop_kinematics_unit.cpp +++ b/tesseract_kinematics/test/rop_kinematics_unit.cpp @@ -101,7 +101,8 @@ InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneG TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit) // NOLINT { - auto scene_graph = getSceneGraphABBOnPositioner(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraphABBOnPositioner(locator); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); diff --git a/tesseract_scene_graph/test/CMakeLists.txt b/tesseract_scene_graph/test/CMakeLists.txt index e571705d9d1..0f6fae1ce95 100644 --- a/tesseract_scene_graph/test/CMakeLists.txt +++ b/tesseract_scene_graph/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_gtest() -find_package(tesseract_support REQUIRED) add_executable(${PROJECT_NAME}_unit tesseract_scene_graph_unit.cpp) target_link_libraries( @@ -7,7 +6,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -30,7 +28,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_joint_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -52,7 +49,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_link_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -74,7 +70,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_serialization_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_srdf/examples/CMakeLists.txt b/tesseract_srdf/examples/CMakeLists.txt index 8fb627414e1..976ac8af302 100644 --- a/tesseract_srdf/examples/CMakeLists.txt +++ b/tesseract_srdf/examples/CMakeLists.txt @@ -1,4 +1,3 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_common REQUIRED) add_executable(${PROJECT_NAME}_parse_srdf_example parse_srdf_example.cpp) @@ -6,8 +5,7 @@ target_link_libraries( ${PROJECT_NAME}_parse_srdf_example ${PROJECT_NAME} tesseract::tesseract_common - console_bridge::console_bridge - tesseract::tesseract_support) + console_bridge::console_bridge) target_compile_options(${PROJECT_NAME}_parse_srdf_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_clang_tidy(${PROJECT_NAME}_parse_srdf_example ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) diff --git a/tesseract_srdf/examples/parse_srdf_example.cpp b/tesseract_srdf/examples/parse_srdf_example.cpp index cd0c0d06be5..231b4d6e51d 100644 --- a/tesseract_srdf/examples/parse_srdf_example.cpp +++ b/tesseract_srdf/examples/parse_srdf_example.cpp @@ -7,7 +7,6 @@ #include #include #include -#include using namespace tesseract_scene_graph; using namespace tesseract_srdf; @@ -103,7 +102,7 @@ int main(int /*argc*/, char** /*argv*/) // documentation:end:1: Create scene graph // documentation:start:2: Get the srdf file path - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; std::string srdf_file = locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); // documentation:end:2: Get the srdf file path diff --git a/tesseract_srdf/test/CMakeLists.txt b/tesseract_srdf/test/CMakeLists.txt index 2925bb46e66..b99b09e4bd3 100644 --- a/tesseract_srdf/test/CMakeLists.txt +++ b/tesseract_srdf/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_gtest() -find_package(tesseract_support REQUIRED) add_executable(${PROJECT_NAME}_serialization_unit tesseract_srdf_serialization_unit.cpp) target_link_libraries( @@ -7,7 +6,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_serialization_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -30,7 +28,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp index edc14bfd933..f816beeb44a 100644 --- a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp @@ -34,7 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include #include #include #include @@ -131,10 +130,9 @@ SceneGraph getSceneGraph() return g; } -SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) +SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); auto srdf = std::make_shared(); srdf->initFile(scene_graph, path, locator); @@ -144,17 +142,18 @@ SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) TEST(TesseractSRDFSerializeUnit, KinematicsInformation) // NOLINT { + GeneralResourceLocator locator; auto graph = getSceneGraph(); - auto srdf = getSRDFModel(graph); + auto srdf = getSRDFModel(graph, locator); tesseract_common::testSerialization(srdf->kinematics_information, "KinematicsInformation"); } TEST(TesseractSRDFSerializeUnit, SRDFModel) // NOLINT { - TesseractSupportResourceLocator locator; + GeneralResourceLocator locator; auto graph = getSceneGraph(); - auto srdf = getSRDFModel(graph); + auto srdf = getSRDFModel(graph, locator); tesseract_common::testSerialization(*srdf, "SRDFModel"); } diff --git a/tesseract_srdf/test/tesseract_srdf_unit.cpp b/tesseract_srdf/test/tesseract_srdf_unit.cpp index d62b60f03a9..a9cc6b392d4 100644 --- a/tesseract_srdf/test/tesseract_srdf_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_unit.cpp @@ -20,7 +20,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include #include #include #include @@ -238,9 +237,10 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit) // NOLINT using namespace tesseract_srdf; using namespace tesseract_common; - std::string srdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; + GeneralResourceLocator locator; + std::string srdf_file = + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); - TesseractSupportResourceLocator locator; SceneGraph g; g.setName("kuka_lbr_iiwa_14_r820"); @@ -355,7 +355,7 @@ TEST(TesseractSRDFUnit, TesseractSRDFModelUnit) // NOLINT using namespace tesseract_srdf; using namespace tesseract_common; - TesseractSupportResourceLocator locator; + GeneralResourceLocator locator; SRDFModel srdf; // Set Name @@ -447,7 +447,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFailureCasesUnit) // NOLINT using namespace tesseract_srdf; using namespace tesseract_common; - TesseractSupportResourceLocator locator; + GeneralResourceLocator locator; SceneGraph::Ptr g = getABBSceneGraph(); { // Success @@ -1977,7 +1977,7 @@ TEST(TesseractSRDFUnit, AddRemoveGroupTCPUnit) // NOLINT TEST(TesseractSRDFUnit, ParseConfigFilePathUnit) // NOLINT { std::array version{ 1, 0, 0 }; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph(); { // valid @@ -2062,7 +2062,7 @@ TEST(TesseractSRDFUnit, ParseConfigFilePathUnit) // NOLINT TEST(TesseractSRDFUnit, ParseContactManagersPluginConfigUnit) // NOLINT { std::array version{ 1, 0, 0 }; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph(); { // valid @@ -2128,7 +2128,7 @@ TEST(TesseractSRDFUnit, ParseContactManagersPluginConfigUnit) // NOLINT TEST(TesseractSRDFUnit, ParseKinematicsPluginConfigUnit) // NOLINT { std::array version{ 1, 0, 0 }; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph(); { // valid @@ -2214,7 +2214,7 @@ TEST(TesseractSRDFUnit, ParseKinematicsPluginConfigUnit) // NOLINT TEST(TesseractSRDFUnit, ParseCalibrationConfigUnit) // NOLINT { std::array version{ 1, 0, 0 }; - tesseract_common::TesseractSupportResourceLocator locator; + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::SceneGraph::Ptr g = getABBSceneGraph(); { // valid diff --git a/tesseract_state_solver/test/CMakeLists.txt b/tesseract_state_solver/test/CMakeLists.txt index 832a1f1780f..f02ddc7d05c 100644 --- a/tesseract_state_solver/test/CMakeLists.txt +++ b/tesseract_state_solver/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_gtest() -find_package(tesseract_support REQUIRED) find_package(tesseract_urdf REQUIRED) add_executable(${PROJECT_NAME}_unit tesseract_state_solver_ofkt_unit.cpp) @@ -9,8 +8,7 @@ target_link_libraries( GTest::Main ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ofkt - tesseract::tesseract_urdf - tesseract::tesseract_support) + tesseract::tesseract_urdf) target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_state_solver/test/state_solver_test_suite.h b/tesseract_state_solver/test/state_solver_test_suite.h index 484bd8e5fe1..9a71f9adc38 100644 --- a/tesseract_state_solver/test/state_solver_test_suite.h +++ b/tesseract_state_solver/test/state_solver_test_suite.h @@ -16,15 +16,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include namespace tesseract_scene_graph::test_suite { -inline SceneGraph::UPtr getSceneGraph() +inline SceneGraph::UPtr getSceneGraph(const tesseract_common::ResourceLocator& locator) { - std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; - - tesseract_common::TesseractSupportResourceLocator locator; + std::string path = locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); return tesseract_urdf::parseURDFFile(path, locator); } @@ -360,7 +357,8 @@ template inline void runJacobianTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); StateSolver::UPtr state_solver_clone = state_solver.clone(); @@ -740,7 +738,8 @@ template void runAddandRemoveLinkTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); auto visual = std::make_shared(); @@ -1010,7 +1009,8 @@ template void runAddSceneGraphTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); auto subgraph = std::make_unique(); @@ -1151,7 +1151,8 @@ template void runChangeJointOriginTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); const std::string link_name1 = "link_n1"; @@ -1221,7 +1222,8 @@ template void runMoveJointTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); const std::string link_name1 = "link_n1"; @@ -1337,7 +1339,8 @@ template void runMoveLinkTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); const std::string link_name1 = "link_n1"; @@ -1461,7 +1464,8 @@ template void runChangeJointLimitsTest() { // Get the scene graph - auto scene_graph = getSceneGraph(); + tesseract_common::GeneralResourceLocator locator; + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); double new_lower = 1.0; @@ -1557,9 +1561,10 @@ void runChangeJointLimitsTest() template void runReplaceJointTest() { + tesseract_common::GeneralResourceLocator locator; { // Replace joint with same type // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint joint_1("joint_a1"); @@ -1584,7 +1589,7 @@ void runReplaceJointTest() { // Replace joint which exist but the link does not which should fail // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint joint_1("joint_a1"); @@ -1607,7 +1612,7 @@ void runReplaceJointTest() { // Replace joint with same type but change transform // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); @@ -1629,7 +1634,7 @@ void runReplaceJointTest() { // Replace joint with different type (Fixed) // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); @@ -1652,7 +1657,7 @@ void runReplaceJointTest() { // Replace joint with different type (Continuous) // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); @@ -1675,7 +1680,7 @@ void runReplaceJointTest() { // Replace joint with different type (Prismatic) // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); @@ -1698,7 +1703,7 @@ void runReplaceJointTest() { // Replace joint with different parent which is a replace and move // Get the scene graph - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone(); @@ -1720,7 +1725,7 @@ void runReplaceJointTest() } { // Replace joint which does not exist which should fail - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone("joint_does_not_exist"); @@ -1738,7 +1743,7 @@ void runReplaceJointTest() } { // Replace joint where parent link does not exist - auto scene_graph = getSceneGraph(); + auto scene_graph = getSceneGraph(locator); auto state_solver = S(*scene_graph); Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone(); diff --git a/tesseract_support/CMakeLists.txt b/tesseract_support/CMakeLists.txt index 6b594c2b223..50f0aa7b73b 100644 --- a/tesseract_support/CMakeLists.txt +++ b/tesseract_support/CMakeLists.txt @@ -5,52 +5,17 @@ find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) project(${pkg_extracted_name} VERSION ${pkg_extracted_version}) -find_package(tesseract_common REQUIRED) - # Load variable for clang tidy args, compiler options and cxx version +find_package(tesseract_common REQUIRED) tesseract_variables() -initialize_code_coverage(ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -set(COVERAGE_EXCLUDE - /usr/* - /opt/* - ${CMAKE_CURRENT_LIST_DIR}/test/* - /*/gtest/* - /*/bullet/LinearMath/* - /*/bullet/BulletCollision/*) -add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) - -add_library(${PROJECT_NAME} src/tesseract_support_resource_locator.cpp) -target_link_libraries(${PROJECT_NAME} PUBLIC tesseract::tesseract_common) -target_compile_options(${PROJECT_NAME} PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) -target_compile_options(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS} - TESSERACT_SUPPORT_DIR="${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}") -target_clang_tidy(${PROJECT_NAME} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME} - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") - -configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME} DEPENDENCIES tesseract_common) - -install(DIRECTORY include/${PROJECT_NAME} DESTINATION include) +# Configure Package +configure_package() foreach(dir urdf meshes) install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir}) endforeach() -if(TESSERACT_ENABLE_TESTING OR TESSERACT_SUPPORT_ENABLE_TESTING) - enable_testing() - add_run_tests_target(ENABLE ${TESSERACT_ENABLE_RUN_TESTING}) - add_subdirectory(test) -endif() - if(TESSERACT_PACKAGE) cpack( VERSION ${pkg_extracted_version} @@ -58,9 +23,7 @@ if(TESSERACT_PACKAGE) MAINTAINER_EMAIL ${pkg_extracted_maintainer_email} DESCRIPTION ${pkg_extracted_description} LICENSE_FILE ${CMAKE_CURRENT_LIST_DIR}/../LICENSE - README_FILE ${CMAKE_CURRENT_LIST_DIR}/../README.md - LINUX_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-common" - WINDOWS_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-common") + README_FILE ${CMAKE_CURRENT_LIST_DIR}/../README.md) if(UNIX AND TESSERACT_PACKAGE_SOURCE) cpack_debian_source_package( diff --git a/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h b/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h deleted file mode 100644 index 6bd0cd9cf93..00000000000 --- a/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h +++ /dev/null @@ -1,62 +0,0 @@ -/** - * @file tesseract_support_resource_locator.h - * @brief Locate and retrieve resource data in tesseract_support - * - * @author Levi Armstrong - * @date March 31, 2022 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2022, Levi Armstrong - * - * @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_SUPPORT_TESSERACT_SUPPORT_RESOURCE_LOCATOR_H -#define TESSERACT_SUPPORT_TESSERACT_SUPPORT_RESOURCE_LOCATOR_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace boost::serialization -{ -class access; -} - -namespace tesseract_common -{ -/** @brief Abstract class for resource loaders */ -class TesseractSupportResourceLocator : public ResourceLocator -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - std::shared_ptr locateResource(const std::string& url) const override final; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; -} // namespace tesseract_common - -BOOST_CLASS_EXPORT_KEY(tesseract_common::TesseractSupportResourceLocator) - -#endif // TESSERACT_SUPPORT_TESSERACT_SUPPORT_RESOURCE_LOCATOR_H diff --git a/tesseract_support/package.xml b/tesseract_support/package.xml index 68baf17337c..e738361dd90 100644 --- a/tesseract_support/package.xml +++ b/tesseract_support/package.xml @@ -9,11 +9,8 @@ cmake - - tesseract_common ros_industrial_cmake_boilerplate - - gtest + tesseract_common cmake diff --git a/tesseract_support/src/tesseract_support_resource_locator.cpp b/tesseract_support/src/tesseract_support_resource_locator.cpp deleted file mode 100644 index da73f1c0ea0..00000000000 --- a/tesseract_support/src/tesseract_support_resource_locator.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/** - * @file tesseract_support_resource_locator.h - * @brief Locate and retrieve resource data in tesseract_support - * - * @author Levi Armstrong - * @date March 31, 2022 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2022, Levi Armstrong - * - * @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 -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_common -{ -std::shared_ptr TesseractSupportResourceLocator::locateResource(const std::string& url) const -{ - std::string mod_url = url; - if (url.find("package://tesseract_support") == 0) - { - mod_url.erase(0, strlen("package://tesseract_support")); - size_t pos = mod_url.find('/'); - if (pos == std::string::npos) - return nullptr; - - std::string package = mod_url.substr(0, pos); - mod_url.erase(0, pos); - std::string package_path = std::string(TESSERACT_SUPPORT_DIR); - - if (package_path.empty()) - return nullptr; - - mod_url = package_path + mod_url; - } - - if (!tesseract_common::fs::path(mod_url).is_absolute()) - return nullptr; - - return std::make_shared( - url, mod_url, std::make_shared(*this)); -} - -template -void TesseractSupportResourceLocator::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ResourceLocator); -} - -} // namespace tesseract_common - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::TesseractSupportResourceLocator) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::TesseractSupportResourceLocator) diff --git a/tesseract_support/test/CMakeLists.txt b/tesseract_support/test/CMakeLists.txt deleted file mode 100644 index b344ede12cf..00000000000 --- a/tesseract_support/test/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -find_gtest() - -add_executable(${PROJECT_NAME}_unit tesseract_support_unit.cpp) -target_link_libraries(${PROJECT_NAME}_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) -target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} - ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS} - INSTALL_LOCATION="${CMAKE_INSTALL_PREFIX}") -target_clang_tidy(${PROJECT_NAME}_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_unit PUBLIC VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_unit) -add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME}) -add_dependencies(run_tests ${PROJECT_NAME}_unit) diff --git a/tesseract_support/test/tesseract_support_unit.cpp b/tesseract_support/test/tesseract_support_unit.cpp deleted file mode 100644 index b9c45fcb1b6..00000000000 --- a/tesseract_support/test/tesseract_support_unit.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -TEST(TesseractSupportUnit, TesseractSupportResourceLocatorUnit) // NOLINT -{ - using namespace tesseract_common; - tesseract_common::fs::path file_path(INSTALL_LOCATION); - tesseract_common::fs::path package_path = file_path / "share/tesseract_support"; - - ResourceLocator::Ptr locator = std::make_shared(); - - Resource::Ptr resource = locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf"); - EXPECT_TRUE(resource != nullptr); - EXPECT_TRUE(resource->isFile()); - EXPECT_EQ(resource->getUrl(), "package://tesseract_support/urdf/abb_irb2400.urdf"); - EXPECT_EQ(tesseract_common::fs::path(resource->getFilePath()), (package_path / "urdf/abb_irb2400.urdf")); - EXPECT_FALSE(resource->getResourceContents().empty()); - EXPECT_TRUE(resource->getResourceContentStream() != nullptr); - - Resource::Ptr sub_resource = resource->locateResource("boxbot.urdf"); - EXPECT_TRUE(sub_resource != nullptr); - EXPECT_TRUE(sub_resource->isFile()); - EXPECT_EQ(sub_resource->getUrl(), "package://tesseract_support/urdf/boxbot.urdf"); - EXPECT_EQ(tesseract_common::fs::path(sub_resource->getFilePath()), (package_path / "urdf/boxbot.urdf")); - EXPECT_FALSE(sub_resource->getResourceContents().empty()); - EXPECT_TRUE(sub_resource->getResourceContentStream() != nullptr); - - tesseract_common::Resource::Ptr resource_empty = locator->locateResource(""); - EXPECT_TRUE(resource_empty == nullptr); - - tesseract_common::Resource::Ptr resource_does_not_exist = locator->locateResource("package://tesseract_support/urdf/" - "does_not_exist.urdf"); - EXPECT_TRUE(resource_does_not_exist != nullptr); - EXPECT_TRUE(resource_does_not_exist->getResourceContents().empty()); - EXPECT_TRUE(resource_does_not_exist->getResourceContentStream() == nullptr); -} - -TEST(TesseractSupportUnit, TesseractSupportResourceLocatorSerializUnit) // NOLINT -{ - using namespace tesseract_common; - ResourceLocator::Ptr locator = std::make_shared(); - tesseract_common::testSerializationDerivedClass(locator, - "TesseractSupportRe" - "sourceLocator"); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/tesseract_urdf/examples/CMakeLists.txt b/tesseract_urdf/examples/CMakeLists.txt index 3a2623eee08..71bd5ae88b4 100644 --- a/tesseract_urdf/examples/CMakeLists.txt +++ b/tesseract_urdf/examples/CMakeLists.txt @@ -1,12 +1,7 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_geometry REQUIRED) add_executable(${PROJECT_NAME}_load_urdf_example load_urdf_example.cpp) -target_link_libraries( - ${PROJECT_NAME}_load_urdf_example - ${PROJECT_NAME} - console_bridge::console_bridge - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_load_urdf_example ${PROJECT_NAME} console_bridge::console_bridge) target_compile_options(${PROJECT_NAME}_load_urdf_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_clang_tidy(${PROJECT_NAME}_load_urdf_example ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) diff --git a/tesseract_urdf/examples/load_urdf_example.cpp b/tesseract_urdf/examples/load_urdf_example.cpp index c8635fef745..883fdca46b6 100644 --- a/tesseract_urdf/examples/load_urdf_example.cpp +++ b/tesseract_urdf/examples/load_urdf_example.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include using namespace tesseract_scene_graph; using namespace tesseract_urdf; @@ -23,11 +23,12 @@ std::string toString(bool b) { return b ? "true" : "false"; } int main(int /*argc*/, char** /*argv*/) { // documentation:start:2: Get the urdf file path - std::string urdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + tesseract_common::GeneralResourceLocator locator; + std::string urdf_file = + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); // documentation:end:2: Get the urdf file path // documentation:start:3: Create scene graph - tesseract_common::TesseractSupportResourceLocator locator; SceneGraph::Ptr g = parseURDFFile(urdf_file, locator); // documentation:end:3: Create scene graph diff --git a/tesseract_urdf/test/CMakeLists.txt b/tesseract_urdf/test/CMakeLists.txt index e9dfc013044..efce108c906 100644 --- a/tesseract_urdf/test/CMakeLists.txt +++ b/tesseract_urdf/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_gtest() -find_package(tesseract_support REQUIRED) add_executable( ${PROJECT_NAME}_unit @@ -36,7 +35,6 @@ target_link_libraries( PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME} - tesseract::tesseract_support tesseract::tesseract_geometry) target_include_directories(${PROJECT_NAME}_unit PRIVATE "$") target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} diff --git a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp index ac025f429c8..668da446887 100644 --- a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp @@ -7,12 +7,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_collision) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"( diff --git a/tesseract_urdf/test/tesseract_urdf_common_unit.h b/tesseract_urdf/test/tesseract_urdf_common_unit.h index 239f8d56826..fc32790de86 100644 --- a/tesseract_urdf/test/tesseract_urdf_common_unit.h +++ b/tesseract_urdf/test/tesseract_urdf_common_unit.h @@ -16,7 +16,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include template bool runTest(ElementType& type, diff --git a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp index 88b93ad2ab8..0f59d8d60e4 100644 --- a/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_convex_mesh_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include "tesseract_urdf_common_unit.h" static std::string getTempPkgPath() @@ -22,7 +22,7 @@ static std::string getTempPkgPath() TEST(TesseractURDFUnit, parse_convex_mesh) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"()"; diff --git a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp index 3cce1a7cbcc..e4ff692031e 100644 --- a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp @@ -7,12 +7,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_geometry) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"( diff --git a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp index 3a6ee49d887..3c8aaae12b6 100644 --- a/tesseract_urdf/test/tesseract_urdf_link_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_link_unit.cpp @@ -7,12 +7,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_link) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::unordered_map empty_available_materials; diff --git a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp index 00a331076de..57ea6710b32 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp @@ -7,12 +7,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_mesh_material_dae) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"()"; std::vector meshes; @@ -86,7 +86,7 @@ TEST(TesseractURDFUnit, parse_mesh_material_dae) // NOLINT #ifdef TESSERACT_ASSIMP_USE_PBRMATERIAL TEST(TesseractURDFUnit, parse_mesh_material_gltf2) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"()"; std::vector meshes; diff --git a/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp index 723b8d38eab..290b610dbf0 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include "tesseract_urdf_common_unit.h" static std::string getTempPkgPath() @@ -22,7 +22,7 @@ static std::string getTempPkgPath() TEST(TesseractURDFUnit, parse_mesh) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"()"; diff --git a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp index 3aabb16ba86..aecfd81199c 100644 --- a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp @@ -9,12 +9,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_octree) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; // { // // Create octomap and add save it // pcl::PointCloud full_cloud; diff --git a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp index 1ecc1f4a1d2..a0fe2578046 100644 --- a/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_sdf_mesh_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include "tesseract_urdf_common_unit.h" static std::string getTempPkgPath() @@ -22,7 +22,7 @@ static std::string getTempPkgPath() TEST(TesseractURDFUnit, parse_sdf_mesh) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"()"; diff --git a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp index 915a5fdce9a..010d5dffba7 100644 --- a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp @@ -8,12 +8,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_urdf) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"( @@ -232,24 +232,16 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT } { - tesseract_scene_graph::SceneGraph::Ptr sg = tesseract_urdf::parseURDFFile(std::string(TESSERACT_SUPPORT_DIR) + "/ur" - "df/" - "lbr" - "_ii" - "wa_" - "14_" - "r82" - "0." - "urd" - "f", - resource_locator); + const std::string path = + resource_locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); + tesseract_scene_graph::SceneGraph::Ptr sg = tesseract_urdf::parseURDFFile(path, resource_locator); EXPECT_TRUE(sg != nullptr); } } TEST(TesseractURDFUnit, parse_urdf_with_available_materials) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::string str = R"( @@ -413,9 +405,10 @@ TEST(TesseractURDFUnit, LoadURDFUnit) // NOLINT { using namespace tesseract_scene_graph; - std::string urdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + tesseract_common::GeneralResourceLocator locator; + std::string urdf_file = + locator.locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); - tesseract_common::TesseractSupportResourceLocator locator; auto g = tesseract_urdf::parseURDFFile(urdf_file, locator); EXPECT_TRUE(g->getJoints().size() == 9); diff --git a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp index 7d8fd3af662..53df03e9be7 100644 --- a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp @@ -8,12 +8,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "tesseract_urdf_common_unit.h" TEST(TesseractURDFUnit, parse_visual) // NOLINT { - tesseract_common::TesseractSupportResourceLocator resource_locator; + tesseract_common::GeneralResourceLocator resource_locator; { std::unordered_map empty_available_materials; std::string str = R"(