Use node logging in moveit_ros

doc/MIGRATION_GUIDE.md

@@ -4,36 +4,44 @@
 
 In MoveIt 1 we commonly use named ROS logging macros (i.e. `ROS_INFO_NAMED`) with file-specific logger names defined as `constexpr char LOGNAME[]="logger_name"`.
 ROS 2 provides similar macros (i.e. `RCLCPP_INFO`) that instead of an optional name always require a `rclcpp::Logger` instance for scoping the namespace.
-All source files that use ROS logging should now define a file-specific `static const rclcpp::Logger` named `LOGGER`, located at the top of the file and inside the namespace with the narrowest scope (if there is one).
-This leads to the following basic migration steps:
 
-1. Replace `LOGNAME` with `rclcpp::Logger` instance:
+In ROS 2 logging is tied to the Node object for publishing to /rosout.
+For namespaces there is the option of creating a child logger.
+However because of initaliziation order (child logger has to be created after rclcpp::init is called and from a method on the logger from the node) you can no longer define the logger object as a file level static.
+`moveit_core` provides a `util` library which contains some functions to make this situation more ergonomic.
+```C++
+#include <moveit/utils/logger.hpp>
+```
 
-    <b>Old:</b>
+Wherever the node is created there is the option to set the global logger for moveit using this syntax:
 
-        constexpr char LOGNAME[] = "logger_name";
+```C++
+moveit::setLogger(node->get_logger());
+```
 
-    <b>New:</b>
+Then wherever you call a logging macro you can use the `moveit::getLogger()` function:
+```C++
+RCLCPP_INFO(moveit::getLogger(), "Very important info message");
+```
 
-        static const rclcpp::Logger LOGGER = rclcpp::get_logger("logger_name");
+To have namespaces you need to create a child logger.
+There is a function for that too.
+This creates a child of the global logger.
+You'll find this in the constructor of many of our classes.
 
-2. Replace logging macros:
+```C++
+, logger_(moveit::makeChildLogger("servo"))
+```
 
-    <b>Old:</b>
-
-        ROS_INFO_NAMED(LOGNAME, "Very important info message");
-
-   <b>New:</b>
-
-       RCLCPP_INFO(LOGGER, "Very important info message");
+Once you have a child logger you can use it in logging macros:
+```C++
+RCLCPP_INFO(logger_, "Very important info message");
+```
 
 ### Logger naming convention
 
 Migrating the loggers is a good opportunity to make logger names more consistent.
 In order to create unique and descriptive logger names we encourage the following naming pattern: general `LIBRARY_NAME.SOURCE_FILE_NAME`.
-For instance, the file `joint_model_group.cpp` inside the library `moveit_robot_model` contains the following logger:
-
-    static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.joint_model_group");
 
 For finding the `LIBRARY_NAME` refer to the line `add_library(LIBRARY_NAME ...)` in the library's `CMakeLists.txt`.
 If the source file name is the same or very similar to the library name it is sufficient to only use the source file name.
@@ -42,4 +50,4 @@ If the source file name is the same or very similar to the library name it is su
 
 Some classes declared in header files may contain log messages, for instance to warn about not-implemented virtual functions in abstract classes.
 A logger defined in the header file would not tell us what derived class is missing the implementation, since the source name would be resolved from the header file.
-For this case, the base class should declare a private member variable `static const rclcpp::Logger LOGGER` which is to be defined in the implementing class using the corresponding source file name.
+For this case, the base class should declare a private member variable `rclcpp::Logger logger_;` which is to be defined in the implementing class using the corresponding source file name.

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