def test_get_robot_pan_default(self):
robot, pose, _ = self.init_robot_and_pose()
cam_angles = get_robot_horizon_p_deg(robot, 1)
self.assertTrue(abs(cam_angles[1]) < 1e-10)
def test_get_robot_pan2(self):
angle = -10
robot, pose, _ = self.init_robot_and_pose(roll_angle=angle)
cam_angles = get_robot_horizon_p_deg(robot, 1)
self.assertTrue(abs(cam_angles[1] - angle) < 1e-10)
def test_get_robot_tilt2(self):
angle = 13
robot, pose, internal_camera_angle = self.init_robot_and_pose(pitch_angle=angle)
cam_angles = get_robot_horizon_p_deg(robot, 1)
self.assertEquals(internal_camera_angle, -13)
self.assertTrue(abs(cam_angles[0] + internal_camera_angle - angle) < 1e-10)
