diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index b61f1252c57..4b0b1ff2ffd 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -49,7 +49,7 @@ 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); +rclcpp::Logger make_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 8f5a593fa66..d89bfe7c76d 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace moveit { @@ -48,8 +49,20 @@ const rclcpp::Logger& get_logger() return get_logger_mut(); } -rclcpp::Logger get_child_logger(const std::string& name) +rclcpp::Logger make_child_logger(const std::string& name) { + // On versions of ROS older than Iron we need to create a node for each child logger + // Remove once Humble is EOL + // References: + // Use parent logger (rcl PR) - https://github.com/ros2/rcl/pull/921 + // Request for backport (rclpy issue) - https://github.com/ros2/rclpy/issues/1131 + // MoveIt PR that added this - https://github.com/ros-planning/moveit2/pull/2445 +#if !RCLCPP_VERSION_GTE(21, 0, 3) + static std::vector> child_nodes; + std::string ns = get_logger().get_name(); + child_nodes.push_back(std::make_shared(name, ns)); +#endif + auto logger = get_logger_mut().get_child(name); return logger; } diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index c4a5ca29fd2..bb52a7e292b 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -1,11 +1,25 @@ +# Build devices under test +add_executable(logger_dut logger_dut.cpp) +target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils) -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}) +add_executable(logger_from_child_dut logger_from_child_dut.cpp) +target_link_libraries(logger_from_child_dut rclcpp::rclcpp moveit_utils) +# Install is needed to run these as launch tests +install( + TARGETS + logger_dut + logger_from_child_dut + DESTINATION lib/${PROJECT_NAME} +) -# Runs logget_test_node and observes ros logging -add_launch_test(logger_launch_test.py) +# Add the launch tests +find_package(launch_testing_ament_cmake) +add_launch_test(rosout_publish_test.py + TARGET test-node_logging + ARGS "dut:=logger_dut" +) +add_launch_test(rosout_publish_test.py + TARGET test-node_logging_from_child + ARGS "dut:=logger_from_child_dut" +) diff --git a/moveit_core/utils/test/logger_test_node.cpp b/moveit_core/utils/test/logger_dut.cpp similarity index 69% rename from moveit_core/utils/test/logger_test_node.cpp rename to moveit_core/utils/test/logger_dut.cpp index a73635e2367..ca1525f1fcd 100644 --- a/moveit_core/utils/test/logger_test_node.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -36,37 +36,19 @@ #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"); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_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 + // Set the moveit logger to be from node 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 + auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), + [&] { RCLCPP_INFO(moveit::get_logger(), "hello from node!"); }); - std::this_thread::sleep_for(std::chrono::seconds(5)); + rclcpp::spin(node); } diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp new file mode 100644 index 00000000000..a444d038b7a --- /dev/null +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -0,0 +1,56 @@ +/********************************************************************* + * 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 + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); + + // Set the moveit logger to be from node + moveit::get_logger_mut() = node->get_logger(); + + // Make a child logger + const auto child_logger = moveit::make_child_logger("child"); + + // publish via a timer + auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), + [&] { RCLCPP_INFO(child_logger, "hello from child node!"); }); + rclcpp::spin(node); +} diff --git a/moveit_core/utils/test/logger_launch_test.py b/moveit_core/utils/test/rosout_publish_test.py similarity index 81% rename from moveit_core/utils/test/logger_launch_test.py rename to moveit_core/utils/test/rosout_publish_test.py index 0817470d90c..4c1ac9d2e38 100644 --- a/moveit_core/utils/test/logger_launch_test.py +++ b/moveit_core/utils/test/rosout_publish_test.py @@ -32,9 +32,6 @@ # 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 @@ -54,17 +51,16 @@ 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", + executable=launch.substitutions.LaunchConfiguration("dut"), 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( [ + launch.actions.DeclareLaunchArgument( + "dut", + description="Executable to launch", + ), dut_process, # Start tests right away - no need to wait for anything launch_testing.actions.ReadyToTest(), @@ -72,11 +68,10 @@ def generate_test_description(): ), {"dut_process": dut_process} -class MakeTestNode(Node): - def __init__(self, name="test_node"): +class MakeRosoutObserverNode(Node): + def __init__(self, name="rosout_observer_node"): super().__init__(name) self.msg_event_object = Event() - self.rosout_msgs = [] def start_subscriber(self): # Create a subscriber @@ -91,22 +86,16 @@ def start_subscriber(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 = MakeRosoutObserverNode() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=5.0) assert msgs_received_flag, "Did not receive msgs !"