def test_laser_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure([], [], ["laser1"])
cache.set_max_sizes( {}, {}, {"laser1":100})
for n in range(1,11):
msg = LaserMeasurement()
cache.add_laser_measurement("laser1", msg, rospy.Time(n*10,0), rospy.Time(n*10+2,0))
m_robot = cache.request_robot_measurement(rospy.Time(19,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 0 )
self.assertEquals( len(m_robot.M_chain), 0 )
self.assertEquals( len(m_robot.M_laser), 1 )
m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(21,0))
self.assertTrue(m_robot is None)
def test_chain_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure([], ["chain1"], [])
cache.set_max_sizes( {}, {"chain1":100}, {})
for n in range(1,11):
msg = ChainMeasurement()
msg.header.stamp = rospy.Time(n*10,0)
cache.add_chain_measurement("chain1", msg)
m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 0 )
self.assertEquals( len(m_robot.M_chain), 1 )
self.assertEquals( len(m_robot.M_laser), 0 )
m_robot = cache.request_robot_measurement(rospy.Time(21,0), rospy.Time(29,0))
self.assertTrue(m_robot is None)
def test_multi_cam_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure(["cam1", "cam2"], [], [])
cache.set_max_sizes( {"cam1":100, "cam2":100}, {}, {})
for n in range(1,11):
msg = CameraMeasurement()
msg.header.stamp = rospy.Time(n*10,0)
cache.add_cam_measurement("cam1", msg)
for n in range(1,11):
msg = CameraMeasurement()
msg.header.stamp = rospy.Time(n*10+1,0)
cache.add_cam_measurement("cam2", msg)
m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 2 )
self.assertEquals( len(m_robot.M_chain), 0 )
self.assertEquals( len(m_robot.M_laser), 0 )
m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(30,0))
self.assertTrue(m_robot is None)
