9 from actionlib_msgs.msg
import GoalStatus
10 from ensenso_camera_msgs.msg
import CalibrateWorkspaceAction, CalibrateWorkspaceGoal
11 from ensenso_camera_msgs.msg
import LocatePatternAction, LocatePatternGoal
13 from geometry_msgs.msg
import Pose, Point, Quaternion
21 self.calibration_client.wait_for_server()
23 self.locate_pattern_client.wait_for_server()
27 x = random.randint(-500, 500)
28 y = random.randint(-500, 500)
29 z = random.randint(-500, 500)
30 pose = Pose(Point(x, y, z), Quaternion(0, 0, 0, 1))
33 self.calibration_client.send_goal(CalibrateWorkspaceGoal(defined_pattern_pose=pose))
34 self.calibration_client.wait_for_result()
35 self.assertEqual(self.calibration_client.get_state(), GoalStatus.SUCCEEDED)
36 self.assertEqual(self.calibration_client.get_result().error.code, 0)
37 self.assertTrue(self.calibration_client.get_result().successful)
40 self.locate_pattern_client.send_goal(LocatePatternGoal())
41 self.locate_pattern_client.wait_for_result()
42 self.assertEqual(self.locate_pattern_client.get_state(), GoalStatus.SUCCEEDED)
43 result = self.locate_pattern_client.get_result()
44 self.assertEqual(result.error.code, 0)
46 self.assertTrue(result.found_pattern)
47 self.assertEqual(len(result.pattern_poses), 1)
49 pose = result.pattern_poses[0].pose
50 self.assertTrue(abs(pose.position.x - x) < tolerance)
51 self.assertTrue(abs(pose.position.y - y) < tolerance)
52 self.assertTrue(abs(pose.position.z - z) < tolerance)
55 if __name__ ==
"__main__":
57 rospy.init_node(
"test_workspace_calibration")
58 rostest.rosrun(
"ensenso_camera_test",
"test_workspace_calibration", TestWorkspaceCalibration)
59 except rospy.ROSInterruptException:
def test_workspace_calibration(self)