#include #include #include #include #include #include #include #include #include #include using namespace moveit::task_constructor; TEST(PickPlaceDemo, run) { ros::NodeHandle nh, pnh("~"); moveit_task_constructor_demo::setupDemoScene(pnh); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); ASSERT_TRUE(pick_place_task.init()); ASSERT_TRUE(pick_place_task.plan()); ASSERT_TRUE(pick_place_task.execute()); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "pick_place_test"); ros::AsyncSpinner spinner(1); spinner.start(); return RUN_ALL_TESTS(); }