Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('planning_environment')
00004 import rospy
00005 import arm_navigation_msgs.srv
00006 import sys
00007 import std_srvs.srv
00008 import unittest
00009 import mapping_msgs.msg
00010 from mapping_msgs.msg import CollisionObject
00011 from mapping_msgs.msg import AttachedCollisionObject
00012 from arm_navigation_msgs.msg import CollisionOperation
00013 from arm_navigation_msgs.msg import Shape
00014 from geometry_msgs.msg import Pose
00015 from std_msgs.msg import String
00016
00017 set_allowed_collision_service_name = "set_allowed_collisions"
00018 revert_allowed_collision_service_name = "revert_allowed_collisions"
00019 get_allowed_collision_service_name = "get_current_allowed_collision_matrix"
00020 default_prefix = "/environment_server"
00021
00022 monk = False
00023
00024 class TestAllowedCollisionOperations(unittest.TestCase):
00025
00026 def setUp(self):
00027
00028 rospy.init_node('test_allowed_collisions')
00029
00030 obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00031 att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject,latch=True)
00032
00033 full_name = default_prefix+'/'+revert_allowed_collision_service_name
00034 rospy.wait_for_service(full_name)
00035 revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
00036
00037 clear_all_att = AttachedCollisionObject()
00038 clear_all_att.object.header.stamp = rospy.Time.now()
00039 clear_all_att.object.header.frame_id = "base_link"
00040 clear_all_att.link_name = "all"
00041 clear_all_att.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00042
00043 clear_all_obj = CollisionObject()
00044 clear_all_obj.header.stamp = rospy.Time.now()
00045 clear_all_obj.header.frame_id = "base_link"
00046 clear_all_obj.id = "all"
00047 clear_all_obj.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00048
00049 empty_req = std_srvs.srv.EmptyRequest()
00050
00051 revert_allowed_collision_service_proxy(empty_req)
00052
00053 def tearDown(self):
00054
00055 full_name = default_prefix+'/'+revert_allowed_collision_service_name
00056 rospy.wait_for_service(full_name)
00057 revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
00058
00059 empty_req = std_srvs.srv.EmptyRequest()
00060
00061 revert_allowed_collision_service_proxy(empty_req)
00062
00063 def test_self_collisions(self):
00064
00065 global set_allowed_collision_service_name
00066 global get_allowed_collision_service_name
00067 global revert_allowed_collision_service_name
00068 full_name = default_prefix+'/'+set_allowed_collision_service_name
00069 rospy.wait_for_service(full_name)
00070 set_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
00071
00072 full_name = default_prefix+'/'+get_allowed_collision_service_name
00073 rospy.wait_for_service(full_name)
00074 get_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
00075
00076 full_name = default_prefix+'/'+revert_allowed_collision_service_name
00077 rospy.wait_for_service(full_name)
00078 revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
00079
00080
00081
00082 get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
00083
00084 get_res = get_allowed_collision_service_proxy(get_mat)
00085
00086
00087 try:
00088 r_gripper_palm_link_index = get_res.matrix.link_names.index("r_gripper_palm_link")
00089 except ValueError:
00090 self.fail("No r_gripper_palm_link in AllowedCollisionMatrix link names")
00091
00092 try:
00093 l_gripper_palm_link_index = get_res.matrix.link_names.index("l_gripper_palm_link")
00094 except ValueError:
00095 self.fail("No l_gripper_palm_link in AllowedCollisionMatrix link names")
00096
00097 self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
00098 self.failIf(len(get_res.matrix.entries) <= l_gripper_palm_link_index)
00099
00100 self.failIf(len(get_res.matrix.entries[l_gripper_palm_link_index].enabled) <= r_gripper_palm_link_index)
00101 self.failIf(len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= l_gripper_palm_link_index)
00102
00103 self.assertEqual(False, get_res.matrix.entries[l_gripper_palm_link_index].enabled[r_gripper_palm_link_index])
00104 self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[l_gripper_palm_link_index])
00105
00106
00107 set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest()
00108 set_allowed_request.ord.collision_operations = [CollisionOperation() for _ in range(1)]
00109 set_allowed_request.ord.collision_operations[0].object1 = "r_gripper_palm_link"
00110 set_allowed_request.ord.collision_operations[0].object2 = "l_gripper_palm_link"
00111 set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.DISABLE
00112
00113 set_allowed_collision_service_proxy(set_allowed_request)
00114
00115 rospy.sleep(1.0)
00116
00117 get_res = get_allowed_collision_service_proxy(get_mat)
00118
00119 self.assertEqual(True, get_res.matrix.entries[l_gripper_palm_link_index].enabled[r_gripper_palm_link_index])
00120 self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[l_gripper_palm_link_index])
00121
00122
00123 set_allowed_request.ord.collision_operations[0].object1 = "r_end_effector"
00124 set_allowed_request.ord.collision_operations[0].object2 = "l_end_effector"
00125 set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.ENABLE
00126
00127 set_allowed_collision_service_proxy(set_allowed_request)
00128
00129 rospy.sleep(1.0)
00130
00131 get_res = get_allowed_collision_service_proxy(get_mat)
00132
00133 self.assertEqual(False, get_res.matrix.entries[l_gripper_palm_link_index].enabled[r_gripper_palm_link_index])
00134 self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[l_gripper_palm_link_index])
00135
00136 def test_object_collisions(self):
00137 global set_allowed_collision_service_name
00138 global get_allowed_collision_service_name
00139 global revert_allowed_collision_service_name
00140 full_name = default_prefix+'/'+set_allowed_collision_service_name
00141 rospy.wait_for_service(full_name)
00142 set_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
00143
00144 full_name = default_prefix+'/'+get_allowed_collision_service_name
00145 rospy.wait_for_service(full_name)
00146 get_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
00147
00148 full_name = default_prefix+'/'+revert_allowed_collision_service_name
00149 rospy.wait_for_service(full_name)
00150 revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
00151
00152 obj_pub = rospy.Publisher('collision_object',CollisionObject)
00153 att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
00154
00155 rospy.init_node('test_allowed_collisions')
00156
00157 obj1 = CollisionObject()
00158
00159 obj1.header.stamp = rospy.Time.now()
00160 obj1.header.frame_id = "base_link"
00161 obj1.id = "obj1";
00162 obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00163 obj1.shapes = [Shape() for _ in range(2)]
00164 obj1.shapes[0].type = Shape.BOX
00165 obj1.shapes[0].dimensions = [float() for _ in range(3)]
00166 obj1.shapes[0].dimensions[0] = 1.0
00167 obj1.shapes[0].dimensions[1] = 1.0
00168 obj1.shapes[0].dimensions[2] = .05
00169 obj1.shapes[1].type = Shape.BOX
00170 obj1.shapes[1].dimensions = [float() for _ in range(3)]
00171 obj1.shapes[1].dimensions[0] = 1.0
00172 obj1.shapes[1].dimensions[1] = 1.0
00173 obj1.shapes[1].dimensions[2] = .05
00174 obj1.poses = [Pose() for _ in range(2)]
00175 obj1.poses[0].position.x = 1.0
00176 obj1.poses[0].position.y = 0
00177 obj1.poses[0].position.z = .5
00178 obj1.poses[0].orientation.x = 0
00179 obj1.poses[0].orientation.y = 0
00180 obj1.poses[0].orientation.z = 0
00181 obj1.poses[0].orientation.w = 1
00182 obj1.poses[1].position.x = 1.0
00183 obj1.poses[1].position.y = 0
00184 obj1.poses[1].position.z = .75
00185 obj1.poses[1].orientation.x = 0
00186 obj1.poses[1].orientation.y = 0
00187 obj1.poses[1].orientation.z = 0
00188 obj1.poses[1].orientation.w = 1
00189
00190 obj_pub.publish(obj1)
00191
00192 obj2 = CollisionObject();
00193
00194 obj2.header.stamp = rospy.Time.now()
00195 obj2.header.frame_id = "base_link"
00196 obj2.id = "obj2";
00197 obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00198 obj2.shapes = [Shape() for _ in range(1)]
00199 obj2.shapes[0].type = Shape.BOX
00200 obj2.shapes[0].dimensions = [float() for _ in range(3)]
00201 obj2.shapes[0].dimensions[0] = 1.0
00202 obj2.shapes[0].dimensions[1] = 1.0
00203 obj2.shapes[0].dimensions[2] = .05
00204 obj2.poses = [Pose() for _ in range(1)]
00205 obj2.poses[0].position.x = .15
00206 obj2.poses[0].position.y = 0
00207 obj2.poses[0].position.z = .5
00208 obj2.poses[0].orientation.x = 0
00209 obj2.poses[0].orientation.y = 0
00210 obj2.poses[0].orientation.z = 0
00211 obj2.poses[0].orientation.w = 1
00212
00213 obj_pub.publish(obj2)
00214
00215 rospy.sleep(2.0)
00216
00217 get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
00218
00219 get_res = get_allowed_collision_service_proxy(get_mat)
00220
00221 try:
00222 r_gripper_palm_link_index = get_res.matrix.link_names.index("r_gripper_palm_link")
00223 except ValueError:
00224 self.fail("No r_gripper_palm_link in AllowedCollisionMatrix link names")
00225
00226 try:
00227 obj1_index = get_res.matrix.link_names.index("obj1")
00228 except ValueError:
00229 self.fail("No obj1 in AllowedCollisionMatrix link names")
00230
00231 self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
00232 self.failIf(len(get_res.matrix.entries) <= obj1_index)
00233
00234 self.failIf(len(get_res.matrix.entries[obj1_index].enabled) <= r_gripper_palm_link_index)
00235 self.failIf(len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= obj1_index)
00236
00237 self.assertEqual(False, get_res.matrix.entries[obj1_index].enabled[r_gripper_palm_link_index])
00238 self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
00239
00240
00241
00242
00243 set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest()
00244 set_allowed_request.ord.collision_operations = [CollisionOperation() for _ in range(1)]
00245 set_allowed_request.ord.collision_operations[0].object1 = "r_gripper_palm_link"
00246 set_allowed_request.ord.collision_operations[0].object2 = CollisionOperation.COLLISION_SET_OBJECTS
00247 set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.DISABLE
00248
00249 set_allowed_collision_service_proxy(set_allowed_request)
00250
00251 rospy.sleep(1.)
00252
00253 get_res = get_allowed_collision_service_proxy(get_mat)
00254
00255 self.assertEqual(True, get_res.matrix.entries[obj1_index].enabled[r_gripper_palm_link_index])
00256 self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
00257
00258 def test_attached_object_collision_operations(self):
00259
00260 global set_allowed_collision_service_name
00261 global get_allowed_collision_service_name
00262 global revert_allowed_collision_service_name
00263 full_name = default_prefix+'/'+set_allowed_collision_service_name
00264 rospy.wait_for_service(full_name)
00265 set_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
00266
00267 full_name = default_prefix+'/'+get_allowed_collision_service_name
00268 rospy.wait_for_service(full_name)
00269 get_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
00270
00271 full_name = default_prefix+'/'+revert_allowed_collision_service_name
00272 rospy.wait_for_service(full_name)
00273 revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
00274
00275 att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
00276
00277 rospy.init_node('test_allowed_collisions')
00278
00279 att_object = AttachedCollisionObject();
00280 att_object.object.header.stamp = rospy.Time.now()
00281 att_object.object.header.frame_id = "base_link"
00282 att_object.link_name = "r_gripper_r_finger_tip_link"
00283 att_object.touch_links = [str() for _ in range(1)]
00284 att_object.touch_links[0] = "r_gripper_palm_link"
00285 att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00286 att_object.object = CollisionObject();
00287
00288 att_object.object.header.stamp = rospy.Time.now()
00289 att_object.object.header.frame_id = "base_link"
00290 att_object.object.id = "obj3"
00291 att_object.object.shapes = [Shape() for _ in range(1)]
00292 att_object.object.shapes[0].type = Shape.BOX
00293 att_object.object.shapes[0].dimensions = [float() for _ in range(3)]
00294 att_object.object.shapes[0].dimensions[0] = .2
00295 att_object.object.shapes[0].dimensions[1] = 0.3
00296 att_object.object.shapes[0].dimensions[2] = .05
00297 att_object.object.poses = [Pose() for _ in range(1)]
00298 att_object.object.poses[0].position.x = .15
00299 att_object.object.poses[0].position.y = 0
00300 att_object.object.poses[0].position.z = 1.0
00301 att_object.object.poses[0].orientation.x = 0
00302 att_object.object.poses[0].orientation.y = 0
00303 att_object.object.poses[0].orientation.z = 0
00304 att_object.object.poses[0].orientation.w = 1
00305 att_pub.publish(att_object)
00306
00307 rospy.sleep(5.0)
00308
00309 get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
00310
00311 get_res = get_allowed_collision_service_proxy(get_mat)
00312
00313 try:
00314 r_gripper_palm_link_index = get_res.matrix.link_names.index("r_gripper_palm_link")
00315 except ValueError:
00316 self.fail("No r_gripper_palm_link in AllowedCollisionMatrix link names")
00317
00318 try:
00319 r_gripper_r_finger_tip_link_index = get_res.matrix.link_names.index("r_gripper_r_finger_tip_link")
00320 except ValueError:
00321 self.fail("No r_gripper_r_finger_tip_link in AllowedCollisionMatrix link names")
00322
00323 try:
00324 obj3_index = get_res.matrix.link_names.index("obj3")
00325 except ValueError:
00326 self.fail("No obj3 in AllowedCollisionMatrix link names")
00327
00328 self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_r_finger_tip_link_index])
00329 self.assertEqual(True, get_res.matrix.entries[r_gripper_r_finger_tip_link_index].enabled[obj3_index])
00330
00331 self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
00332 self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
00333
00334
00335 set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest()
00336 set_allowed_request.ord.collision_operations = [CollisionOperation() for _ in range(1)]
00337 set_allowed_request.ord.collision_operations[0].object1 = "obj3"
00338 set_allowed_request.ord.collision_operations[0].object2 = "r_gripper_palm_link"
00339 set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.ENABLE
00340
00341 set_allowed_collision_service_proxy(set_allowed_request)
00342
00343 rospy.sleep(1.)
00344
00345 get_res = get_allowed_collision_service_proxy(get_mat)
00346
00347 self.assertEqual(False, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
00348 self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
00349
00350 set_allowed_request.ord.collision_operations[0].object1 = "r_gripper_palm_link"
00351 set_allowed_request.ord.collision_operations[0].object2 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
00352 set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.DISABLE
00353
00354 set_allowed_collision_service_proxy(set_allowed_request)
00355
00356 rospy.sleep(1.)
00357
00358 get_res = get_allowed_collision_service_proxy(get_mat)
00359
00360 self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
00361 self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
00362
00363 if __name__ == '__main__':
00364 import rostest
00365 rostest.unitrun('test_allowed_collisions', 'test_allowed_collisions', TestAllowedCollisionOperations)