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})
|
||||
|
||||
add_subdirectory(src)
|
||||
add_subdirectory(demo)
|
||||
add_subdirectory(test)
|
||||
|
||||
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)
|
||||
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()
|
||||
|
||||
@ -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
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 <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
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 <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
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