workspace_calibration.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import rostest
4 
5 import random
6 import unittest
7 
8 import actionlib
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
12 
13 from geometry_msgs.msg import Pose, Point, Quaternion
14 
15 tolerance = 0.001
16 
17 
18 class TestWorkspaceCalibration(unittest.TestCase):
19  def setUp(self):
20  self.calibration_client = actionlib.SimpleActionClient("calibrate_workspace", CalibrateWorkspaceAction)
21  self.calibration_client.wait_for_server()
22  self.locate_pattern_client = actionlib.SimpleActionClient("locate_pattern", LocatePatternAction)
23  self.locate_pattern_client.wait_for_server()
24 
26  for i in range(5):
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))
31 
32  # Calibrate for the random pose.
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)
38 
39  # Locate the pattern. It should now be at the random pose.
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)
45 
46  self.assertTrue(result.found_pattern)
47  self.assertEqual(len(result.pattern_poses), 1)
48 
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)
53 
54 
55 if __name__ == "__main__":
56  try:
57  rospy.init_node("test_workspace_calibration")
58  rostest.rosrun("ensenso_camera_test", "test_workspace_calibration", TestWorkspaceCalibration)
59  except rospy.ROSInterruptException:
60  pass


ensenso_camera_test
Author(s): Ensenso
autogenerated on Thu May 6 2021 02:50:10