diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt
index 6f51b5210d853..955234f95ff18 100644
--- a/localization/ndt_scan_matcher/CMakeLists.txt
+++ b/localization/ndt_scan_matcher/CMakeLists.txt
@@ -41,6 +41,11 @@ link_directories(${PCL_LIBRARY_DIRS})
target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES})
if(BUILD_TESTING)
+ add_ros_test(
+ test/test_ndt_scan_matcher_launch.py
+ TIMEOUT "30"
+ )
+
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_tpe
test/test_tpe.cpp
diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml
index 92c690a708492..986179b9e02e2 100644
--- a/localization/ndt_scan_matcher/package.xml
+++ b/localization/ndt_scan_matcher/package.xml
@@ -38,6 +38,7 @@
ament_cmake_cppcheck
ament_lint_auto
+ ros_testing
ament_cmake
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 9c94467dc94c0..7512ae2e77a21 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -55,12 +55,6 @@ MapUpdateModule::MapUpdateModule(
pcd_loader_client_ =
node->create_client("pcd_loader_service");
- while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
- RCLCPP_INFO(
- logger_,
- "Waiting for pcd loader service. Check if the enable_differential_load in "
- "pointcloud_map_loader is set `true`.");
- }
double map_update_dt = 1.0;
auto period_ns = std::chrono::duration_cast(
@@ -117,7 +111,14 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
request->area.radius = static_cast(dynamic_map_loading_map_radius_);
request->cached_ids = ndt_ptr_->getCurrentMapIDs();
- // // send a request to map_loader
+ while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+ RCLCPP_INFO(
+ logger_,
+ "Waiting for pcd loader service. Check if the enable_differential_load in "
+ "pointcloud_map_loader is set `true`.");
+ }
+
+ // send a request to map_loader
auto result{pcd_loader_client_->async_send_request(
request,
[](rclcpp::Client::SharedFuture) {})};
diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py
new file mode 100644
index 0000000000000..2dc4958c4704f
--- /dev/null
+++ b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import unittest
+
+from ament_index_python import get_package_share_directory
+import launch
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.logging import get_logger
+import launch_testing
+import pytest
+import rclpy
+from std_srvs.srv import SetBool
+
+logger = get_logger(__name__)
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ test_ndt_scan_matcher_launch_file = os.path.join(
+ get_package_share_directory("ndt_scan_matcher"),
+ "launch",
+ "ndt_scan_matcher.launch.xml",
+ )
+ ndt_scan_matcher = IncludeLaunchDescription(
+ AnyLaunchDescriptionSource(test_ndt_scan_matcher_launch_file),
+ )
+
+ return launch.LaunchDescription(
+ [
+ ndt_scan_matcher,
+ # Start tests right away - no need to wait for anything
+ launch_testing.actions.ReadyToTest(),
+ ]
+ )
+
+
+class TestNDTScanMatcher(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.test_node = rclpy.create_node("test_node")
+
+ def tearDown(self):
+ self.test_node.destroy_node()
+
+ @staticmethod
+ def print_message(stat):
+ logger.debug("===========================")
+ logger.debug(stat)
+
+ def test_launch(self):
+ # Trigger ndt_scan_matcher to activate the node
+ cli_trigger = self.test_node.create_client(SetBool, "/trigger_node")
+ while not cli_trigger.wait_for_service(timeout_sec=1.0):
+ continue
+
+ request = SetBool.Request()
+ request.data = True
+ future = cli_trigger.call_async(request)
+ rclpy.spin_until_future_complete(self.test_node, future)
+
+ if future.result() is not None:
+ self.test_node.get_logger().info("Result of bool service: %s" % future.result().message)
+ self.assertTrue(future.result().success, "ndt_scan_matcher is not activated")
+ else:
+ self.test_node.get_logger().error(
+ "Exception while calling service: %r" % future.exception()
+ )
+ raise self.failureException("service trigger failed")
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+ def test_exit_code(self, proc_info):
+ # Check that process exits with code 0: no error
+ launch_testing.asserts.assertExitCodes(proc_info)