pressure_info_test.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #***********************************************************
3 #* Software License Agreement (BSD License)
4 #*
5 #* Copyright (c) 2008, Willow Garage, Inc.
6 #* All rights reserved.
7 #*
8 #* Redistribution and use in source and binary forms, with or without
9 #* modification, are permitted provided that the following conditions
10 #* are met:
11 #*
12 #* * Redistributions of source code must retain the above copyright
13 #* notice, this list of conditions and the following disclaimer.
14 #* * Redistributions in binary form must reproduce the above
15 #* copyright notice, this list of conditions and the following
16 #* disclaimer in the documentation and/or other materials provided
17 #* with the distribution.
18 #* * Neither the name of the Willow Garage nor the names of its
19 #* contributors may be used to endorse or promote products derived
20 #* from this software without specific prior written permission.
21 #*
22 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 #* POSSIBILITY OF SUCH DAMAGE.
34 #***********************************************************
35 
36 # Author: Blaise Gassend
37 
38 import roslib; roslib.load_manifest('fingertip_pressure')
39 
40 import rospy
41 import sys
42 import time
43 import unittest
44 import rostest
45 
46 from fingertip_pressure.msg import PressureInfo
47 
48 ## Tests the PressureInfo message produced by sensor_info.py
49 class PressureInfoTest(unittest.TestCase):
50  def __init__(self, *args):
51  super(PressureInfoTest, self).__init__(*args)
52  rospy.init_node('pressure_info_test')
53 
54  # Read one message from the topic
55  self.msg = None
56  sub = rospy.Subscriber('pressure/r_gripper_motor_info',
57  PressureInfo, self.callback)
58  timeout_t = rospy.get_time() + 2
59  print ('waiting for message')
60  while self.msg == None and timeout_t > rospy.get_time():
61  rospy.sleep(0.1)
62  print ('done waiting for message')
63  sub.unregister()
64  self.assertNotEquals(self.msg, None)
65 
66  def callback(self, msg):
67  print ('got message')
68  # Account for offset origin of sensor origin.
69  for i in range(0,2):
70  for j in range(0,22):
71  fact = [1,-1][i]
72  msg.sensor[i].center[j].x = msg.sensor[i].center[j].x + 0.004
73  msg.sensor[i].center[j].y = msg.sensor[i].center[j].y + 0.015 * fact
74  self.msg = msg
75 
76  def test_array_sizes(self):
77  self.assertEquals(len(self.msg.sensor), 2, "Should have two PressureInfoElements")
78  self.assertEquals(len(self.msg.sensor[1].center), 22, "Should have 22 centers.")
79  self.assertEquals(len(self.msg.sensor[1].halfside1), 22, "Should have 22 halfside1")
80  self.assertEquals(len(self.msg.sensor[1].halfside2), 22, "Should have 22 halfside2")
81  self.assertEquals(len(self.msg.sensor[1].force_per_unit), 22, "Should have 22 pressure")
82 
84  for j in range(0,2):
85  yori = [1, -1][j]
86  for i in range(0,22):
87  a = self.msg.sensor[j].halfside1[i]
88  b = self.msg.sensor[j].halfside2[i]
89  c = self.msg.sensor[j].center[i]
90  c.y = c.y - yori * .005 # Ensure that we are inside the sensor
91  det = a.x * b.y * c.z + b.x * c.y * a.z + c.x * a.y * b.z\
92  -a.x * c.y * b.z - b.x * a.y * c.z - c.x * b.y * a.z
93  self.assertTrue(det > 0,
94  "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))
95 
96  def test_bounding_box(self):
97  for j in range(0,2):
98  yori = [1, -1][j]
99  for i in range(0,22):
100  a = self.msg.sensor[j].halfside1[i]
101  b = self.msg.sensor[j].halfside2[i]
102  c = self.msg.sensor[j].center[i]
103  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)
104  self.assertTrue(c.x - abs(a.x) > 0, msg)
105  self.assertTrue(c.x - abs(b.x) > 0, msg)
106  self.assertTrue(c.x + abs(a.x) <= 0.035, msg)
107  self.assertTrue(c.x + abs(b.x) <= 0.035, msg)
108  self.assertTrue(yori * c.y - abs(a.y) >= 0, msg)
109  self.assertTrue(yori * c.y - abs(b.y) >= 0, msg)
110  self.assertTrue(yori * c.y + abs(a.y) < 11, msg)
111  self.assertTrue(yori * c.y + abs(b.y) < 11, msg)
112  self.assertTrue(abs(c.z) + abs(a.z) <= 11.5, msg)
113  self.assertTrue(abs(c.z) + abs(b.z) <= 11.5, msg)
114 
115 
116 if __name__ == '__main__':
117  import rostest
118  time.sleep(0.75)
119  try:
120  rostest.rosrun('fingertip_pressure', 'pressure_info_test', PressureInfoTest)
121  except KeyboardInterrupt as e:
122  pass
123  print ("exiting")
124 
125 
126 
Tests the PressureInfo message produced by sensor_info.py.


fingertip_pressure
Author(s): Blaise Gassend
autogenerated on Thu Mar 4 2021 03:10:25