Go to the documentation of this file.00001
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
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
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
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
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
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
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)