planning_environment: planning_environment: test_allowed_collision

00001 #! /usr/bin/env python
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         #first we get the default
00081         
00082         get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
00083     
00084         get_res = get_allowed_collision_service_proxy(get_mat)
00085 
00086         #entry for left gripper and right gripper should be false - just an example
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         #if enable just this link, it should be true
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         #now we enable r_end_effector versus l_end_effector, and it should be false again
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         #now disable between all objects
00241 
00242         #if enable just this link, it should be true
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         #this should overwrite touch links
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)
test_allowed_collision_operations.py
