test_robot_measurement_cache.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 import roslib; roslib.load_manifest('calibration_launch')
35 
36 import sys
37 import unittest
38 import rospy
39 
40 from capture_executive.robot_measurement_cache import RobotMeasurementCache
41 from calibration_msgs.msg import *
42 
43 ## A sample python unit test
44 class TestCache(unittest.TestCase):
45  ## test 1 == 1
47  cache = RobotMeasurementCache()
48  cache.reconfigure(["cam1"], [], [])
49  cache.set_max_sizes( {"cam1":100}, {}, {})
50 
51  for n in range(1,11):
52  msg = CameraMeasurement()
53  msg.header.stamp = rospy.Time(n*10,0)
54  cache.add_cam_measurement("cam1", msg)
55 
56  m_robot = cache.request_robot_measurement(rospy.Time(19,0), rospy.Time(31,0))
57 
58  self.assertTrue(m_robot is not None)
59  self.assertEquals( len(m_robot.M_cam), 1 )
60  self.assertEquals( len(m_robot.M_chain), 0 )
61  self.assertEquals( len(m_robot.M_laser), 0 )
62 
63  m_robot = cache.request_robot_measurement(rospy.Time(21,0), rospy.Time(29,0))
64  self.assertTrue(m_robot is None)
65 
67  cache = RobotMeasurementCache()
68  cache.reconfigure(["cam1", "cam2"], [], [])
69  cache.set_max_sizes( {"cam1":100, "cam2":100}, {}, {})
70 
71  for n in range(1,11):
72  msg = CameraMeasurement()
73  msg.header.stamp = rospy.Time(n*10,0)
74  cache.add_cam_measurement("cam1", msg)
75 
76  for n in range(1,11):
77  msg = CameraMeasurement()
78  msg.header.stamp = rospy.Time(n*10+1,0)
79  cache.add_cam_measurement("cam2", msg)
80 
81  m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
82 
83  self.assertTrue(m_robot is not None)
84  self.assertEquals( len(m_robot.M_cam), 2 )
85  self.assertEquals( len(m_robot.M_chain), 0 )
86  self.assertEquals( len(m_robot.M_laser), 0 )
87 
88  m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(30,0))
89  self.assertTrue(m_robot is None)
90 
91  def test_chain_easy(self):
92  cache = RobotMeasurementCache()
93  cache.reconfigure([], ["chain1"], [])
94  cache.set_max_sizes( {}, {"chain1":100}, {})
95 
96  for n in range(1,11):
97  msg = ChainMeasurement()
98  msg.header.stamp = rospy.Time(n*10,0)
99  cache.add_chain_measurement("chain1", msg)
100 
101  m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
102 
103  self.assertTrue(m_robot is not None)
104  self.assertEquals( len(m_robot.M_cam), 0 )
105  self.assertEquals( len(m_robot.M_chain), 1 )
106  self.assertEquals( len(m_robot.M_laser), 0 )
107 
108  m_robot = cache.request_robot_measurement(rospy.Time(21,0), rospy.Time(29,0))
109  self.assertTrue(m_robot is None)
110 
111  def test_laser_easy(self):
112  cache = RobotMeasurementCache()
113  cache.reconfigure([], [], ["laser1"])
114  cache.set_max_sizes( {}, {}, {"laser1":100})
115 
116  for n in range(1,11):
117  msg = LaserMeasurement()
118  cache.add_laser_measurement("laser1", msg, rospy.Time(n*10,0), rospy.Time(n*10+2,0))
119 
120  m_robot = cache.request_robot_measurement(rospy.Time(19,0), rospy.Time(32,0))
121 
122  self.assertTrue(m_robot is not None)
123  self.assertEquals( len(m_robot.M_cam), 0 )
124  self.assertEquals( len(m_robot.M_chain), 0 )
125  self.assertEquals( len(m_robot.M_laser), 1 )
126 
127  m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(21,0))
128  self.assertTrue(m_robot is None)
129 
130 if __name__ == '__main__':
131  import rostest
132  rostest.unitrun('pr2_calibration_executive', 'test_robot_measurement_cache', TestCache)


calibration_launch
Author(s): Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:33