tests

moveit_core/package.xml

@@ -72,6 +72,9 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_index_cpp</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>rcl_interfaces</test_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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()

moveit_core/utils/include/moveit/utils/logger.hpp

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <rclcpp/logger.hpp>
+#include <string>
 
 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();
 

moveit_core/utils/src/logger.cpp

@@ -36,6 +36,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/utils/logger.hpp>
+#include <rclcpp/version.h>
 
 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()
 {

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)

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)

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 <chrono>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/executors.hpp>
+#include <moveit/utils/logger.hpp>
+#include <thread>
+#include <rclcpp/version.h>
+
+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));
+}