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)); +}