diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 80ec9d4a..c5570922 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -149,8 +149,8 @@ protected: 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 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>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 3d6b741a..1a967503 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -25,24 +25,28 @@ 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): @@ -50,10 +54,11 @@ class TestModifyPlanningScene(unittest.TestCase): 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()) @@ -61,6 +66,46 @@ class TestModifyPlanningScene(unittest.TestCase): 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") diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index d26fe029..319dba85 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/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 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); + 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); + // 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); + // allow/forbid collisions + for (const auto& pairs : collision_matrix_edits_) + allowCollisions(*scene, pairs, invert); - if (callback_) - callback_(scene, properties()); - - return result; + 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