test_ar.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2017, TORK (Tokyo Opensource Robotics Kyokai Association)
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of Tokyo Opensource Robotics Kyokai Association
20 # nor the names of its contributors may be used to endorse or promote
21 # products derived from this software without specific prior written
22 # permission.
23 #
24 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 # POSSIBILITY OF SUCH DAMAGE.
36 
37 import numpy
38 import math
39 import unittest
40 
41 from geometry_msgs.msg import Pose
42 import rospy
43 import tf
44 from tf.transformations import quaternion_from_euler
45 
46 class TestArAlvarRos(unittest.TestCase):
47  '''
48  Tests are based on http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-01-30-15-03-19.bag
49  '''
50 
51  def setUp(self):
52  rospy.init_node('test_armarker_ros_detect')
54 
55  def tearDown(self):
56  True # TODO impl something meaningful
57 
58  def _lookup_tf(self, origin, target):
59  '''
60  DEPRECATED: This does not return meaningful values for some reason.
61  @param origin: lookupTransform's 1st argument.
62  @param target: lookupTransform's 2nd argument.
63  @rtype: ([str], [str])
64  @return: Translation and rotation.
65  '''
66  while not rospy.is_shutdown():
67  try:
68  (trans,rot) = self.tflistener.lookupTransform(origin, target, rospy.Time(0))
69  break
71  rospy.logerr(str(e) + ' target={}'.format(target))
72  continue
73  return (trans, rot)
74 
75  def test_markers(self):
76  '''
77  Aiming the real output taken from a bag file.
78  '''
79  # The values in this list are ordered in the marker's number.
80  tf_expected = [[[0.04464459977845401, 0.05341412598745035, 0.26796900627543024], [0.6726999185589797, 0.7391474391806558, -0.01739267319701629, -0.028868280906256056]],
81  [[-0.04805772245624329, 0.039528315926071665, 0.26775882622136327], [0.48207151562664247, 0.8758763282975102, -0.016363763970395625, -0.013414118615296202]],
82  [[0.007233278235745441, 0.015615692018491452, 0.26619586686955365], [0.08546919545682985, 0.9959809257461487, 0.00424040439, -0.02677659572186436]],
83  [[0.06223014382428272, 0.014613815037010106, 0.26226145707174475], [-0.46400320825216246, 0.8850390875261293, 0.032644264656690035, -0.018471282241381157]]]
84  for i in range (0, len(tf_expected)):
85  while not rospy.is_shutdown():
86  try:
87  target_frame = '/ar_marker_{}'.format(i)
88  (trans, rot) = self.tflistener.lookupTransform('/camera', target_frame, rospy.Time(0))
89  break
91  rospy.logerr(str(e) + ' target_frame={}'.format(target_frame))
92  continue
93  # Compare each translation element (x, y, z)
94  for v_ret, v_expected in zip(trans, tf_expected[i][0]):
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)
98  # Compare each orientation element (x, y, z, w)
99  for v_ret, v_expected in zip(rot, tf_expected[i][1]):
100  numpy.testing.assert_approx_equal(
101  v_ret, v_expected, significant=1)
102 
103 if __name__ == '__main__':
104  import rostest
105  rostest.rosrun('ar_track_alvar', 'test_ar_alvar_ros', TestArAlvarRos)
def _lookup_tf(self, origin, target)
Definition: test_ar.py:58
def test_markers(self)
Definition: test_ar.py:75
def tearDown(self)
Definition: test_ar.py:55


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:24