test_collision_objects.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 PKG = 'planning_environment'
00004 
00005 import roslib; roslib.load_manifest(PKG)
00006 import rospy
00007 import arm_navigation_msgs.srv
00008 import sys
00009 import unittest
00010 
00011 
00012 import sensor_msgs.msg
00013 import mapping_msgs.msg
00014 from mapping_msgs.msg import CollisionObject
00015 from mapping_msgs.msg import AttachedCollisionObject
00016 from arm_navigation_msgs.msg import CollisionOperation
00017 from arm_navigation_msgs.msg import Shape
00018 from geometry_msgs.msg import Pose
00019 
00020 get_collision_objects_service_name = "get_collision_objects"
00021 default_prefix = "/environment_server"
00022 
00023 class TestCollisionObjects(unittest.TestCase):
00024 
00025     def test_add_convert_objects(self):
00026 
00027         rospy.init_node('test_collision_objects')
00028 
00029         obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00030         att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject,latch=True)
00031 
00032         global get_collision_objects_service_name 
00033         full_name = default_prefix+'/'+get_collision_objects_service_name
00034         rospy.wait_for_service(full_name)
00035         get_collision_objects_service = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetCollisionObjects)  
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         #tripling for publish wonkitude
00050 
00051         att_pub.publish(clear_all_att)
00052 
00053         obj_pub.publish(clear_all_obj)
00054 
00055         rospy.sleep(2.)
00056       
00057         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00058         try:
00059             get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00060         except rospy.ServiceException, e:
00061             print "Service call failed: %s"%e
00062 
00063         #should have no objects
00064         self.assertEquals(0, len(get_collision_objects_res.collision_objects))
00065         self.assertEquals(0, len(get_collision_objects_res.attached_collision_objects))
00066         
00067         # first add a couple objects to the environment
00068         
00069         obj1 = CollisionObject()
00070 
00071         obj1.header.stamp = rospy.Time.now()
00072         obj1.header.frame_id = "base_link"
00073         obj1.id = "obj1";
00074         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00075         obj1.shapes = [Shape() for _ in range(2)]
00076         obj1.shapes[0].type = Shape.BOX
00077         obj1.shapes[0].dimensions = [float() for _ in range(3)]
00078         obj1.shapes[0].dimensions[0] = 1.0
00079         obj1.shapes[0].dimensions[1] = 1.0
00080         obj1.shapes[0].dimensions[2] = .05
00081         obj1.shapes[1].type = Shape.BOX
00082         obj1.shapes[1].dimensions = [float() for _ in range(3)]
00083         obj1.shapes[1].dimensions[0] = 1.0
00084         obj1.shapes[1].dimensions[1] = 1.0
00085         obj1.shapes[1].dimensions[2] = .05
00086         obj1.poses = [Pose() for _ in range(2)]
00087         obj1.poses[0].position.x = 1.0
00088         obj1.poses[0].position.y = 0
00089         obj1.poses[0].position.z = .5
00090         obj1.poses[0].orientation.x = 0
00091         obj1.poses[0].orientation.y = 0
00092         obj1.poses[0].orientation.z = 0
00093         obj1.poses[0].orientation.w = 1
00094         obj1.poses[1].position.x = 1.0
00095         obj1.poses[1].position.y = 0
00096         obj1.poses[1].position.z = .75
00097         obj1.poses[1].orientation.x = 0
00098         obj1.poses[1].orientation.y = 0
00099         obj1.poses[1].orientation.z = 0
00100         obj1.poses[1].orientation.w = 1
00101         
00102         obj_pub.publish(obj1)
00103         
00104         rospy.sleep(2.)
00105 
00106         obj2 = CollisionObject();
00107         
00108         obj2.header.stamp = rospy.Time.now()
00109         obj2.header.frame_id = "base_link"
00110         obj2.id = "obj2";
00111         obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00112         obj2.shapes = [Shape() for _ in range(1)]
00113         obj2.shapes[0].type = Shape.BOX
00114         obj2.shapes[0].dimensions = [float() for _ in range(3)]
00115         obj2.shapes[0].dimensions[0] = 1.0
00116         obj2.shapes[0].dimensions[1] = 1.0
00117         obj2.shapes[0].dimensions[2] = .05
00118         obj2.poses = [Pose() for _ in range(1)]
00119         obj2.poses[0].position.x = .15
00120         obj2.poses[0].position.y = 0
00121         obj2.poses[0].position.z = .5
00122         obj2.poses[0].orientation.x = 0
00123         obj2.poses[0].orientation.y = 0
00124         obj2.poses[0].orientation.z = 0
00125         obj2.poses[0].orientation.w = 1
00126         
00127         obj_pub.publish(obj2)
00128 
00129         rospy.loginfo("WTF")
00130         
00131         rospy.sleep(2.)
00132 
00133         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00134         
00135         get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00136         
00137         #should have two objects and no attached objects
00138         self.assertEquals(2, len(get_collision_objects_res.collision_objects))
00139         self.assertEquals(0, len(get_collision_objects_res.attached_collision_objects))
00140 
00141         conv_object = AttachedCollisionObject();
00142         conv_object.object.header.stamp = rospy.Time.now()
00143         conv_object.object.header.frame_id = "base_link"
00144         conv_object.link_name = "base_link"
00145         conv_object.object.id = "obj2"
00146         conv_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
00147         att_pub.publish(conv_object)
00148 
00149         rospy.sleep(2.)
00150 
00151         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00152         
00153         get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00154         
00155         #should have one object non-attached and one attached
00156         self.assertEquals(1, len(get_collision_objects_res.collision_objects))
00157         self.assertEquals(1, len(get_collision_objects_res.attached_collision_objects))
00158         self.assertEquals("obj1", get_collision_objects_res.collision_objects[0].id)
00159         self.assertEquals("obj2", get_collision_objects_res.attached_collision_objects[0].object.id)
00160 
00161         #now we add attach another object directly to a different link
00162         att_object = AttachedCollisionObject();
00163         att_object.object.header.stamp = rospy.Time.now()
00164         att_object.object.header.frame_id = "base_link"
00165         att_object.link_name = "r_gripper_r_finger_tip_link"
00166         att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00167         att_object.object = CollisionObject();
00168         
00169         att_object.object.header.stamp = rospy.Time.now()
00170         att_object.object.header.frame_id = "base_link"
00171         att_object.object.id = "obj3"
00172         att_object.object.shapes = [Shape() for _ in range(1)]
00173         att_object.object.shapes[0].type = Shape.BOX
00174         att_object.object.shapes[0].dimensions = [float() for _ in range(3)]
00175         att_object.object.shapes[0].dimensions[0] = .2
00176         att_object.object.shapes[0].dimensions[1] = 0.3
00177         att_object.object.shapes[0].dimensions[2] = .05
00178         att_object.object.poses = [Pose() for _ in range(1)]
00179         att_object.object.poses[0].position.x = .15
00180         att_object.object.poses[0].position.y = 0
00181         att_object.object.poses[0].position.z = 1.0
00182         att_object.object.poses[0].orientation.x = 0
00183         att_object.object.poses[0].orientation.y = 0
00184         att_object.object.poses[0].orientation.z = 0
00185         att_object.object.poses[0].orientation.w = 1
00186         att_pub.publish(att_object)
00187     
00188         rospy.sleep(2.)
00189 
00190         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00191         
00192         get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00193         
00194         #should have one object and two attached
00195         self.assertEquals(1, len(get_collision_objects_res.collision_objects))
00196         self.assertEquals(2, len(get_collision_objects_res.attached_collision_objects))
00197         self.assertEquals("obj1", get_collision_objects_res.collision_objects[0].id)
00198 
00199         #deleting those attached to base link
00200         del_object = AttachedCollisionObject();
00201         del_object.object.header.stamp = rospy.Time.now()
00202         del_object.object.header.frame_id = "base_link"
00203         del_object.link_name = "base_link"
00204         del_object.object.id = "all"
00205         del_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00206         att_pub.publish(del_object)
00207 
00208         rospy.sleep(2.)
00209 
00210         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00211         
00212         get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00213         
00214         #should have one object and two attached
00215         self.assertEquals(1, len(get_collision_objects_res.collision_objects))
00216         self.assertEquals(1, len(get_collision_objects_res.attached_collision_objects))
00217         self.assertEquals("obj1", get_collision_objects_res.collision_objects[0].id)
00218         self.assertEquals("obj3", get_collision_objects_res.attached_collision_objects[0].object.id)
00219 
00220         #converting back to object
00221         back_object = AttachedCollisionObject();
00222         back_object.object.header.stamp = rospy.Time.now()
00223         back_object.object.header.frame_id = "base_link"
00224         back_object.link_name = "r_gripper_r_finger_tip_link"
00225         back_object.object.id = "obj3"
00226         back_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
00227         att_pub.publish(back_object)
00228 
00229         rospy.sleep(2.)
00230 
00231         get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest()
00232         
00233         get_collision_objects_res = get_collision_objects_service(get_collision_objects_req)
00234         
00235         #should have one object and two attached
00236         self.assertEquals(2, len(get_collision_objects_res.collision_objects))
00237         self.assertEquals(0, len(get_collision_objects_res.attached_collision_objects))
00238 
00239 if __name__ == '__main__':
00240     import rostest
00241     rostest.unitrun('test_collision_objects', 'test_collision_objects', TestCollisionObjects)
00242 
00243     


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43