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 import sensor_msgs.msg
00012 import mapping_msgs.msg
00013 from arm_navigation_msgs import arm_navigation_msgs_utils
00014 from mapping_msgs.msg import CollisionObject
00015 from mapping_msgs.msg import AttachedCollisionObject
00016 from arm_navigation_msgs.msg import CollisionOperation, ArmNavigationErrorCodes, AllowedContactSpecification
00017 from arm_navigation_msgs.msg import Shape
00018 from geometry_msgs.msg import Pose, PointStamped
00019 from arm_navigation_msgs.srv import GetRobotState, GetStateValidity, GetRobotStateRequest, GetStateValidityRequest, GetGroupInfoRequest, GetGroupInfo
00020 from tf import TransformListener
00021
00022 default_prefix = "/environment_server"
00023
00024 class TestGetStateValidity(unittest.TestCase):
00025
00026 def setUp(self):
00027
00028 rospy.init_node('test_allowed_collision_near_start')
00029
00030 self.tf = TransformListener()
00031
00032 self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00033
00034 rospy.wait_for_service(default_prefix+'/get_robot_state')
00035 self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
00036
00037 rospy.wait_for_service(default_prefix+'/get_state_validity')
00038 self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
00039
00040 rospy.wait_for_service(default_prefix+'/get_group_info')
00041 self.get_group_info_server = rospy.ServiceProxy(default_prefix+'/get_group_info', GetGroupInfo)
00042
00043 rospy.sleep(3.)
00044
00045 obj1 = CollisionObject()
00046
00047 obj1.header.stamp = rospy.Time.now()
00048 obj1.header.frame_id = "r_gripper_palm_link"
00049 obj1.id = "test_object"
00050 obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00051 obj1.shapes = [Shape() for _ in range(1)]
00052 obj1.shapes[0].type = Shape.BOX
00053 obj1.shapes[0].dimensions = [float() for _ in range(3)]
00054 obj1.shapes[0].dimensions[0] = .03
00055 obj1.shapes[0].dimensions[1] = .03
00056 obj1.shapes[0].dimensions[2] = .03
00057 obj1.poses = [Pose() for _ in range(1)]
00058 obj1.poses[0].position.x = 0
00059 obj1.poses[0].position.y = 0
00060 obj1.poses[0].position.z = 0
00061 obj1.poses[0].orientation.x = 0
00062 obj1.poses[0].orientation.y = 0
00063 obj1.poses[0].orientation.z = 0
00064 obj1.poses[0].orientation.w = 1
00065
00066 self.obj_pub.publish(obj1)
00067
00068 rospy.sleep(2.)
00069
00070
00071 obj2 = CollisionObject()
00072
00073 obj2.header.stamp = rospy.Time.now()
00074 obj2.header.frame_id = "base_link"
00075 obj2.id = "base_object"
00076 obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00077 obj2.shapes = [Shape() for _ in range(1)]
00078 obj2.shapes[0].type = Shape.BOX
00079 obj2.shapes[0].dimensions = [float() for _ in range(3)]
00080 obj2.shapes[0].dimensions[0] = .1
00081 obj2.shapes[0].dimensions[1] = .1
00082 obj2.shapes[0].dimensions[2] = .1
00083 obj2.poses = [Pose() for _ in range(1)]
00084 obj2.poses[0].position.x = 0
00085 obj2.poses[0].position.y = 0
00086 obj2.poses[0].position.z = 0
00087 obj2.poses[0].orientation.x = 0
00088 obj2.poses[0].orientation.y = 0
00089 obj2.poses[0].orientation.z = 0
00090 obj2.poses[0].orientation.w = 1
00091
00092 self.obj_pub.publish(obj2)
00093
00094 rospy.sleep(5.)
00095
00096 def test_get_state_validity(self):
00097
00098 req = GetRobotStateRequest()
00099
00100 cur_state = self.get_robot_state_server.call(req)
00101
00102
00103
00104
00105 state_req = GetStateValidityRequest()
00106 state_req.robot_state = cur_state.robot_state
00107
00108 group_req = GetGroupInfoRequest()
00109 group_req.group_name = 'right_arm'
00110
00111 group_res = self.get_group_info_server.call(group_req)
00112
00113 self.failIf(len(group_res.link_names) == 0)
00114
00115 right_arm_links = group_res.link_names
00116
00117 group_req.group_name = ''
00118
00119 group_res = self.get_group_info_server.call(group_req)
00120
00121 self.failIf(len(group_res.link_names) == 0)
00122
00123 state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(group_res.link_names,
00124 right_arm_links)
00125
00126 for i in state_req.ordered_collision_operations.collision_operations:
00127 print 'Disabling ', i.object1
00128
00129 state_req.check_collisions = True
00130
00131 res = self.get_state_validity_server.call(state_req)
00132
00133
00134 self.failIf(res.error_code.val == res.error_code.SUCCESS)
00135 self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00136
00137
00138 self.failIf(len(res.contacts) == 0)
00139
00140 for c in res.contacts:
00141
00142
00143 contact_point = PointStamped()
00144 contact_point.header = c.header
00145 contact_point.point = c.position
00146 contact_point_base = self.tf.transformPoint("base_link",contact_point)
00147
00148 gripper_point = PointStamped()
00149 gripper_point.header.stamp = c.header.stamp
00150 gripper_point.header.frame_id = "r_gripper_palm_link"
00151 gripper_point.point.x = 0
00152 gripper_point.point.y = 0
00153 gripper_point.point.z = 0
00154 gripper_point_base = self.tf.transformPoint("base_link", gripper_point)
00155
00156 print 'x diff:', abs(gripper_point_base.point.x-contact_point_base.point.x)
00157 print 'y diff:', abs(gripper_point_base.point.y-contact_point_base.point.y)
00158 print 'z diff:', abs(gripper_point_base.point.z-contact_point_base.point.z)
00159
00160 self.failIf(abs(gripper_point_base.point.x-contact_point_base.point.x) > .031)
00161 self.failIf(abs(gripper_point_base.point.y-contact_point_base.point.y) > .031)
00162 self.failIf(abs(gripper_point_base.point.z-contact_point_base.point.z) > .031)
00163
00164
00165 obj2 = CollisionObject()
00166
00167 obj2.header.stamp = rospy.Time.now()
00168 obj2.header.frame_id = "base_link"
00169 obj2.id = "test_object"
00170 obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00171
00172 self.obj_pub.publish(obj2)
00173
00174 rospy.sleep(5.)
00175
00176 cur_state = self.get_robot_state_server.call(req)
00177 state_req.robot_state = cur_state.robot_state
00178
00179 res = self.get_state_validity_server.call(state_req)
00180
00181
00182 self.failIf(res.error_code.val != res.error_code.SUCCESS)
00183
00184
00185 state_req.ordered_collision_operations.collision_operations = []
00186
00187 res = self.get_state_validity_server.call(state_req)
00188
00189
00190 self.failIf(res.error_code.val == res.error_code.SUCCESS)
00191
00192 if __name__ == '__main__':
00193 import rostest
00194 rostest.unitrun('test_get_state_validity', 'test_get_state_validity', TestGetStateValidity, sys.argv)
00195
00196