CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-05 17:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_pose: loose assertion even more

Browse Source
This commit is contained in:
Oleg Kalachev
2019-03-07 01:35:28 +03:00
parent 134fbf5713
commit ec6c5e71bc
1 changed files with 41 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
aruco_pose/test/basic.py
View File

@@ -21,45 +21,45 @@ class TestArucoPose(unittest.TestCase):
self.assertEqual(markers.header.frame_id, 'main_camera_optical')
self.assertEqual(markers.markers[0].id, 2)
self.assertAlmostEqual(markers.markers[0].length, 0.33, places=5)
self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=5)
self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=5)
self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=5)
self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=5)
self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=5)
self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=5)
self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=5)
self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=5)
self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=5)
self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=5)
self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=5)
self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=5)
self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=5)
self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=5)
self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=5)
self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
self.assertEqual(markers.markers[3].id, 3)
self.assertAlmostEqual(markers.markers[3].length, 0.1, places=5)
self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=5)
self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=5)
self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=5)
self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=5)
self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=5)
self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=5)
self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=5)
self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=5)
self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=5)
self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=5)
self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=5)
self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=5)
self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=5)
self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=5)
self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=5)
self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
self.assertEqual(markers.markers[1].id, 1)
self.assertAlmostEqual(markers.markers[1].length, 0.33, places=5)
self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
self.assertEqual(markers.markers[2].id, 4)
self.assertAlmostEqual(markers.markers[2].length, 0.33, places=5)
self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)
def test_visualization(self):
vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
@@ -74,13 +74,13 @@ class TestArucoPose(unittest.TestCase):
def test_map(self):
pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
self.assertEqual(pose.header.frame_id, 'main_camera_optical')
self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=5)
self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=5)
self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=5)
self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=5)
self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=5)
self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=5)
self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=5)
self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
def test_map_image(self):
img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)