Use node logging in moveit_ros

Signed-off-by: Tyler Weaver <[email protected]>

moveit_core/utils/CMakeLists.txt

@@ -8,7 +8,7 @@ target_include_directories(moveit_utils PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/moveit_core>
 )
-ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp)
+ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp rsl fmt)
 set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -30,6 +30,8 @@ ament_target_dependencies(moveit_test_utils
   urdfdom
   urdfdom_headers
   rclcpp
+  rsl
+  fmt
 )
 set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 

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

@@ -42,16 +42,30 @@
 namespace moveit
 {
 
-// Function for getting a reference to a logger object kept in a global variable.
-// Use getLoggerMut to set the logger to a node logger.
+// Get reference to global logger.
+// Use `setLogger` to set the logger to a node logger.
+// This can be used in place in macro logger lines. For example:
+// ```c++
+// RCLCPP_WARN(moveit::getLogger(), "Something went wrong");
+// ```
 const rclcpp::Logger& getLogger();
 
-// Function for getting a child logger. In Humble this also creates a node.
-// Do not use this in place as it will create a new logger each time,
-// instead store it in the state of your class or method.
+// Make a child logger.
+// This should only be used after a node logger has been set through the
+// `setLogger` function as it calls `get_child` on the result of `getLogger`.
+//
+// On Humble the resulting child logger does not log to /rosout.
+// We had the option of logging to /rosout or keeping namespaces (on Humble)
+// and we chose namespaces as /rosout was not working before this change.
+//
+// On Iron and Rolling, child loggers log to /rosout.
 rclcpp::Logger makeChildLogger(const std::string& name);
 
-// Mutable access to global logger for setting to node logger.
-rclcpp::Logger& getLoggerMut();
+// Set the global logger.
+// Use:
+// ```c++
+// moveit::setLogger(node->get_logger());
+// ```
+void setLogger(const rclcpp::Logger& logger);
 
 }  // namespace moveit

moveit_core/utils/src/logger.cpp

@@ -36,42 +36,68 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/utils/logger.hpp>
-#include <rclcpp/version.h>
-#include <unordered_map>
 #include <string>
+#include <rsl/random.hpp>
+#include <fmt/format.h>
 
 namespace moveit
 {
 
+// This is the function that stores the global logger used by moveit.
+// As it returns a reference to the static logger it can be changed through the
+// `setLogger` function.
+//
+// You can get a const reference to the logger through the public function
+// `getLogger`.
+//
+// As there is no way to know if `getLogger` is called before `setLogger`
+// we try to initialize a node that will setup /rosout logging with this logger.
+// If that fails (likely due to rclcpp::init not haven't been called) we set the
+// global logger to one that is not associated with a node. When a logger not
+// associated with a node is used the logs only go to the console and files,
+// they do not go to /rosout. This is because publishing is done by a node.
+//
+// The node and logger created here is not intended to be used. But if it is,
+// we append a random number to the name of the node name and logger to make it
+// unique. This helps to prevent problems that arise from when multiple
+// nodes use the same name.
+rclcpp::Logger& getGlobalLoggerRef()
+{
+  static rclcpp::Logger logger = [&] {
+    // A random number is appended to the name used for the node to make it unique.
+    // This unique node and logger name is only used if a user does not set a logger
+    // through the `setLogger` method to their node's logger.
+    auto name = fmt::format("moveit_{}", rsl::rng()());
+    try
+    {
+      static auto moveit_node = std::make_shared<rclcpp::Node>(name);
+      return moveit_node->get_logger();
+    }
+    catch (const std::exception& ex)
+    {
+      // rclcpp::init was not called so rcl context is null, return non-node logger
+      auto logger = rclcpp::get_logger(name);
+      RCLCPP_WARN_STREAM(logger, "exception thrown while creating node for logging: " << ex.what());
+      RCLCPP_WARN(logger, "if rclcpp::init was not called, messages from this logger may be missing from /rosout");
+      return logger;
+    }
+  }();
+  return logger;
+}
+
 const rclcpp::Logger& getLogger()
 {
-  return getLoggerMut();
+  return getGlobalLoggerRef();
 }
 
 rclcpp::Logger makeChildLogger(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::unordered_map<std::string, std::shared_ptr<rclcpp::Node>> child_nodes;
-  if (child_nodes.find(name) == child_nodes.end())
-  {
-    std::string ns = getLogger().get_name();
-    child_nodes[name] = std::make_shared<rclcpp::Node>(name, ns);
-  }
-#endif
-
-  return getLoggerMut().get_child(name);
+  return getGlobalLoggerRef().get_child(name);
 }
 
-rclcpp::Logger& getLoggerMut()
+void setLogger(const rclcpp::Logger& logger)
 {
-  static rclcpp::Logger logger = rclcpp::get_logger("moveit");
-  return logger;
+  getGlobalLoggerRef() = logger;
 }
 
 }  // namespace moveit

moveit_core/utils/test/CMakeLists.txt

@@ -5,21 +5,38 @@ target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils)
 add_executable(logger_from_child_dut logger_from_child_dut.cpp)
 target_link_libraries(logger_from_child_dut rclcpp::rclcpp moveit_utils)
 
+add_executable(logger_from_only_child_dut logger_from_only_child_dut.cpp)
+target_link_libraries(logger_from_only_child_dut rclcpp::rclcpp moveit_utils)
+
 # Install is needed to run these as launch tests
 install(
   TARGETS
     logger_dut
     logger_from_child_dut
+    logger_from_only_child_dut
   DESTINATION lib/${PROJECT_NAME}
 )
 
-# Add the launch tests
 find_package(launch_testing_ament_cmake)
+
+# Test node logger to rosout
 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"
-)
+
+# These tests do not work on Humble as /rosout logging from child loggers
+# does not work.
+if(NOT $ENV{ROS_DISTRO} STREQUAL "humble")
+  # Test init node logging then log from child logger to rosout
+  add_launch_test(rosout_publish_test.py
+      TARGET test-node_logging_from_child
+      ARGS "dut:=logger_from_child_dut"
+  )
+
+  # Test init only creating child logger and logging goes to rosout
+  add_launch_test(rosout_publish_test.py
+      TARGET test-logger_from_only_child_dut
+      ARGS "dut:=logger_from_only_child_dut"
+  )
+endif()

moveit_core/utils/test/logger_dut.cpp

@@ -44,7 +44,7 @@ int main(int argc, char** argv)
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node");
 
   // Set the moveit logger to be from node
-  moveit::getLoggerMut() = node->get_logger();
+  moveit::setLogger(node->get_logger());
 
   // A node logger, should be in the file output and rosout
   auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100),

moveit_core/utils/test/logger_from_child_dut.cpp

@@ -44,7 +44,7 @@ int main(int argc, char** argv)
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node");
 
   // Set the moveit logger to be from node
-  moveit::getLoggerMut() = node->get_logger();
+  moveit::setLogger(node->get_logger());
 
   // Make a child logger
   const auto child_logger = moveit::makeChildLogger("child");

moveit_core/utils/test/logger_from_only_child_dut.cpp

@@ -0,0 +1,53 @@
+/*********************************************************************
+ * 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 <moveit/utils/logger.hpp>
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node");
+
+  // Make a child logger
+  const auto child_logger = moveit::makeChildLogger("child");
+
+  // publish via a timer
+  auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100),
+                                            [&] { RCLCPP_INFO(child_logger, "hello from only the child node!"); });
+  rclcpp::spin(node);
+}