|
| 1 | +#! /usr/bin/env python |
| 2 | + |
| 3 | +from __future__ import print_function |
| 4 | +import unittest |
| 5 | +import rostest |
| 6 | +from moveit_commander.roscpp_initializer import roscpp_initialize |
| 7 | +from moveit_commander import PlanningSceneInterface |
| 8 | +from moveit.task_constructor import core, stages |
| 9 | +from geometry_msgs.msg import PoseStamped |
| 10 | + |
| 11 | + |
| 12 | +def make_pose(x, y, z): |
| 13 | + pose = PoseStamped() |
| 14 | + pose.header.frame_id = "base_link" |
| 15 | + pose.pose.position.x = x |
| 16 | + pose.pose.position.y = y |
| 17 | + pose.pose.position.z = z |
| 18 | + pose.pose.orientation.w = 1.0 |
| 19 | + return pose |
| 20 | + |
| 21 | + |
| 22 | +class TestModifyPlanningScene(unittest.TestCase): |
| 23 | + PLANNING_GROUP = "manipulator" |
| 24 | + |
| 25 | + def setUp(self): |
| 26 | + super(TestModifyPlanningScene, self).setUp() |
| 27 | + self.psi = PlanningSceneInterface(synchronous=True) |
| 28 | + self.make_box = self.psi._PlanningSceneInterface__make_box |
| 29 | + # insert a box to collide with |
| 30 | + self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2]) |
| 31 | + self.task = task = core.Task() |
| 32 | + task.add(stages.CurrentState("current")) |
| 33 | + |
| 34 | + def insertMove(self, position=-1): |
| 35 | + # colliding motion |
| 36 | + move = stages.MoveRelative("move", core.JointInterpolationPlanner()) |
| 37 | + move.group = self.PLANNING_GROUP |
| 38 | + move.setDirection({"joint_1": 0.3}) |
| 39 | + self.task.insert(move, position) |
| 40 | + |
| 41 | + def test_collision(self): |
| 42 | + self.insertMove() |
| 43 | + self.assertFalse(self.task.plan()) |
| 44 | + |
| 45 | + def test_allow_collision_list(self): |
| 46 | + mps = stages.ModifyPlanningScene("allow specific collisions for box") |
| 47 | + mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True) |
| 48 | + self.task.add(mps) |
| 49 | + self.insertMove() |
| 50 | + self.assertTrue(self.task.plan()) |
| 51 | + |
| 52 | + def test_allow_collision_all(self): |
| 53 | + # insert an extra collision object that is unknown to ACM |
| 54 | + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) |
| 55 | + # attach box to end effector |
| 56 | + self.psi.attach_box("link_6", "box") |
| 57 | + self.insertMove() |
| 58 | + self.assertFalse(self.task.plan()) |
| 59 | + |
| 60 | + # allow all collisions for attached "box" object |
| 61 | + mps = stages.ModifyPlanningScene("allow all collisions for box") |
| 62 | + mps.allowCollisions("box", True) |
| 63 | + self.task.insert(mps, 1) |
| 64 | + self.assertTrue(self.task.plan()) |
| 65 | + # restore original state |
| 66 | + self.psi.remove_attached_object("link_6", "box") |
| 67 | + self.psi.remove_world_object("block") |
| 68 | + |
| 69 | + def test_fw_add_object(self): |
| 70 | + mps = stages.ModifyPlanningScene("addObject(block)") |
| 71 | + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) |
| 72 | + self.task.add(mps) |
| 73 | + self.insertMove() |
| 74 | + self.assertFalse(self.task.plan()) |
| 75 | + |
| 76 | + def test_fw_remove_object(self): |
| 77 | + mps = stages.ModifyPlanningScene("removeObject(box)") |
| 78 | + mps.removeObject("box") |
| 79 | + self.task.insert(mps, 1) |
| 80 | + self.assertTrue(self.task.plan()) |
| 81 | + s = self.task.solutions[0].toMsg() |
| 82 | + self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box") |
| 83 | + |
| 84 | + def DISABLED_test_bw_add_object(self): |
| 85 | + # add object to move_group's planning scene |
| 86 | + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) |
| 87 | + |
| 88 | + # backward operation will actually remove the object |
| 89 | + mps = stages.ModifyPlanningScene("addObject(block) backwards") |
| 90 | + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) |
| 91 | + self.task.insert(mps, 0) |
| 92 | + self.insertMove(0) |
| 93 | + self.assertTrue(self.task.plan()) |
| 94 | + |
| 95 | + self.psi.remove_world_object("block") # restore original state |
| 96 | + |
| 97 | + s = self.task.solutions[0].toMsg() |
| 98 | + for ps in [s.start_scene] + [s.sub_trajectory[i].scene_diff for i in [0, 1]]: |
| 99 | + objects = [o.id for o in ps.world.collision_objects] |
| 100 | + self.assertTrue(objects == ["block", "box"]) |
| 101 | + |
| 102 | + def DISABLED_test_bw_remove_object(self): |
| 103 | + mps = stages.ModifyPlanningScene("removeObject(box) backwards") |
| 104 | + mps.removeObject("box") |
| 105 | + self.task.insert(mps, 0) |
| 106 | + self.insertMove(0) |
| 107 | + self.assertFalse(self.task.plan()) |
| 108 | + |
| 109 | + |
| 110 | +if __name__ == "__main__": |
| 111 | + roscpp_initialize("test_mtc") |
| 112 | + rostest.rosrun("mtc", "mps", TestModifyPlanningScene) |
0 commit comments