def test_get_set(self):
pose = Pose(Position(1, 2), 3)
self.assertTrue(hasattr(pose, 'position'))
self.assertTrue(hasattr(pose, 'orientation'))
self.assertTrue(pose.position == Position(1, 2))
self.assertTrue(pose.orientation == 3)
pose.position = Position(2, 3)
pose.orientation = 1
self.assertTrue(pose.position == Position(2, 3))
self.assertTrue(pose.orientation == 1)
self.assertEqual(pose[0], 2)
self.assertEqual(pose[1], 3)
self.assertEqual(pose[2], 1)
with self.assertRaises(IndexError):
pose[3]
def test_compare_orientation(self):
# numeric input
pose = Pose(Position(), 0)
self.assertTrue(pose.compare_orientation(0))
tol = 0.01
self.assertTrue(pose.compare_orientation(0.00399, tol))
self.assertTrue(pose.compare_orientation(-0.00399, tol))
pose.orientation = m.pi
self.assertTrue(pose.compare_orientation(m.pi))
self.assertTrue(pose.compare_orientation(-m.pi))
tol = 0.01
self.assertTrue(pose.compare_orientation(m.pi + 0.999 * tol, tol))
self.assertTrue(pose.compare_orientation(-m.pi - 0.999 * tol, tol))
self.assertTrue(pose.compare_orientation(m.pi - 0.999 * tol, tol))
self.assertTrue(pose.compare_orientation(-m.pi + 0.999 * tol, tol))
self.assertFalse(pose.compare_orientation(m.pi + 1.001 * tol, tol))
self.assertFalse(pose.compare_orientation(-m.pi - 1.001 * tol, tol))
self.assertFalse(pose.compare_orientation(m.pi - 1.001 * tol, tol))
self.assertFalse(pose.compare_orientation(-m.pi + 1.001 * tol, tol))
pose1 = Pose(Position(1, 1), 0)
pose2 = Pose(Position(10, 10), 0)
self.assertTrue(pose1.compare_orientation(pose2))
