9 from actionlib_msgs.msg
import GoalStatus
10 from ensenso_camera_msgs.msg
import LocatePatternMonoAction, LocatePatternMonoGoal
12 from helper
import Pose
14 DATA_SET_DIRECTORY =
"../data/mono_camera/locate_pattern/" 20 self.locate_pattern_client.wait_for_server()
23 self.locate_pattern_client.send_goal(LocatePatternMonoGoal())
24 self.locate_pattern_client.wait_for_result()
25 self.assertEqual(self.locate_pattern_client.get_state(), GoalStatus.SUCCEEDED)
26 result = self.locate_pattern_client.get_result()
28 self.assertEqual(result.error.code, 0)
30 self.assertTrue(result.found_pattern)
31 self.assertEqual(len(result.mono_patterns), 1)
32 self.assertEqual(len(result.mono_pattern_poses), 1)
34 self.assertNotEqual(result.frame,
"")
36 pattern = result.mono_patterns[0]
38 self.assertEqual(pattern.grid_spacing, 0.03625)
39 self.assertEqual(pattern.grid_size_x, 7)
40 self.assertEqual(pattern.grid_size_y, 7)
41 self.assertEqual(len(pattern.points), 49)
43 self.assertNotEqual(result.mono_pattern_poses[0].header.stamp, 0)
44 self.assertNotEqual(result.mono_pattern_poses[0].header.frame_id,
"")
46 reference_pattern_pose = Pose.from_json(os.path.join(DATA_SET_DIRECTORY,
"pattern_pose.json"))
47 estimated_pattern_pose = Pose.from_message(result.mono_pattern_poses[0].pose)
48 self.assertTrue(reference_pattern_pose.equals(estimated_pattern_pose))
50 if __name__ ==
"__main__":
52 rospy.init_node(
"test_locate_pattern_mono")
53 rostest.rosrun(
"ensenso_camera_test",
"test_locate_pattern_mono", TestLocatePattern)
54 except rospy.ROSInterruptException:
def test_locate_pattern(self)