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();
+}