diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 4ccd5278..0b530ed1 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -33,6 +33,9 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) + add_rostest_gtest(${PROJECT_NAME}-test-move-to move_to.test test_move_to.cpp) + target_link_libraries(${PROJECT_NAME}-test-move-to ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils 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) diff --git a/core/test/move_to.test b/core/test/move_to.test new file mode 100644 index 00000000..105e936a --- /dev/null +++ b/core/test/move_to.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp new file mode 100644 index 00000000..8eacfdb0 --- /dev/null +++ b/core/test/test_move_to.cpp @@ -0,0 +1,106 @@ +#include "models.h" + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveTo : public testing::Test +{ + Task t; + stages::MoveTo* move_to; + PlanningScenePtr scene; + + PandaMoveTo() { + t.setRobotModel(loadModel()); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move = std::make_unique("move", std::make_shared()); + move_to = move.get(); + move_to->setGroup("panda_arm"); + t.add(std::move(move)); + } +}; + +#define EXPECT_ONE_SOLUTION \ + { \ + EXPECT_TRUE(t.plan()); \ + EXPECT_EQ(t.solutions().size(), 1u); \ + } + +TEST_F(PandaMoveTo, namedTarget) { + move_to->setGoal("extended"); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, mapTarget) { + move_to->setGoal(std::map{ { "panda_joint1", TAU / 8 }, { "panda_joint2", TAU / 8 } }); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, stateTarget) { + move_to->setGoal([]() { + moveit_msgs::RobotState state; + state.is_diff = true; + state.joint_state.name = { "panda_joint1", "panda_joint2" }; + state.joint_state.position = { TAU / 8, TAU / 8 }; + return state; + }()); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, DISABLED_pointTarget) { + move_to->setGoal([this]() { + RobotState state{ scene->getCurrentState() }; + state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); + auto frame{ state.getFrameTransform("panda_link8") }; + geometry_msgs::PointStamped point; + point.header.frame_id = scene->getPlanningFrame(); + point.point = tf2::toMsg(Eigen::Vector3d{ frame.translation() }); + return point; + }()); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, DISABLED_poseTarget) { + move_to->setGoal([this]() { + RobotState state{ scene->getCurrentState() }; + state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); + auto frame{ state.getFrameTransform("panda_link8") }; + geometry_msgs::PoseStamped pose; + pose.header.frame_id = scene->getPlanningFrame(); + pose.pose = tf2::toMsg(frame); + return pose; + }()); + EXPECT_ONE_SOLUTION; +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "move_to_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +}