00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 import roslib; roslib.load_manifest('fingertip_pressure')
00039
00040 import rospy
00041 import sys
00042 import time
00043 import unittest
00044 import rostest
00045
00046 from fingertip_pressure.msg import PressureInfo
00047
00048
00049 class PressureInfoTest(unittest.TestCase):
00050 def __init__(self, *args):
00051 super(PressureInfoTest, self).__init__(*args)
00052 rospy.init_node('pressure_info_test')
00053
00054
00055 self.msg = None
00056 sub = rospy.Subscriber('pressure/r_gripper_motor_info',
00057 PressureInfo, self.callback)
00058 timeout_t = rospy.get_time() + 2
00059 print 'waiting for message'
00060 while self.msg == None and timeout_t > rospy.get_time():
00061 rospy.sleep(0.1)
00062 print 'done waiting for message'
00063 sub.unregister()
00064 self.assertNotEquals(self.msg, None)
00065
00066 def callback(self, msg):
00067 print 'got message'
00068
00069 for i in range(0,2):
00070 for j in range(0,22):
00071 fact = [1,-1][i]
00072 msg.sensor[i].center[j].x = msg.sensor[i].center[j].x + 0.004
00073 msg.sensor[i].center[j].y = msg.sensor[i].center[j].y + 0.015 * fact
00074 self.msg = msg
00075
00076 def test_array_sizes(self):
00077 self.assertEquals(len(self.msg.sensor), 2, "Should have two PressureInfoElements")
00078 self.assertEquals(len(self.msg.sensor[1].center), 22, "Should have 22 centers.")
00079 self.assertEquals(len(self.msg.sensor[1].halfside1), 22, "Should have 22 halfside1")
00080 self.assertEquals(len(self.msg.sensor[1].halfside2), 22, "Should have 22 halfside2")
00081 self.assertEquals(len(self.msg.sensor[1].force_per_unit), 22, "Should have 22 pressure")
00082
00083 def test_cross_products(self):
00084 for j in range(0,2):
00085 yori = [1, -1][j]
00086 for i in range(0,22):
00087 a = self.msg.sensor[j].halfside1[i]
00088 b = self.msg.sensor[j].halfside2[i]
00089 c = self.msg.sensor[j].center[i]
00090 c.y = c.y - yori * .005
00091 det = a.x * b.y * c.z + b.x * c.y * a.z + c.x * a.y * b.z\
00092 -a.x * c.y * b.z - b.x * a.y * c.z - c.x * b.y * a.z
00093 self.assertTrue(det > 0,
00094 "Wrong orientation for sensor %i on tip %i, det=%e, %e %e %e %e %e %e %e %e %e"%(i, j, det, a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z))
00095
00096 def test_bounding_box(self):
00097 for j in range(0,2):
00098 yori = [1, -1][j]
00099 for i in range(0,22):
00100 a = self.msg.sensor[j].halfside1[i]
00101 b = self.msg.sensor[j].halfside2[i]
00102 c = self.msg.sensor[j].center[i]
00103 msg = "Bound box error sensor %i, tip %i, %e %e %e %e %e %e %e %e %e"%(i, j, a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z)
00104 self.assertTrue(c.x - abs(a.x) > 0, msg)
00105 self.assertTrue(c.x - abs(b.x) > 0, msg)
00106 self.assertTrue(c.x + abs(a.x) <= 0.035, msg)
00107 self.assertTrue(c.x + abs(b.x) <= 0.035, msg)
00108 self.assertTrue(yori * c.y - abs(a.y) >= 0, msg)
00109 self.assertTrue(yori * c.y - abs(b.y) >= 0, msg)
00110 self.assertTrue(yori * c.y + abs(a.y) < 11, msg)
00111 self.assertTrue(yori * c.y + abs(b.y) < 11, msg)
00112 self.assertTrue(abs(c.z) + abs(a.z) <= 11.5, msg)
00113 self.assertTrue(abs(c.z) + abs(b.z) <= 11.5, msg)
00114
00115
00116 if __name__ == '__main__':
00117 import rostest
00118 time.sleep(0.75)
00119 try:
00120 rostest.rosrun('fingertip_pressure', 'pressure_info_test', PressureInfoTest)
00121 except KeyboardInterrupt, e:
00122 pass
00123 print "exiting"
00124
00125
00126