def test_map_odometry_to_table(self): odometry_pos = [1, 2, 3] offset = [0.1, 0.2, 0.3] table_pos = position.map_odometry_to_table(odometry_pos, offset) self.assertAlmostEqual(cos(0.3) * 1 - sin(0.3) * 2 + 0.1, table_pos[0]) self.assertAlmostEqual(sin(0.3) * 1 + cos(0.3) * 2 + 0.2, table_pos[1]) self.assertAlmostEqual(3 + 0.3, table_pos[2])
def test_compute_odometry_offset(self): table_pos = [1, 2, 3] odometry_pos = [4, 5, 6] offset = position.compute_odometry_offset(table_pos, odometry_pos) np.testing.assert_almost_equal(table_pos, position.map_odometry_to_table(odometry_pos, offset))