diff --git a/moveit_core/package.xml b/moveit_core/package.xml
index 1cbe79cfb7d..85c2c2633aa 100644
--- a/moveit_core/package.xml
+++ b/moveit_core/package.xml
@@ -72,6 +72,9 @@
ament_cmake_gtest
ament_cmake_gmock
ament_index_cpp
+ launch_testing_ament_cmake
+ rclpy
+ rcl_interfaces
ament_lint_auto
ament_lint_common
diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt
index 5fa121ad7da..a206662a197 100644
--- a/moveit_core/utils/CMakeLists.txt
+++ b/moveit_core/utils/CMakeLists.txt
@@ -32,3 +32,7 @@ ament_target_dependencies(moveit_test_utils
rclcpp
)
set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+if(BUILD_TESTING)
+ add_subdirectory(test)
+endif()
diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp
index 12965044856..b61f1252c57 100644
--- a/moveit_core/utils/include/moveit/utils/logger.hpp
+++ b/moveit_core/utils/include/moveit/utils/logger.hpp
@@ -37,6 +37,7 @@
#pragma once
#include
+#include
namespace moveit
{
@@ -45,6 +46,11 @@ namespace moveit
// Use get_logger_mut to set the logger to a node logger
const rclcpp::Logger& get_logger();
+// Function for getting a child logger. In Humble this also creates a node
+// Do no use this in place as it will create a new logger each time
+// instead store it in the state of your class or method.
+rclcpp::Logger get_child_logger(const std::string& name);
+
// Mutable access to global logger for setting to node logger
rclcpp::Logger& get_logger_mut();
diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp
index 0dd2d3ee6ef..8f5a593fa66 100644
--- a/moveit_core/utils/src/logger.cpp
+++ b/moveit_core/utils/src/logger.cpp
@@ -36,6 +36,7 @@
#include
#include
+#include
namespace moveit
{
@@ -47,6 +48,12 @@ const rclcpp::Logger& get_logger()
return get_logger_mut();
}
+rclcpp::Logger get_child_logger(const std::string& name)
+{
+ auto logger = get_logger_mut().get_child(name);
+ return logger;
+}
+
// Mutable access to global logger for setting to node logger
rclcpp::Logger& get_logger_mut()
{
diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt
new file mode 100644
index 00000000000..c4a5ca29fd2
--- /dev/null
+++ b/moveit_core/utils/test/CMakeLists.txt
@@ -0,0 +1,11 @@
+
+find_package(launch_testing_ament_cmake)
+
+add_executable(logger_test_node logger_test_node.cpp)
+target_link_libraries(logger_test_node rclcpp::rclcpp moveit_utils)
+install(TARGETS logger_test_node
+ DESTINATION lib/${PROJECT_NAME})
+
+
+# Runs logget_test_node and observes ros logging
+add_launch_test(logger_launch_test.py)
diff --git a/moveit_core/utils/test/logger_launch_test.py b/moveit_core/utils/test/logger_launch_test.py
new file mode 100644
index 00000000000..0817470d90c
--- /dev/null
+++ b/moveit_core/utils/test/logger_launch_test.py
@@ -0,0 +1,122 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2023, PickNik Robotics Inc.
+# 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 Willow Garage 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.
+
+# Author: Tyler Weaver
+
+# Based on https://github.com/ros2/launch/blob/master/launch_testing/test/launch_testing/examples/good_proc_launch_test.py
+
+import os
+import unittest
+from threading import Event
+from threading import Thread
+
+import launch
+import launch_ros
+import launch_testing
+import rclpy
+from rclpy.node import Node
+from rcl_interfaces.msg import Log
+
+import pytest
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ # dut: device under test
+ dut_process = launch_ros.actions.Node(
+ package="moveit_core",
+ executable="logger_test_node",
+ name="logger_test_node",
+ output="screen",
+ )
+
+ # This is necessary to get unbuffered output from the process under test
+ proc_env = os.environ.copy()
+ proc_env["PYTHONUNBUFFERED"] = "1"
+
+ return launch.LaunchDescription(
+ [
+ dut_process,
+ # Start tests right away - no need to wait for anything
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ), {"dut_process": dut_process}
+
+
+class MakeTestNode(Node):
+ def __init__(self, name="test_node"):
+ super().__init__(name)
+ self.msg_event_object = Event()
+ self.rosout_msgs = []
+
+ def start_subscriber(self):
+ # Create a subscriber
+ self.subscription = self.create_subscription(
+ Log, "rosout", self.subscriber_callback, 10
+ )
+
+ # Add a spin thread
+ self.ros_spin_thread = Thread(
+ target=lambda node: rclpy.spin(node), args=(self,)
+ )
+ self.ros_spin_thread.start()
+
+ def subscriber_callback(self, data):
+ self.rosout_msgs.append(data)
+ self.msg_event_object.set()
+
+
+# These tests will run concurrently with the dut process. After all these tests are done,
+# the launch system will shut down the processes that it started up
+class TestFixture(unittest.TestCase):
+ def test_stdout_print(self, proc_output):
+ proc_output.assertWaitFor("Before", timeout=10, stream="stdout")
+ proc_output.assertWaitFor("After", timeout=10, stream="stdout")
+ proc_output.assertWaitFor("Child", timeout=10, stream="stdout")
+
+ def test_rosout_msgs_published(self, proc_output):
+ rclpy.init()
+ try:
+ node = MakeTestNode("test_node")
+ node.start_subscriber()
+ msgs_received_flag = node.msg_event_object.wait(timeout=5.0)
+ assert msgs_received_flag, "Did not receive msgs !"
+ finally:
+ rclpy.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+ def test_exit_code(self, proc_info):
+ # Check that all processes in the launch (in this case, there's just one) exit
+ # with code 0
+ launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/moveit_core/utils/test/logger_test_node.cpp b/moveit_core/utils/test/logger_test_node.cpp
new file mode 100644
index 00000000000..a73635e2367
--- /dev/null
+++ b/moveit_core/utils/test/logger_test_node.cpp
@@ -0,0 +1,72 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2023, PickNik Robotics Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+/* Author: Tyler Weaver */
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("logger_test_node");
+
+ // Not a node logger, should print but should not be in file or rosout output
+ RCLCPP_INFO(moveit::get_logger(), "Before");
+
+ // Set the node logger
+ moveit::get_logger_mut() = node->get_logger();
+
+ // A node logger, should be in the file output and rosout
+ RCLCPP_INFO(moveit::get_logger(), "After");
+
+ // A child logger, should also be in the file output and rosout
+ const auto child_logger = moveit::get_child_logger("child");
+ RCLCPP_INFO(child_logger, "Child");
+
+ // Spin the node to publish to /rosout
+#if RCLCPP_VERSION_GTE(22, 1, 0) // https://github.com/ros2/rclcpp/commit/a17d26b20ac41cc9d5bf3583de8475bb7c18bb1e
+ rclcpp::spin_all(node, std::chrono::seconds(1));
+#else // Humble or Iron
+ rclcpp::executors::SingleThreadedExecutor exec;
+ exec.add_node(node);
+ exec.spin_all(std::chrono::seconds(1));
+#endif
+
+ std::this_thread::sleep_for(std::chrono::seconds(5));
+}