test_attached_object_collisions.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 from arm_navigation_msgs.srv import GetStateValidity,GetStateValidityRequest
00020 from arm_navigation_msgs.msg import ContactInformation
00021 
00022 default_prefix = "/environment_server"
00023 
00024 class TestAttachedObjectCollisions(unittest.TestCase):
00025 
00026     def setUp(self):
00027 
00028         rospy.init_node('test_attached_object_collisions')
00029 
00030         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00031         self.att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject,latch=True)
00032 
00033         rospy.wait_for_service(default_prefix+'/get_state_validity')
00034         self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
00035         
00036         self.state_req = GetStateValidityRequest()
00037         self.state_req.robot_state.joint_state.name = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']
00038         self.state_req.robot_state.joint_state.position = [float(0.0) for _ in range(7)]
00039         self.state_req.check_collisions = True
00040 
00041         self.table = CollisionObject()
00042 
00043         self.table.header.stamp = rospy.Time.now()
00044         self.table.header.frame_id = "base_link"
00045         self.table.id = "tabletop"
00046         self.table.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00047         self.table.shapes = [Shape() for _ in range(1)]
00048         self.table.shapes[0].type = Shape.BOX
00049         self.table.shapes[0].dimensions = [float() for _ in range(3)]
00050         self.table.shapes[0].dimensions[0] = 1.0
00051         self.table.shapes[0].dimensions[1] = 1.0
00052         self.table.shapes[0].dimensions[2] = .05
00053         self.table.poses = [Pose() for _ in range(1)]
00054         self.table.poses[0].position.x = 1.0
00055         self.table.poses[0].position.y = 0
00056         self.table.poses[0].position.z = .5
00057         self.table.poses[0].orientation.x = 0
00058         self.table.poses[0].orientation.y = 0
00059         self.table.poses[0].orientation.z = 0
00060         self.table.poses[0].orientation.w = 1
00061 
00062         #not publishing table right away
00063 
00064         self.att_object = AttachedCollisionObject();
00065         self.att_object.object.header.stamp = rospy.Time.now()
00066         self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
00067         self.att_object.link_name = "r_gripper_r_finger_tip_link"
00068         self.att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00069         self.att_object.object = CollisionObject();
00070 
00071         self.att_object.object.header.stamp = rospy.Time.now()
00072         self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
00073         self.att_object.object.id = "pole"
00074         self.att_object.object.shapes = [Shape() for _ in range(1)]
00075         self.att_object.object.shapes[0].type = Shape.CYLINDER
00076         self.att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
00077         self.att_object.object.shapes[0].dimensions[0] = .02
00078         self.att_object.object.shapes[0].dimensions[1] = 1.2
00079         self.att_object.object.poses = [Pose() for _ in range(1)]
00080         self.att_object.object.poses[0].position.x = 0
00081         self.att_object.object.poses[0].position.y = 0
00082         self.att_object.object.poses[0].position.z = 0
00083         self.att_object.object.poses[0].orientation.x = 0
00084         self.att_object.object.poses[0].orientation.y = 0
00085         self.att_object.object.poses[0].orientation.z = 0
00086         self.att_object.object.poses[0].orientation.w = 1
00087 
00088         self.att_pub.publish(self.att_object)
00089 
00090         self.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
00091                             'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link']
00092 
00093         rospy.sleep(2.)
00094         
00095     def test_attached_collisions_touch_links(self):
00096 
00097         #original attached object had no touch links
00098         
00099         self.state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
00100 
00101         res = self.get_state_validity_server.call(self.state_req)
00102 
00103         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00104         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00105 
00106         #should be some contacts
00107         self.failIf(len(res.contacts) == 0)
00108         
00109         for c in res.contacts:
00110 
00111             is_first = False
00112             
00113             if(c.contact_body_1 == 'r_gripper_r_finger_tip_link'):
00114                 is_first = True
00115             elif(c.contact_body_2 != 'r_gripper_r_finger_tip_link'):
00116                 self.fail("Contacts but not with correct link")
00117 
00118             if c.contact_body_1 not in self.touch_links:
00119                 self.fail("Contact body 1 ", c.contact_body_1, " not in touch links");
00120 
00121             if c.contact_body_2 not in self.touch_links:
00122                 self.fail("Contact body 2 ", c.contact_body_2, " not in touch links");
00123 
00124             self.failIf(not(c.contact_body_1 in self.touch_links))
00125             self.failIf(not(c.contact_body_2 in self.touch_links))
00126 
00127             if(is_first):
00128                 self.failIf(c.body_type_1 != ContactInformation.ATTACHED_BODY)
00129                 self.failIf(c.attached_body_1 != "pole")
00130             else:
00131                 self.failIf(c.body_type_2 != ContactInformation.ATTACHED_BODY)
00132                 self.failIf(c.attached_body_2 != "pole")
00133 
00134             
00135 
00136         #adding in touch links
00137         self.att_object.touch_links = self.touch_links
00138         self.att_pub.publish(self.att_object)
00139 
00140         rospy.sleep(2.)
00141 
00142         res = self.get_state_validity_server.call(self.state_req)
00143 
00144         #should be ok now
00145         self.failIf(res.error_code.val != res.error_code.SUCCESS)
00146 
00147     def test_attached_collisions_with_static_objects(self):
00148         
00149         self.att_object.touch_links = self.touch_links
00150         self.att_pub.publish(self.att_object)
00151 
00152         rospy.sleep(2.)
00153 
00154         self.obj_pub.publish(self.table)
00155         
00156         rospy.sleep(2.)
00157 
00158         self.state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
00159 
00160         res = self.get_state_validity_server.call(self.state_req)
00161 
00162         self.failIf(res.error_code.val == res.error_code.SUCCESS)
00163         self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 
00164         
00165         #should be some contacts
00166         self.failIf(len(res.contacts) == 0)
00167         
00168         for c in res.contacts:
00169             
00170             self.failIf(c.contact_body_1 != 'r_gripper_r_finger_tip_link')
00171             self.failIf(c.body_type_1 != ContactInformation.ATTACHED_BODY)
00172             self.failIf(c.attached_body_1 != "pole")
00173 
00174             self.failIf(c.contact_body_2 != 'tabletop')
00175             self.failIf(c.body_type_2 != ContactInformation.OBJECT)
00176 
00177 if __name__ == '__main__':
00178     import rostest
00179     rostest.unitrun('test_attached_object_collisions', 'test_attached_object_collisions', TestAttachedObjectCollisions, sys.argv)


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