Add system test.

.travis.yml

@@ -12,11 +12,11 @@ notifications:
       - [email protected]
 env:
   matrix:
-    - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true
-    - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu USE_DEB=true
+    - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true NOT_TEST_INSTALL=true
+    - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu USE_DEB=true NOT_TEST_INSTALL=true VERBOSE_OUTPUT='true'
     - ROS_DISTRO="indigo" PRERELEASE=true
-    - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true
-    - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu USE_DEB=true
+    - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true NOT_TEST_INSTALL=true VERBOSE_OUTPUT='true'
+    - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu USE_DEB=true NOT_TEST_INSTALL=true
     - ROS_DISTRO="jade"   PRERELEASE=true
 matrix:
   allow_failures:
@@ -26,4 +26,4 @@ install:
   - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
 
 script:
-  - source .ci_config/travis.sh
+  - .ci_config/travis.sh

CMakeLists.txt

@@ -145,3 +145,26 @@ install(DIRECTORY include/${PROJECT_NAME}/
 install(DIRECTORY launch bundles
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
 )
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS roslaunch rostest)
+
+  file(GLOB LAUNCH_FILES launch/*.launch test/*.test)
+  foreach(LAUNCH_FILE ${LAUNCH_FILES})
+    roslaunch_add_file_check(${LAUNCH_FILE})
+  endforeach()
+
+  catkin_download_test_data(
+    ${PROJECT_NAME}_4markers_tork.bag
+    http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-02-08-11-21-14.bag
+    # Workaround the issue http://answers.ros.org/question/253787/accessing-data-downloaded-via-catkin_download_test_data/
+    # by downloading into source folder.
+    #DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/test
+    MD5 627aa0316bbfe4334e06023d7c2b4087
+   )
+  add_rostest(test/marker_arg_config-basic.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
+  add_rostest(test/marker_arg_config-full.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
+  add_rostest(test/marker_param_config-basic.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
+  add_rostest(test/marker_param_config-full.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
+endif()

package.xml

@@ -49,6 +49,9 @@ This package is a ROS wrapper for Alvar, an open source AR tag tracking library.
  <run_depend>visualization_msgs</run_depend>
  <run_depend>dynamic_reconfigure</run_depend>
 
+ <test_depend>rosbag</test_depend>
+ <test_depend>rostest</test_depend>
+
 </package>
 
 

test/marker_abstract.launch.xml

@@ -0,0 +1,33 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <arg name="marker_size" default="2.3" />
+  <arg name="max_new_marker_error" default="0.08" />
+  <arg name="max_track_error" default="0.2" />
+  <arg name="max_frequency" default="100" />
+  <arg name="marker_resolution" default="5" />
+  <arg name="marker_margin" default="2" />
+  <arg name="cam_image_topic" default="/camera/image_raw" />
+  <arg name="cam_info_topic" default="/camera/camera_info" />	
+  <arg name="output_frame" default="/camera" />
+
+  <arg name="config_full" default="true" />
+  <arg name="play_bag" default="true" />
+  <arg name="rosnode_type" default="individualMarkersNoKinect" />
+  <arg name="start_node_withparam" default="true" />
+  <arg name="suffix_testname" default="WITH_PARAM" />
+
+  <include file="$(find ar_track_alvar)/test/test_markerdetect.launch.xml" pass_all_args="true" />
+
+  <!-- If 1 or more ar_pose_marker topic is published, that means the marker detection is functioning. -->
+  <arg name='TESTNAME_MARKER_DETECTION' value='marker_recog_hz_$(arg suffix_testname)' />
+  <arg name='TESTDURATION' value='30' />
+  <test pkg="rostest" type="hztest" test-name="$(arg TESTNAME_MARKER_DETECTION)" name="$(arg TESTNAME_MARKER_DETECTION)" time-limit="$(arg TESTDURATION)" retry="3">
+    <param name="topic" value="ar_pose_marker" />
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="50.00" />
+    <param name="test_duration" value="$(arg TESTDURATION)" />
+    <param name="wait_time" value="$(arg TESTDURATION)" />  <!-- Time for downloading may need to be included in this. -->
+  </test>
+
+  <test pkg="ar_track_alvar" type="test_ar.py" test-name="marker_quarternions_$(arg suffix_testname)" name="marker_quarternions_$(arg suffix_testname)" time-limit="$(arg TESTDURATION)" retry="3" />  
+</launch>

test/marker_arg_config-basic.test

@@ -0,0 +1,9 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <!-- This test uses commandline argument for config, which is deprecated after 0.5.6 (Indigo, Jade), 0.6.3 (Kinetic) -->
+  <arg name="config_full" default="false" />
+  <arg name="play_bag" default="true" />
+  <arg name="start_node_withparam" default="false" />
+  <arg name="suffix_testname" default="WITHARG_CONFIGBASIC" />
+  <include file="$(find ar_track_alvar)/test/marker_abstract.launch.xml" pass_all_args="true" />
+</launch>

test/marker_arg_config-full.test

@@ -0,0 +1,7 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <!-- This test uses commandline argument for config, which is deprecated after 0.5.6 (Indigo, Jade), 0.6.3 (Kinetic) -->
+  <arg name="start_node_withparam" default="false" />
+  <arg name="suffix_testname" default="WITHARG_CONFIGFULL" />
+  <include file="$(find ar_track_alvar)/test/marker_abstract.launch.xml" pass_all_args="true" />
+</launch>

test/marker_param_config-basic.test

@@ -0,0 +1,8 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <!-- This test uses parameter for config. -->
+  <arg name="config_full" default="false" />
+  <arg name="start_node_withparam" default="true" />
+  <arg name="suffix_testname" default="WITHPARAM_CONFIGBASIC" />
+  <include file="$(find ar_track_alvar)/test/marker_abstract.launch.xml" pass_all_args="true" />
+</launch>

test/marker_param_config-full.test

@@ -0,0 +1,7 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <!-- This test uses parameter for config. -->
+  <arg name="start_node_withparam" default="true" />
+  <arg name="suffix_testname" default="WITHPARAM_CONFIGFULL" />
+  <include file="$(find ar_track_alvar)/test/marker_abstract.launch.xml" pass_all_args="true" />
+</launch>

test/test_ar.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2017, TORK (Tokyo Opensource Robotics Kyokai Association) 
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Tokyo Opensource Robotics Kyokai Association
+#    nor the names of its contributors may be used to endorse or promote 
+#    products derived from this software without specific prior written 
+#    permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import math
+import unittest
+
+from geometry_msgs.msg import Pose
+import rospy
+import tf
+from tf.transformations import quaternion_from_euler
+
+class TestArAlvarRos(unittest.TestCase):
+    '''
+    Tests are based on http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-01-30-15-03-19.bag
+    '''
+
+    def setUp(self):
+        rospy.init_node('test_armarker_ros_detect')
+        self.tflistener = tf.TransformListener()
+
+    def tearDown(self):
+        True  # TODO impl something meaningful
+
+    def _lookup_tf(self, origin, target):
+        '''
+        DEPRECATED: This does not return meaningful values for some reason.
+        @param origin: lookupTransform's 1st argument.
+        @param target: lookupTransform's 2nd argument.
+        @rtype: ([str], [str])
+        @return: Translation and rotation.
+        '''
+        while not rospy.is_shutdown():
+            try:
+                (trans,rot) = self.tflistener.lookupTransform(origin, target, rospy.Time(0))
+                break
+            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+                rospy.logerr(str(e) + ' target={}'.format(target))
+                continue
+        return (trans, rot)
+
+    def test_markers(self):
+        '''
+        Aiming the real output taken from a bag file.
+        '''
+        # The values in this list are ordered in the marker's number. 
+        tf_expected = [[[0.04464459977845401, 0.05341412598745035, 0.26796900627543024], [0.6726999185589797, 0.7391474391806558, -0.01739267319701629, -0.028868280906256056]],
+                       [[-0.04805772245624329, 0.039528315926071665, 0.26775882622136327], [0.48207151562664247, 0.8758763282975102, -0.016363763970395625, -0.013414118615296202]],
+                       [[0.007233278235745441, 0.015615692018491452, 0.26619586686955365], [0.08546919545682985, 0.9959809257461487, -0.0001615529469785548, -0.02677659572186436]],
+                       [[0.06223014382428272, 0.014613815037010106, 0.26226145707174475], [-0.46400320825216246, 0.8850390875261293, 0.032644264656690035, -0.018471282241381157]]]
+        for i in range (0, len(tf_expected)):
+            while not rospy.is_shutdown():
+                try:
+                    target_frame = '/ar_marker_{}'.format(i)
+                    (trans, rot) = self.tflistener.lookupTransform('/camera', target_frame, rospy.Time(0))
+                    break
+                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+                    rospy.logerr(str(e) + ' target_frame={}'.format(target_frame))
+                    continue
+            # Compare each translation element (x, y, z) 
+            for v_ret, v_expected in zip(trans, tf_expected[i][0]):
+                self.assertAlmostEqual(v_ret, v_expected, 2)
+            # Compare each orientation element (x, y, z, w) 
+            for v_ret, v_expected in zip(rot, tf_expected[i][1]):
+                self.assertAlmostEqual(v_ret, v_expected, 2)
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun('ar_track_alvar', 'test_ar_alvar_ros', TestArAlvarRos)

test/test_markerdetect.launch.xml

@@ -0,0 +1,49 @@
+<launch>
+  <!-- This test assumes 4markers_tork.bag is being played. -->
+  <arg name="cam_image_topic" default="/camera/image_raw" />
+  <arg name="cam_info_topic" default="/camera/camera_info" />	
+  <arg name="marker_margin" default="2" />
+  <arg name="marker_resolution" default="5" />
+  <arg name="marker_size" default="2.3" />
+  <arg name="max_new_marker_error" default="0.08" />
+  <arg name="max_frequency" default="100" />
+  <arg name="max_track_error" default="0.2" />
+  <arg name="output_frame" default="/camera" />
+
+  <arg name="config_full" default="true" />
+  <arg name="rosnode_type" default="individualMarkers" />
+  <arg name="rosnode_runtime_name" default="ar_rosnode_$(arg rosnode_type)" />
+  <arg name="play_bag" default="true" />
+  <arg name="start_node_withparam" default="false" />
+  <arg name="suffix_testname" default="WITH_PARAM" />  <!-- Not used in this launch but still defined in order only to allow client launch files to use a convenient 'pass_all_args' arg. -->
+
+  <group if="$(arg play_bag)">
+    <param name="use_sim_time" value="true" />
+    <node pkg="rosbag" type="play" name="playing_bag" args="--clock -l $(find ar_track_alvar)/test/ar_track_alvar_4markers_tork_2017-02-08-11-21-14.bag" />
+  </group>
+
+  <group if="$(arg start_node_withparam)">
+    <node pkg="ar_track_alvar" type="$(arg rosnode_type)" name="$(arg rosnode_runtime_name)" respawn="false" output="screen" args="">
+      <param name="marker_size"           type="double" value="$(arg marker_size)" />
+      <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
+      <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
+      <param name="output_frame"          type="string" value="$(arg output_frame)" />
+      <!-- Only when config_full is NOT set. -->
+      <param name="max_frequency"         type="double" value="$(arg max_frequency)" if="$(arg config_full)" />
+      <param name="marker_resolution"     type="double" value="$(arg marker_resolution)" if="$(arg config_full)" />
+      <param name="marker_margin"         type="double" value="$(arg marker_margin)" if="$(arg config_full)" />
+
+      <remap from="camera_image"  to="$(arg cam_image_topic)" />
+      <remap from="camera_info"   to="$(arg cam_info_topic)" />
+    </node>
+  </group>
+  <group unless="$(arg start_node_withparam)">
+    <group if="$(arg config_full)">
+      <node pkg="ar_track_alvar" type="$(arg rosnode_type)" name="$(arg rosnode_runtime_name)" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg max_frequency) $(arg marker_resolution) $(arg marker_margin)" />
+    </group>
+    <group unless="$(arg config_full)">
+      <node pkg="ar_track_alvar" type="$(arg rosnode_type)" name="$(arg rosnode_runtime_name)" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
+    </group>
+  </group>
+
+</launch>