Port auto_init_403 test

Author: agutenkunst

fiducial_slam/CMakeLists.txt

@@ -88,6 +88,7 @@ install(FILES fiducials.rviz
 if(BUILD_TESTING)
 
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_nose REQUIRED)
   find_package(ros_testing REQUIRED)
 
   install(DIRECTORY test/test_images
@@ -109,6 +110,13 @@ if(BUILD_TESTING)
   target_link_libraries(auto_init_403_test)
   ament_target_dependencies(auto_init_403_test ${DEPENDENCIES})
   add_ros_test(test/auto_init_403.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # init_map_aruco
+  install(FILES test/map_test.py # I guess there is a better way
+    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test
+    PERMISSIONS OWNER_READ OWNER_EXECUTE
+  )
+  add_ros_test(test/init_map_aruco.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}/test")
 endif()
 # if(CATKIN_ENABLE_TESTING)
 #         find_package(catkin REQUIRED COMPONENTS

fiducial_slam/package.xml

@@ -30,6 +30,9 @@
 
   <test_depend>ros_testing</test_depend>
   <test_depend>ament_cmake_test</test_depend>
+  <test_depend>ament_cmake_node</test_depend>
+  <test_depend>fiducial_msgs</test_depend>
+  <test_depend>aruco_detect</test_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
   <export>
     <build_type>ament_cmake</build_type>

fiducial_slam/test/aruco_transforms.db3

@@ -0,0 +1,92 @@
+import unittest
+
+import launch
+import launch_ros
+import launch_testing
+from pprint import pprint
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from ament_index_python.packages import get_package_share_directory
+
+import pytest
+
+@pytest.mark.rostest
+def generate_test_description():
+    test_node = launch_ros.actions.Node(
+        executable=PathJoinSubstitution([LaunchConfiguration('test_binary_dir'), 'map_test.py']),
+        output='screen',
+        parameters=[
+            {"min_lines":7},
+            {"expected_pose":"0 0 0 0 0 0 1"},
+        ],
+    )
+
+    rosbag_player = launch.actions.ExecuteProcess(
+            cmd=['ros2', 'bag', 'play', '-l', '/home/alex/Projects/PortFiducialsToROS2/ros2_fiducials_ws/src/fiducials/fiducial_slam/test/aruco_transforms.db3'],
+            output='screen'
+    )
+
+    static_tranform_publisher = launch_ros.actions.Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='camera_base',
+        arguments="0.0 0.0 0.0 0.0 0.0 0.0 1.0 base_link raspicam".split()
+    )
+
+
+    # aruco_detect = launch_ros.actions.Node(
+    #     package='aruco_detect',
+    #     namespace='aruco_detect',
+    #     executable='aruco_detect',
+    #     name='aruco_detect',
+    #     parameters=[
+    #         {"image_transport":'raw'},
+    #         {"publish_images":True},
+    #         {"fiducial_len":0.145},
+    #     ],
+    #     #Not sure if the remapping is working properly TODO
+    #     remappings=[
+    #         ('camera/compressed', 'camera/image/compressed'),
+    #         ('fiducial_vertices', '/fiducial_vertices'),
+    #         ('fiducial_transforms', '/fiducial_transforms')
+    #     ]
+    # )
+
+    fiducial_slam = launch_ros.actions.Node(
+        package='fiducial_slam',
+        executable='fiducial_slam_node',
+        name='fiducial_slam_node',
+        parameters=[
+            {"map_frame":'map'},
+            {"odom_frame":""},
+            {"base_frame":"base_link"},
+            {"future_date_transforms", "0.0"},
+            {"publish_6dof_pose","true"},
+            {"map_file":"/dev/null"}
+        ],
+    )
+
+
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='test_binary_dir', # From https://github.com/ros-planning/moveit2/blob/1e1337b46daed0aaa1252b87367c1824f46c9b1a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py#L92
+                                             description='Binary directory of package containing test executables'),
+        # other fixture actions
+        fiducial_slam,
+        test_node,
+        rosbag_player,
+        static_tranform_publisher,
+        #launch_testing.util.KeepAliveProc(), # From https://github.com/ros2/geometry2/blob/a1be44fdbd8c032351ffd3ddff8467d000f4c6d7/test_tf2/test/buffer_client_tester.launch.py#L39
+        launch_testing.actions.ReadyToTest(),
+    ]), locals()
+
+
+class TestTestTermination(unittest.TestCase):
+    def test_termination(self, test_node, proc_output, proc_info):
+        proc_info.assertWaitForShutdown(process=test_node, timeout=(30))
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_no_failed_tests(self, proc_output, proc_info, test_node):
+        # Check that process exits with code -15 code: termination request, sent to the program
+        pprint(proc_info[test_node].returncode)
+        number_of_failed_tests = 0
+        launch_testing.asserts.assertExitCodes(proc_info, [number_of_failed_tests], process=test_node)

fiducial_slam/test/auto_init_403.test

@@ -9,8 +9,8 @@
 import os
 import math
 
-import rospy
-import rostest
+import rclpy
+from rclpy.node import Node
 
 from fiducial_msgs.msg import FiducialMapEntryArray
 from geometry_msgs.msg import PoseWithCovarianceStamped
@@ -35,6 +35,10 @@ def __init__(self, *args):
         self.mapEntries = -1
         self.map = {}
         self.pose = None
+        self.node = Node("test_map")
+
+        self.map_subscription = self.node.create_subscription(FiducialMapEntryArray, "/fiducial_map", self.mapCallback, 1)
+        self.pose_subscription = self.node.create_subscription(PoseWithCovarianceStamped,"/fiducial_pose", self.poseCallback, 1)
 
     def mapCallback(self, msg):
         self.mapEntries = len(msg.fiducials)
@@ -49,20 +53,19 @@ def poseCallback(self, msg):
                      orientation.x, orientation.y, orientation.z, orientation.w)
 
     def test_map(self):
-        rospy.init_node('test_map')
-        minLines = rospy.get_param("~min_lines", 1)
-        expect = rospy.get_param("~expect", "")
-        expectedPose = rospy.get_param("~expected_pose", "")
+        self.node.declare_parameter("min_lines", 1)
+        minLines = self.node.get_parameter('min_lines').value
+
+        expect = self.node.declare_parameter("expect", "")
+        expect = self.node.get_parameter('expect').value
 
-        rospy.Subscriber("/fiducial_map", FiducialMapEntryArray,
-                         self.mapCallback)
-        rospy.Subscriber("/fiducial_pose", PoseWithCovarianceStamped,
-                         self.poseCallback)
+        self.node.declare_parameter("expected_pose", "")
+        expectedPose = self.node.get_parameter('expected_pose').value
 
-        print("test_map");
         t = 0
         while True:
-            if self.mapEntries >= minLines and self.pose != None:
+            rclpy.spin_once(self.node) #Spin the node to receive callbacks
+            if self.mapEntries >= minLines and self.pose != None: #If there are map entries and a pose is set
                 if expectedPose != "":
                     ep = expectedPose.split()
                     for i in range(len(ep)):
@@ -82,11 +85,12 @@ def test_map(self):
                                 self.fail("fiducial %d %s expected %s" % \
                                   (fid, fiducial, ex,))
                 return
-	    time.sleep(0.5)
-
-if __name__ == '__main__':
-    try:
-        rostest.run('rostest', NAME, MapTest, sys.argv)
-    except KeyboardInterrupt:
-        pass
-    print("exiting")
+            #time.sleep(0.5)
+
+def main(args=None):
+    sys.argv = [sys.argv[0]]
+    unittest.main()
+
+if __name__ == "__main__":
+    rclpy.init()
+    main()