cast demos as integration unit tests

- moved demos from demo to test folder
- run them as unittest, checking range of solutions
This commit is contained in:
Robert Haschke 2018-06-03 12:34:57 +02:00
parent 3215880b98
commit 932ab5eba3
9 changed files with 136 additions and 60 deletions

View File

@ -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})

View File

@ -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)

View File

@ -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()

View File

@ -13,8 +13,8 @@
#include <moveit/task_constructor/stages/fix_collision_objects.h>
#include <ros/ros.h>
#include <moveit/planning_scene/planning_scene.h>
#include <gtest/gtest.h>
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 <enter>\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();
}

16
core/test/pick_pa10.test Normal file
View File

@ -0,0 +1,16 @@
<launch>
<include file="$(find pa10_shadow_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<include file="$(find pa10_shadow_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
</include>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<test pkg="moveit_task_constructor_core" type="pick_pa10" test-name="pick_pa10"/>
</launch>

View File

@ -9,6 +9,7 @@
#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <gtest/gtest.h>
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<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
Stage* initial_stage = new stages::CurrentState("current state");
t.add(std::unique_ptr<Stage>(initial_stage));
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
@ -82,16 +75,25 @@ int main(int argc, char** argv){
t.add(std::move(pick));
try {
spawnObject();
t.plan();
std::cout << "waiting for <enter>\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();
}

17
core/test/pick_pr2.test Normal file
View File

@ -0,0 +1,17 @@
<launch>
<include file="$(find pr2_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<include file="$(find pr2_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
</include>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<node pkg="tf" type="static_transform_publisher" name="world_joint" args="0 0 0 0 0 0 /odom_combined /base_footprint 100"/>
<test pkg="moveit_task_constructor_core" type="pick_pr2" test-name="pick_pr2"/>
</launch>

View File

@ -9,6 +9,7 @@
#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <gtest/gtest.h>
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 <enter>\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();
}

17
core/test/pick_ur5.test Normal file
View File

@ -0,0 +1,17 @@
<launch>
<include file="$(find tams_ur5_setup_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<include file="$(find tams_ur5_setup_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
</include>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<node pkg="tf" type="static_transform_publisher" name="world_joint" args="0 0 0 0 0 0 /odom_combined /base_footprint 100"/>
<test pkg="moveit_task_constructor_core" type="pick_ur5" test-name="pick_ur5" time-limit="120"/>
</launch>