From d0d641703a0d005d7d9b541c49e0db117f1b9f0a Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Wed, 27 Nov 2024 21:20:03 +0900
Subject: [PATCH] test(nebula_ros): add smoke test for every sensor

Signed-off-by: Max SCHMELLER <max.schmeller@tier4.jp>
---
 nebula_ros/CMakeLists.txt     | 12 ++++++++
 nebula_ros/package.xml        |  2 ++
 nebula_ros/test/smoke_test.py | 56 +++++++++++++++++++++++++++++++++++
 3 files changed, 70 insertions(+)
 create mode 100644 nebula_ros/test/smoke_test.py

diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt
index 709de2025..6caba4845 100644
--- a/nebula_ros/CMakeLists.txt
+++ b/nebula_ros/CMakeLists.txt
@@ -224,7 +224,19 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
+    find_package(ros_testing REQUIRED)
+
     ament_lint_auto_find_test_dependencies()
+
+    foreach(MODEL VLP16 VLP32 VLS128 Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT32 PandarXT32M ARS548 SRR520 Helios Bpearl)
+        string(TOLOWER ${MODEL}_smoke_test test_name)
+        add_ros_test(
+            test/smoke_test.py
+            TARGET ${test_name}
+            ARGS "launch_file_path:=${PROJECT_SOURCE_DIR}/launch/nebula_launch.py" "sensor_model:=${MODEL}"
+            TIMEOUT "10"
+        )
+    endforeach()
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")
diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml
index 3a97db1f6..334a5f3da 100644
--- a/nebula_ros/package.xml
+++ b/nebula_ros/package.xml
@@ -11,6 +11,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ros_environment</buildtool_depend>
+  <buildtool_depend>ros_testing</buildtool_depend>
 
   <depend>continental_msgs</depend>
   <depend>continental_srvs</depend>
@@ -34,6 +35,7 @@
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/nebula_ros/test/smoke_test.py b/nebula_ros/test/smoke_test.py
new file mode 100644
index 000000000..7e6484723
--- /dev/null
+++ b/nebula_ros/test/smoke_test.py
@@ -0,0 +1,56 @@
+import time
+import unittest
+
+from launch import LaunchContext
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import pytest
+import rclpy
+
+
+def resolve_launch_file(context: LaunchContext, *args, **kwargs):
+    sensor_model = LaunchConfiguration("sensor_model").perform(context)
+    launch_file_path = LaunchConfiguration("launch_file_path").perform(context)
+
+    return [
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(launch_file_path),
+            launch_arguments=[("sensor_model", sensor_model), ("launch_hw", "false")],
+        )
+    ]
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    return LaunchDescription(
+        [OpaqueFunction(function=resolve_launch_file), launch_testing.actions.ReadyToTest()]
+    )
+
+
+class DummyTest(unittest.TestCase):
+    def test_wait_for_node_ready(self):
+        """Waiting for the node is ready."""
+        rclpy.init()
+        test_node = rclpy.create_node("test_node")
+        # Wait until both dummy node "test_node" and real tested node are registered and then kill
+        # both of them, if tested node does not register within `timeout` seconds test will fail
+        start_time = time.time()
+        timeout = 2  # seconds
+        timeout_msg = "Smoke test timeout has been reached ({}s)".format(timeout)
+        print("waiting for Nodes to be ready")
+        while len(test_node.get_node_names()) < 2:
+            assert time.time() - start_time < timeout, timeout_msg
+            time.sleep(0.1)
+        rclpy.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+class TestYourNodeShutdown(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        launch_testing.asserts.assertExitCodes(proc_info)