3434# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
3535# POSSIBILITY OF SUCH DAMAGE.
3636
37+ import numpy
3738import math
3839import unittest
3940
@@ -78,7 +79,7 @@ def test_markers(self):
7879 # The values in this list are ordered in the marker's number.
7980 tf_expected = [[[0.04464459977845401 , 0.05341412598745035 , 0.26796900627543024 ], [0.6726999185589797 , 0.7391474391806558 , - 0.01739267319701629 , - 0.028868280906256056 ]],
8081 [[- 0.04805772245624329 , 0.039528315926071665 , 0.26775882622136327 ], [0.48207151562664247 , 0.8758763282975102 , - 0.016363763970395625 , - 0.013414118615296202 ]],
81- [[0.007233278235745441 , 0.015615692018491452 , 0.26619586686955365 ], [0.08546919545682985 , 0.9959809257461487 , - 0.0001615529469785548 , - 0.02677659572186436 ]],
82+ [[0.007233278235745441 , 0.015615692018491452 , 0.26619586686955365 ], [0.08546919545682985 , 0.9959809257461487 , 0.00424040439 , - 0.02677659572186436 ]],
8283 [[0.06223014382428272 , 0.014613815037010106 , 0.26226145707174475 ], [- 0.46400320825216246 , 0.8850390875261293 , 0.032644264656690035 , - 0.018471282241381157 ]]]
8384 for i in range (0 , len (tf_expected )):
8485 while not rospy .is_shutdown ():
@@ -91,10 +92,13 @@ def test_markers(self):
9192 continue
9293 # Compare each translation element (x, y, z)
9394 for v_ret , v_expected in zip (trans , tf_expected [i ][0 ]):
94- self .assertAlmostEqual (v_ret , v_expected , 2 )
95+ # Given that sigfig ignores the leading zero, we only compare the first sigfig.
96+ numpy .testing .assert_approx_equal (
97+ v_ret , v_expected , significant = 1 )
9598 # Compare each orientation element (x, y, z, w)
9699 for v_ret , v_expected in zip (rot , tf_expected [i ][1 ]):
97- self .assertAlmostEqual (v_ret , v_expected , 2 )
100+ numpy .testing .assert_approx_equal (
101+ v_ret , v_expected , significant = 1 )
98102
99103if __name__ == '__main__' :
100104 import rostest
0 commit comments