def test_cov(self):
print ""
config, robot_params = loadSystem()
joint_points = [
JointState(position=[0, 0, 1]),
JointState(position=[pi / 2, 0, 2])
]
sensor = TiltingLaserSensor(
config["tilting_lasers"]["laserA"],
LaserMeasurement(laser_id="laserA", joint_points=joint_points))
sensor.update_config(robot_params)
cov = sensor.compute_cov(None)
print "Cov:"
print cov
self.assertAlmostEqual(cov[0, 0], 1.0, 6)
self.assertAlmostEqual(cov[1, 1], 1.0, 6)
self.assertAlmostEqual(cov[2, 2], 1.0, 6)
self.assertAlmostEqual(cov[3, 3], 4.0, 6)
self.assertAlmostEqual(cov[4, 4], 4.0, 6)
self.assertAlmostEqual(cov[5, 5], 1.0, 6)
