MPS: fixup processCollisionObject

- Declare CollisionObject argument as constant:
  Internally the argument is temporarily modified, but for a caller it is effectively const.
- Correctly restore the old operation mode
- Fixup check in unit test

Author: rhaschke

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

@@ -150,7 +150,8 @@ class ModifyPlanningScene : public PropagatingEitherWay
 protected:
 	// apply stored modifications to scene
 	std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
-	void processCollisionObject(planning_scene::PlanningScene& scene, moveit_msgs::CollisionObject& object, bool invert);
+	void processCollisionObject(planning_scene::PlanningScene& scene, const 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

@@ -95,9 +95,14 @@ def DISABLED_test_bw_add_object(self):
         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"])
+
+        # block shouldn't be in start scene
+        objects = [o.id for o in s.start_scene.world.collision_objects]
+        self.assertTrue(objects == ["box"])
+
+        # only addObject(block) should add it
+        objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
+        self.assertTrue(objects == ["block", "box"])
 
     def DISABLED_test_bw_remove_object(self):
         mps = stages.ModifyPlanningScene("removeObject(box) backwards")

core/src/stages/modify_planning_scene.cpp

@@ -149,17 +149,19 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
 }
 
 void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
-                                                 moveit_msgs::CollisionObject& object, bool invert) {
-	auto op = object.operation;
+                                                 const moveit_msgs::CollisionObject& object, bool invert) {
+	const auto op = object.operation;
 	if (invert) {
 		if (op == moveit_msgs::CollisionObject::ADD)
-			op = moveit_msgs::CollisionObject::REMOVE;
+			// (temporarily) change operation to REMOVE to revert adding the object
+			const_cast<moveit_msgs::CollisionObject&>(object).operation = 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;
+	// restore previous operation (for next call)
+	const_cast<moveit_msgs::CollisionObject&>(object).operation = op;
 }
 }  // namespace stages
 }  // namespace task_constructor