WIP: add a launch test that uses the GPU to execute the TSDF process

yak_ros2/CMakeLists.txt

@@ -66,7 +66,7 @@ install(TARGETS ${PROJECT_NAME}_node
 install(DIRECTORY launch demo
         DESTINATION share/${PROJECT_NAME})
 
-if(BUILD_DEMO)
+if(BUILD_DEMO OR BUILD_TESTING)
     find_package(gl_depth_sim REQUIRED)
     find_package(image_transport REQUIRED)
     add_executable(${PROJECT_NAME}_image_simulator
@@ -101,4 +101,9 @@ if(BUILD_DEMO)
       )
     install(TARGETS ${PROJECT_NAME}_image_simulator ${PROJECT_NAME}_tf_broadcaster
             RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+    if(BUILD_TESTING)
+        find_package(launch_testing_ament_cmake REQUIRED)
+        add_launch_test(test/launch_test/yak_ros2_launch_test.py)
+     endif()
 endif()

yak_ros2/package.xml

@@ -27,6 +27,11 @@
   <depend>yak</depend>
   <depend>yak_ros2_msgs</depend>
 
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

yak_ros2/test/launch_test/yak_ros2_launch_test.py

@@ -0,0 +1,79 @@
+import os
+import unittest
+
+from ament_index_python import get_package_share_directory, get_package_prefix
+import shutil
+import tempfile
+import time
+
+import rclpy
+from rclpy.duration import Duration
+from rclpy.time import Time
+
+import launch
+import launch.actions
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import launch_testing
+import launch_testing.actions
+
+from std_srvs.srv import Trigger
+
+import pytest
+
+@pytest.mark.launch_test
+def generate_test_description():
+    demo_launch = launch.actions.IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([get_package_share_directory('yak_ros2'), '/launch/demo.launch.py']))
+
+    return launch.LaunchDescription([
+        demo_launch,
+        launch_testing.actions.ReadyToTest(),
+    ])
+
+
+class TestSetTransforms(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context for the test node
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        rclpy.shutdown()
+
+    def setUp(self):
+        # Create a ROS node for tests
+        self.node = rclpy.create_node('test_mutable_tf_pub')
+        self.gen_mesh_client = self.node.create_client(Trigger, 'generate_mesh_service')
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def test_tf_lookup(self, proc_output):
+        # spin for a few cycles while the TSDF node initializes and fuses some images
+        time.sleep(5.0)
+
+        req_msg = Trigger.Request()
+        future = self.gen_mesh_client.call_async(req_msg)
+
+        while rclpy.ok():
+            rclpy.spin_once(self.node)
+            if future.done():
+                try:
+                    response = future.result()
+                except Exception as e:
+                    self.node.get_logger().info(
+                        'Service call failed %r' % (e,))
+                else:
+                    self.node.get_logger().info(response.success)
+                break
+
+        assert response.success is True
+
+
+@launch_testing.post_shutdown_test()
+class TestYamlAfterShutdown(unittest.TestCase):
+    # TODO: test if the file is in the expected state after tests finish and nodes shut down
+    pass