test_ar.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2017, TORK (Tokyo Opensource Robotics Kyokai Association) 
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of Tokyo Opensource Robotics Kyokai Association
00020 #    nor the names of its contributors may be used to endorse or promote 
00021 #    products derived from this software without specific prior written 
00022 #    permission.
00023 #
00024 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 
00037 import math
00038 import unittest
00039 
00040 from geometry_msgs.msg import Pose
00041 import rospy
00042 import tf
00043 from tf.transformations import quaternion_from_euler
00044 
00045 class TestArAlvarRos(unittest.TestCase):
00046     '''
00047     Tests are based on http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-01-30-15-03-19.bag
00048     '''
00049 
00050     def setUp(self):
00051         rospy.init_node('test_armarker_ros_detect')
00052         self.tflistener = tf.TransformListener()
00053 
00054     def tearDown(self):
00055         True  # TODO impl something meaningful
00056 
00057     def _lookup_tf(self, origin, target):
00058         '''
00059         DEPRECATED: This does not return meaningful values for some reason.
00060         @param origin: lookupTransform's 1st argument.
00061         @param target: lookupTransform's 2nd argument.
00062         @rtype: ([str], [str])
00063         @return: Translation and rotation.
00064         '''
00065         while not rospy.is_shutdown():
00066             try:
00067                 (trans,rot) = self.tflistener.lookupTransform(origin, target, rospy.Time(0))
00068                 break
00069             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
00070                 rospy.logerr(str(e) + ' target={}'.format(target))
00071                 continue
00072         return (trans, rot)
00073         
00074     def test_markers(self):
00075         '''
00076         Aiming the real output taken from a bag file.
00077         '''
00078         # The values in this list are ordered in the marker's number. 
00079         tf_expected = [[[0.04464459977845401, 0.05341412598745035, 0.26796900627543024], [0.6726999185589797, 0.7391474391806558, -0.01739267319701629, -0.028868280906256056]],
00080                        [[-0.04805772245624329, 0.039528315926071665, 0.26775882622136327], [0.48207151562664247, 0.8758763282975102, -0.016363763970395625, -0.013414118615296202]],
00081                        [[0.007233278235745441, 0.015615692018491452, 0.26619586686955365], [0.08546919545682985, 0.9959809257461487, -0.0001615529469785548, -0.02677659572186436]],
00082                        [[0.06223014382428272, 0.014613815037010106, 0.26226145707174475], [-0.46400320825216246, 0.8850390875261293, 0.032644264656690035, -0.018471282241381157]]]
00083         for i in range (0, len(tf_expected)):
00084             while not rospy.is_shutdown():
00085                 try:
00086                     target_frame = '/ar_marker_{}'.format(i)
00087                     (trans, rot) = self.tflistener.lookupTransform('/camera', target_frame, rospy.Time(0))
00088                     break
00089                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
00090                     rospy.logerr(str(e) + ' target_frame={}'.format(target_frame))
00091                     continue
00092             # Compare each translation element (x, y, z) 
00093             for v_ret, v_expected in zip(trans, tf_expected[i][0]):
00094                 self.assertAlmostEqual(v_ret, v_expected, 2)
00095             # Compare each orientation element (x, y, z, w) 
00096             for v_ret, v_expected in zip(rot, tf_expected[i][1]):
00097                 self.assertAlmostEqual(v_ret, v_expected, 2)
00098 
00099 if __name__ == '__main__':
00100     import rostest
00101     rostest.rosrun('ar_track_alvar', 'test_ar_alvar_ros', TestArAlvarRos)


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02