add move MoveTo tests

(partially disabled because broken)

Author: v4hn

core/test/test_move_to.cpp

@@ -4,6 +4,7 @@
 #include <moveit/task_constructor/stages/move_to.h>
 #include <moveit/task_constructor/stages/fixed_state.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit/task_constructor/moveit_compat.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 
@@ -33,7 +34,8 @@ struct PandaMoveTo : public testing::Test
 
 		scene = std::make_shared<PlanningScene>(t.getRobotModel());
 		scene->getCurrentStateNonConst().setToDefaultValues();
-		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
+		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
+		                                                    "extended");
 		t.add(std::make_unique<stages::FixedState>("start", scene));
 
 		auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
@@ -50,7 +52,7 @@ struct PandaMoveTo : public testing::Test
 	}
 
 TEST_F(PandaMoveTo, namedTarget) {
-	move_to->setGoal("extended");
+	move_to->setGoal("ready");
 	EXPECT_ONE_SOLUTION;
 }
 
@@ -70,29 +72,87 @@ TEST_F(PandaMoveTo, stateTarget) {
 	EXPECT_ONE_SOLUTION;
 }
 
+geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) {
+	state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose);
+	auto frame_eigen{ state.getFrameTransform(frame) };
+	geometry_msgs::PoseStamped p;
+	p.header.frame_id = state.getRobotModel()->getModelFrame();
+	p.pose = tf2::toMsg(frame_eigen);
+	return p;
+}
+
 TEST_F(PandaMoveTo, 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;
-	}());
+	geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") };
+
+	geometry_msgs::PointStamped point;
+	point.header = pose.header;
+	point.point = pose.pose.position;
+	move_to->setGoal(point);
+
 	EXPECT_ONE_SOLUTION;
 }
 
 TEST_F(PandaMoveTo, 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;
+	move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8"));
+	EXPECT_ONE_SOLUTION;
+}
+
+TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
+	const std::string IK_FRAME{ "panda_hand" };
+	move_to->setIKFrame(IK_FRAME);
+	move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
+	EXPECT_ONE_SOLUTION;
+}
+
+moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) {
+	moveit_msgs::AttachedCollisionObject aco;
+	aco.link_name = "panda_hand";
+	aco.object.header.frame_id = aco.link_name;
+	aco.object.operation = aco.object.ADD;
+	aco.object.id = id;
+	aco.object.primitives.resize(1, []() {
+		shape_msgs::SolidPrimitive p;
+		p.type = p.SPHERE;
+		p.dimensions.resize(1);
+		p.dimensions[p.SPHERE_RADIUS] = 0.01;
+		return p;
+	}());
+#if MOVEIT_HAS_OBJECT_POSE
+	aco.object.pose.position.z = 0.2;
+	aco.object.pose.orientation.w = 1.0;
+#else
+	aco.object.primitive_poses.resize(1);
+	aco.object.primitive_poses[0].position.z = 0.2;
+	aco.object.primitive_poses[0].orientation.w = 1.0;
+#endif
+#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
+	aco.object.subframe_names.resize(1, "subframe");
+	aco.object.subframe_poses.resize(1, []() {
+		geometry_msgs::Pose p;
+		p.orientation.w = 1.0;
+		return p;
 	}());
+#endif
+	return aco;
+}
+
+TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
+	const std::string ATTACHED_OBJECT{ "attached_object" };
+	scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
+
+	move_to->setIKFrame(ATTACHED_OBJECT);
+	move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT));
+	EXPECT_ONE_SOLUTION;
+}
+
+TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) {
+	const std::string ATTACHED_OBJECT{ "attached_object" };
+	const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
+
+	scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
+
+	move_to->setIKFrame(IK_FRAME);
+	move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
 	EXPECT_ONE_SOLUTION;
 }