Fix add/remove object in backward operation

- addObject() will actually remove the object from scene
- removeObject() is not supported (we would need to know which object to add)

Author: rhaschke

core/include/moveit/task_constructor/stages/modify_planning_scene.h

@@ -149,8 +149,8 @@ class ModifyPlanningScene : public PropagatingEitherWay
 
 protected:
 	// apply stored modifications to scene
-	InterfaceState apply(const InterfaceState& from, bool invert);
-	void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
+	std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
+	void processCollisionObject(planning_scene::PlanningScene& scene, moveit_msgs::CollisionObject& object, bool invert);
 	void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
 	                   bool invert);
 	void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

core/python/test/rostest_mps.py

@@ -25,42 +25,87 @@ class TestModifyPlanningScene(unittest.TestCase):
     def setUp(self):
         super(TestModifyPlanningScene, self).setUp()
         self.psi = PlanningSceneInterface(synchronous=True)
+        self.make_box = self.psi._PlanningSceneInterface__make_box
         # insert a box to collide with
         self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
+        self.task = task = core.Task()
+        task.add(stages.CurrentState("current"))
 
+    def insertMove(self, position=-1):
         # colliding motion
         move = stages.MoveRelative("move", core.JointInterpolationPlanner())
         move.group = self.PLANNING_GROUP
         move.setDirection({"joint_1": 0.3})
-
-        self.task = task = core.Task()
-        task.add(stages.CurrentState("current"), move)
+        self.task.insert(move, position)
 
     def test_collision(self):
+        self.insertMove()
         self.assertFalse(self.task.plan())
 
     def test_allow_collision_list(self):
-        mps = stages.ModifyPlanningScene("mps")
+        mps = stages.ModifyPlanningScene("allow specific collisions for box")
         mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
-        self.task.insert(mps, 1)
+        self.task.add(mps)
+        self.insertMove()
         self.assertTrue(self.task.plan())
 
     def test_allow_collision_all(self):
         # insert an extra collision object that is unknown to ACM
         self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
         # attach box to end effector
         self.psi.attach_box("link_6", "box")
-        mps = stages.ModifyPlanningScene("mps")
+        self.insertMove()
         self.assertFalse(self.task.plan())
 
         # allow all collisions for attached "box" object
+        mps = stages.ModifyPlanningScene("allow all collisions for box")
         mps.allowCollisions("box", True)
         self.task.insert(mps, 1)
         self.assertTrue(self.task.plan())
         # restore original state
         self.psi.remove_attached_object("link_6", "box")
         self.psi.remove_world_object("block")
 
+    def test_fw_add_object(self):
+        mps = stages.ModifyPlanningScene("addObject(block)")
+        mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
+        self.task.add(mps)
+        self.insertMove()
+        self.assertFalse(self.task.plan())
+
+    def test_fw_remove_object(self):
+        mps = stages.ModifyPlanningScene("removeObject(box)")
+        mps.removeObject("box")
+        self.task.insert(mps, 1)
+        self.assertTrue(self.task.plan())
+        s = self.task.solutions[0].toMsg()
+        self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
+
+    def test_bw_add_object(self):
+        # add object to move_group's planning scene
+        self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
+
+        # backward operation will actually remove the object
+        mps = stages.ModifyPlanningScene("addObject(block) backwards")
+        mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
+        self.task.insert(mps, 0)
+        self.insertMove(0)
+        self.assertTrue(self.task.plan())
+
+        self.psi.remove_world_object("block")  # restore original state
+
+        s = self.task.solutions[0].toMsg()
+        for ps in [s.start_scene] + [s.sub_trajectory[i].scene_diff for i in [0, 1]]:
+            objects = [o.id for o in ps.world.collision_objects]
+            self.assertTrue(objects == ["block", "box"])
+
+    def test_bw_remove_object(self):
+        mps = stages.ModifyPlanningScene("removeObject(box) backwards")
+        mps.removeObject("box")
+        self.task.insert(mps, 0)
+        self.insertMove(0)
+        self.assertFalse(self.task.plan())
+
 
 if __name__ == "__main__":
     roscpp_initialize("test_mtc")

core/src/stages/modify_planning_scene.cpp

@@ -84,11 +84,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit
 }
 
 void ModifyPlanningScene::computeForward(const InterfaceState& from) {
-	sendForward(from, apply(from, false), SubTrajectory());
+	auto result = apply(from, false);
+	sendForward(from, std::move(result.first), std::move(result.second));
 }
 
 void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
-	sendBackward(apply(to, true), to, SubTrajectory());
+	auto result = apply(to, true);
+	sendBackward(std::move(result.first), to, std::move(result.second));
 }
 
 void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
@@ -121,30 +123,43 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene,
 
 // invert indicates, whether to detach instead of attach (and vice versa)
 // as well as to forbid instead of allow collision (and vice versa)
-InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
+std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
 	planning_scene::PlanningScenePtr scene = from.scene()->diff();
-	InterfaceState result(scene);
-	// add/remove objects
-	for (const auto& collision_object : collision_objects_)
-		processCollisionObject(*scene, collision_object);
-
-	// attach/detach objects
-	for (const auto& pair : attach_objects_)
-		attachObjects(*scene, pair, invert);
-
-	// allow/forbid collisions
-	for (const auto& pairs : collision_matrix_edits_)
-		allowCollisions(*scene, pairs, invert);
-
-	if (callback_)
-		callback_(scene, properties());
-
-	return result;
+	InterfaceState state(scene);
+	SubTrajectory traj;
+	try {
+		// add/remove objects
+		for (auto& collision_object : collision_objects_)
+			processCollisionObject(*scene, collision_object, invert);
+
+		// attach/detach objects
+		for (const auto& pair : attach_objects_)
+			attachObjects(*scene, pair, invert);
+
+		// allow/forbid collisions
+		for (const auto& pairs : collision_matrix_edits_)
+			allowCollisions(*scene, pairs, invert);
+
+		if (callback_)
+			callback_(scene, properties());
+	} catch (const std::exception& e) {
+		traj.markAsFailure(e.what());
+	}
+	return std::make_pair(state, traj);
 }
 
 void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
-                                                 const moveit_msgs::CollisionObject& object) {
+                                                 moveit_msgs::CollisionObject& object, bool invert) {
+	auto op = object.operation;
+	if (invert) {
+		if (op == moveit_msgs::CollisionObject::ADD)
+			op = moveit_msgs::CollisionObject::REMOVE;
+		else if (op == moveit_msgs::CollisionObject::REMOVE)
+			throw std::runtime_error("cannot apply removeObject() backwards");
+	}
+
 	scene.processCollisionObjectMsg(object);
+	object.operation = op;
 }
 }  // namespace stages
 }  // namespace task_constructor