From 932ab5eba3c238a8d0e9fab5c7b122b7876dd9ec Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 3 Jun 2018 12:34:57 +0200 Subject: [PATCH] cast demos as integration unit tests - moved demos from demo to test folder - run them as unittest, checking range of solutions --- core/CMakeLists.txt | 1 - core/demo/CMakeLists.txt | 8 ---- core/test/CMakeLists.txt | 25 +++++++++++ .../plan_pick_pa10.cpp => test/pick_pa10.cpp} | 34 ++++++++------- core/test/pick_pa10.test | 16 +++++++ .../plan_pick_pr2.cpp => test/pick_pr2.cpp} | 42 ++++++++++--------- core/test/pick_pr2.test | 17 ++++++++ .../plan_pick_ur5.cpp => test/pick_ur5.cpp} | 36 +++++++++------- core/test/pick_ur5.test | 17 ++++++++ 9 files changed, 136 insertions(+), 60 deletions(-) delete mode 100644 core/demo/CMakeLists.txt rename core/{demo/plan_pick_pa10.cpp => test/pick_pa10.cpp} (93%) create mode 100644 core/test/pick_pa10.test rename core/{demo/plan_pick_pr2.cpp => test/pick_pr2.cpp} (83%) create mode 100644 core/test/pick_pr2.test rename core/{demo/plan_pick_ur5.cpp => test/pick_ur5.cpp} (87%) create mode 100644 core/test/pick_ur5.test diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 944dc79f..89fdf8f3 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -37,7 +37,6 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) include_directories(${catkin_INCLUDE_DIRS}) add_subdirectory(src) -add_subdirectory(demo) add_subdirectory(test) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/core/demo/CMakeLists.txt b/core/demo/CMakeLists.txt deleted file mode 100644 index 80df13f2..00000000 --- a/core/demo/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -add_executable(plan_pick_ur5 plan_pick_ur5.cpp) -target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages) - -add_executable(plan_pick_pr2 plan_pick_pr2.cpp) -target_link_libraries(plan_pick_pr2 ${PROJECT_NAME}_stages) - -add_executable(plan_pick_pa10 plan_pick_pa10.cpp) -target_link_libraries(plan_pick_pa10 ${PROJECT_NAME}_stages) diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 9ff92a58..0680fb94 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -23,4 +23,29 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main) + + + # building these integration tests works without moveit config packages + add_executable(pick_ur5 pick_ur5.cpp) + target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) + + add_executable(pick_pr2 pick_pr2.cpp) + target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages gtest) + + add_executable(pick_pa10 pick_pa10.cpp) + target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages gtest) + + # running these integrations test naturally requires the moveit configs + find_package(tams_ur5_setup_moveit_config QUIET) + if(tams_ur5_setup_moveit_config_FOUND) + add_rostest(pick_ur5.test) + endif() + find_package(pr2_moveit_config_FOUND QUIET) + if(pr2_moveit_config_FOUND) + add_rostest(pick_pr2.test) + endif() + find_package(pa10_shadow_moveit_config QUIET) + if(pa10_shadow_moveit_config_FOUND) + add_rostest(pick_pa10.test) + endif() endif() diff --git a/core/demo/plan_pick_pa10.cpp b/core/test/pick_pa10.cpp similarity index 93% rename from core/demo/plan_pick_pa10.cpp rename to core/test/pick_pa10.cpp index 33cd92a7..101f2571 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -13,8 +13,8 @@ #include #include - #include +#include using namespace moveit::task_constructor; @@ -35,11 +35,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) { scene->processCollisionObjectMsg(o); } -int main(int argc, char** argv){ - ros::init(argc, argv, "plan_pick"); - ros::AsyncSpinner spinner(1); - spinner.start(); - +TEST(PA10, pick) { Task t; t.loadRobotModel(); // define global properties used by most stages @@ -168,15 +164,23 @@ int main(int argc, char** argv){ try { t.plan(); - std::cout << "waiting for \n"; - char ch; - std::cin >> ch; - } - catch (const InitStageException &e) { - std::cerr << e; - t.printState(); - return EINVAL; + } catch (const InitStageException &e) { + ADD_FAILURE() << "planning failed with exception" << std::endl << e << t; } - return 0; + auto solutions = t.solutions().size(); + EXPECT_GE(solutions, 5); + EXPECT_LE(solutions, 10); +} + +int main(int argc, char** argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "pr2"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // wait some time for move_group to come up + ros::WallDuration(5.0).sleep(); + + return RUN_ALL_TESTS(); } diff --git a/core/test/pick_pa10.test b/core/test/pick_pa10.test new file mode 100644 index 00000000..f06a4441 --- /dev/null +++ b/core/test/pick_pa10.test @@ -0,0 +1,16 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + diff --git a/core/demo/plan_pick_pr2.cpp b/core/test/pick_pr2.cpp similarity index 83% rename from core/demo/plan_pick_pr2.cpp rename to core/test/pick_pr2.cpp index 4336980c..9c1e3320 100644 --- a/core/demo/plan_pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -9,6 +9,7 @@ #include #include +#include using namespace moveit::task_constructor; @@ -31,19 +32,11 @@ void spawnObject(){ psi.applyCollisionObject(o); } -int main(int argc, char** argv){ - ros::init(argc, argv, "plan_pick"); - ros::AsyncSpinner spinner(1); - spinner.start(); - - spawnObject(); - +TEST(PR2, pick) { Task t; - Stage* initial_stage = nullptr; - auto initial = std::make_unique("current state"); - initial_stage = initial.get(); - t.add(std::move(initial)); + Stage* initial_stage = new stages::CurrentState("current state"); + t.add(std::unique_ptr(initial_stage)); // planner used for connect auto pipeline = std::make_shared(); @@ -82,16 +75,25 @@ int main(int argc, char** argv){ t.add(std::move(pick)); try { + spawnObject(); t.plan(); - std::cout << "waiting for \n"; - char ch; - std::cin >> ch; - } - catch (const InitStageException &e) { - std::cerr << e; - t.printState(); - return EINVAL; + } catch (const InitStageException &e) { + ADD_FAILURE() << "planning failed with exception" << std::endl << e << t; } - return 0; + auto solutions = t.solutions().size(); + EXPECT_GE(solutions, 5); + EXPECT_LE(solutions, 10); +} + +int main(int argc, char** argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "pr2"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // wait some time for move_group to come up + ros::WallDuration(5.0).sleep(); + + return RUN_ALL_TESTS(); } diff --git a/core/test/pick_pr2.test b/core/test/pick_pr2.test new file mode 100644 index 00000000..ed34b7b3 --- /dev/null +++ b/core/test/pick_pr2.test @@ -0,0 +1,17 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + diff --git a/core/demo/plan_pick_ur5.cpp b/core/test/pick_ur5.cpp similarity index 87% rename from core/demo/plan_pick_ur5.cpp rename to core/test/pick_ur5.cpp index 313c586d..fe474d84 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -9,6 +9,7 @@ #include #include +#include using namespace moveit::task_constructor; @@ -31,13 +32,7 @@ void spawnObject(){ psi.applyCollisionObject(o); } -int main(int argc, char** argv){ - ros::init(argc, argv, "plan_pick"); - ros::AsyncSpinner spinner(1); - spinner.start(); - - spawnObject(); - +TEST(UR5, pick) { Task t; Stage* initial_stage = nullptr; @@ -82,16 +77,25 @@ int main(int argc, char** argv){ t.add(std::move(pick)); try { + spawnObject(); t.plan(); - std::cout << "waiting for \n"; - char ch; - std::cin >> ch; - } - catch (const InitStageException &e) { - std::cerr << e; - t.printState(); - return EINVAL; + } catch (const InitStageException &e) { + ADD_FAILURE() << "planning failed with exception" << std::endl << e << t; } - return 0; + auto solutions = t.solutions().size(); + EXPECT_GE(solutions, 30); + EXPECT_LE(solutions, 60); +} + +int main(int argc, char** argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "pr2"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // wait some time for move_group to come up + ros::WallDuration(5.0).sleep(); + + return RUN_ALL_TESTS(); } diff --git a/core/test/pick_ur5.test b/core/test/pick_ur5.test new file mode 100644 index 00000000..64b3f276 --- /dev/null +++ b/core/test/pick_ur5.test @@ -0,0 +1,17 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + +