mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
3215880b98
commit
932ab5eba3
@ -37,7 +37,6 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
|
|||||||
include_directories(${catkin_INCLUDE_DIRS})
|
include_directories(${catkin_INCLUDE_DIRS})
|
||||||
|
|
||||||
add_subdirectory(src)
|
add_subdirectory(src)
|
||||||
add_subdirectory(demo)
|
|
||||||
add_subdirectory(test)
|
add_subdirectory(test)
|
||||||
|
|
||||||
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
|
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
|
||||||
|
|||||||
@ -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)
|
|
||||||
@ -23,4 +23,29 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main)
|
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()
|
endif()
|
||||||
|
|||||||
@ -13,8 +13,8 @@
|
|||||||
#include <moveit/task_constructor/stages/fix_collision_objects.h>
|
#include <moveit/task_constructor/stages/fix_collision_objects.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
@ -35,11 +35,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
|||||||
scene->processCollisionObjectMsg(o);
|
scene->processCollisionObjectMsg(o);
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
TEST(PA10, pick) {
|
||||||
ros::init(argc, argv, "plan_pick");
|
|
||||||
ros::AsyncSpinner spinner(1);
|
|
||||||
spinner.start();
|
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
t.loadRobotModel();
|
t.loadRobotModel();
|
||||||
// define global properties used by most stages
|
// define global properties used by most stages
|
||||||
@ -168,15 +164,23 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
try {
|
try {
|
||||||
t.plan();
|
t.plan();
|
||||||
std::cout << "waiting for <enter>\n";
|
} catch (const InitStageException &e) {
|
||||||
char ch;
|
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
|
||||||
std::cin >> ch;
|
|
||||||
}
|
|
||||||
catch (const InitStageException &e) {
|
|
||||||
std::cerr << e;
|
|
||||||
t.printState();
|
|
||||||
return EINVAL;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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
16
core/test/pick_pa10.test
Normal 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>
|
||||||
@ -9,6 +9,7 @@
|
|||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
@ -31,19 +32,11 @@ void spawnObject(){
|
|||||||
psi.applyCollisionObject(o);
|
psi.applyCollisionObject(o);
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
TEST(PR2, pick) {
|
||||||
ros::init(argc, argv, "plan_pick");
|
|
||||||
ros::AsyncSpinner spinner(1);
|
|
||||||
spinner.start();
|
|
||||||
|
|
||||||
spawnObject();
|
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
Stage* initial_stage = nullptr;
|
Stage* initial_stage = new stages::CurrentState("current state");
|
||||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
t.add(std::unique_ptr<Stage>(initial_stage));
|
||||||
initial_stage = initial.get();
|
|
||||||
t.add(std::move(initial));
|
|
||||||
|
|
||||||
// planner used for connect
|
// planner used for connect
|
||||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||||
@ -82,16 +75,25 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(pick));
|
t.add(std::move(pick));
|
||||||
|
|
||||||
try {
|
try {
|
||||||
|
spawnObject();
|
||||||
t.plan();
|
t.plan();
|
||||||
std::cout << "waiting for <enter>\n";
|
} catch (const InitStageException &e) {
|
||||||
char ch;
|
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
|
||||||
std::cin >> ch;
|
|
||||||
}
|
|
||||||
catch (const InitStageException &e) {
|
|
||||||
std::cerr << e;
|
|
||||||
t.printState();
|
|
||||||
return EINVAL;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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
17
core/test/pick_pr2.test
Normal 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>
|
||||||
@ -9,6 +9,7 @@
|
|||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
@ -31,13 +32,7 @@ void spawnObject(){
|
|||||||
psi.applyCollisionObject(o);
|
psi.applyCollisionObject(o);
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
TEST(UR5, pick) {
|
||||||
ros::init(argc, argv, "plan_pick");
|
|
||||||
ros::AsyncSpinner spinner(1);
|
|
||||||
spinner.start();
|
|
||||||
|
|
||||||
spawnObject();
|
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
Stage* initial_stage = nullptr;
|
Stage* initial_stage = nullptr;
|
||||||
@ -82,16 +77,25 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(pick));
|
t.add(std::move(pick));
|
||||||
|
|
||||||
try {
|
try {
|
||||||
|
spawnObject();
|
||||||
t.plan();
|
t.plan();
|
||||||
std::cout << "waiting for <enter>\n";
|
} catch (const InitStageException &e) {
|
||||||
char ch;
|
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
|
||||||
std::cin >> ch;
|
|
||||||
}
|
|
||||||
catch (const InitStageException &e) {
|
|
||||||
std::cerr << e;
|
|
||||||
t.printState();
|
|
||||||
return EINVAL;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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
17
core/test/pick_ur5.test
Normal 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>
|
||||||
Loading…
Reference in New Issue
Block a user