Use node logging in moveit_ros

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

@@ -43,15 +43,15 @@ namespace moveit
 {
 
 // Function for getting a reference to a logger object kept in a global variable.
-// Use get_logger_mut to set the logger to a node logger.
+// Use set_logger 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 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.
 rclcpp::Logger make_child_logger(const std::string& name);
 
-// Mutable access to global logger for setting to node logger.
-rclcpp::Logger& get_logger_mut();
+// Set the global logger
+void set_logger(const rclcpp::Logger& logger);
 
 }  // namespace moveit

moveit_core/utils/src/logger.cpp

@@ -43,9 +43,20 @@
 namespace moveit
 {
 
+rclcpp::Logger& get_global_logger_ref()
+{
+  // On versions of ROS older than Iron we need to create a node for rosout logging
+  // Remove once Humble is EOL
+#if !RCLCPP_VERSION_GTE(21, 0, 3)
+  static std::shared_ptr<rclcpp::Node> moveit_node = std::make_shared<rclcpp::Node>("moveit");
+#endif
+  static rclcpp::Logger logger = rclcpp::get_logger("moveit");
+  return logger;
+}
+
 const rclcpp::Logger& get_logger()
 {
-  return get_logger_mut();
+  return get_global_logger_ref();
 }
 
 rclcpp::Logger make_child_logger(const std::string& name)
@@ -65,13 +76,12 @@ rclcpp::Logger make_child_logger(const std::string& name)
   }
 #endif
 
-  return get_logger_mut().get_child(name);
+  return get_global_logger_ref().get_child(name);
 }
 
-rclcpp::Logger& get_logger_mut()
+void set_logger(const rclcpp::Logger& logger)
 {
-  static rclcpp::Logger logger = rclcpp::get_logger("moveit");
-  return logger;
+  get_global_logger_ref() = logger;
 }
 
 }  // namespace moveit

moveit_core/utils/test/CMakeLists.txt

@@ -5,21 +5,34 @@ 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"
 )
+
+# 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"
+)

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::get_logger_mut() = node->get_logger();
+  moveit::set_logger(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::get_logger_mut() = node->get_logger();
+  moveit::set_logger(node->get_logger());
 
   // Make a child logger
   const auto child_logger = moveit::make_child_logger("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::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 only the child node!"); });
+  rclcpp::spin(node);
+}

moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h

@@ -43,8 +43,13 @@ namespace collision_detection
 class CollisionPluginLoader : public CollisionPluginCache
 {
 public:
+  CollisionPluginLoader();
+
   /** @brief Fetch plugin name from parameter server and activate the plugin for the given scene */
   void setupScene(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene);
+
+private:
+  rclcpp::Logger logger_;
 };
 
 }  // namespace collision_detection

moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp

@@ -33,18 +33,22 @@
  *********************************************************************/
 
 #include <moveit/collision_plugin_loader/collision_plugin_loader.h>
+#include <moveit/utils/logger.hpp>
 
-static const std::string LOGNAME = "collision_detection";
 namespace collision_detection
 {
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_plugin_loader");
 
+CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::make_child_logger("collision_plugin_loader"))
+{
+}
+
 void CollisionPluginLoader::setupScene(const rclcpp::Node::SharedPtr& node,
                                        const planning_scene::PlanningScenePtr& scene)
 {
   if (!scene)
   {
-    RCLCPP_WARN(LOGGER, "Cannot setup scene, PlanningScenePtr is null.");
+    RCLCPP_WARN(logger_, "Cannot setup scene, PlanningScenePtr is null.");
     return;
   }
 
@@ -73,7 +77,7 @@ void CollisionPluginLoader::setupScene(const rclcpp::Node::SharedPtr& node,
   }
 
   activate(collision_detector_name, scene);
-  RCLCPP_INFO(LOGGER, "Using collision detector: %s", scene->getCollisionDetectorName().c_str());
+  RCLCPP_INFO(logger_, "Using collision detector: %s", scene->getCollisionDetectorName().c_str());
 }
 
 }  // namespace collision_detection

moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp

@@ -41,15 +41,16 @@
 #include <rclcpp/logging.hpp>
 #include <rclcpp/parameter_value.hpp>
 #include <memory>
+#include <moveit/utils/logger.hpp>
 
 namespace constraint_sampler_manager_loader
 {
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.constraint_sampler_manager_loader");
 
 class ConstraintSamplerManagerLoader::Helper
 {
 public:
-  Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) : node_(node)
+  Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm)
+    : node_(node), logger_(moveit::make_child_logger("constraint_sampler_manager_loader"))
   {
     if (node_->has_parameter("constraint_samplers"))
     {
@@ -63,7 +64,7 @@ class ConstraintSamplerManagerLoader::Helper
       }
       catch (pluginlib::PluginlibException& ex)
       {
-        RCLCPP_ERROR(LOGGER, "Exception while creating constraint sampling plugin loader %s", ex.what());
+        RCLCPP_ERROR(logger_, "Exception while creating constraint sampling plugin loader %s", ex.what());
         return;
       }
       boost::char_separator<char> sep(" ");
@@ -75,11 +76,12 @@ class ConstraintSamplerManagerLoader::Helper
           constraint_samplers::ConstraintSamplerAllocatorPtr csa =
               constraint_sampler_plugin_loader_->createUniqueInstance(*beg);
           csm->registerSamplerAllocator(csa);
-          RCLCPP_INFO(LOGGER, "Loaded constraint sampling plugin %s", std::string(*beg).c_str());
+          RCLCPP_INFO(logger_, "Loaded constraint sampling plugin %s", std::string(*beg).c_str());
         }
         catch (pluginlib::PluginlibException& ex)
         {
-          RCLCPP_ERROR(LOGGER, "Exception while planning adapter plugin '%s': %s", std::string(*beg).c_str(), ex.what());
+          RCLCPP_ERROR(logger_, "Exception while planning adapter plugin '%s': %s", std::string(*beg).c_str(),
+                       ex.what());
         }
       }
     }
@@ -89,6 +91,7 @@ class ConstraintSamplerManagerLoader::Helper
   const rclcpp::Node::SharedPtr node_;
   std::unique_ptr<pluginlib::ClassLoader<constraint_samplers::ConstraintSamplerAllocator>>
       constraint_sampler_plugin_loader_;
+  rclcpp::Logger logger_;
 };
 ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader(
     const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm)

moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp

@@ -42,12 +42,14 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
 #include <memory>
+#include <moveit/utils/logger.hpp>
 
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);
 
   auto node = std::make_shared<rclcpp::Node>("list_planning_adapter_plugins");
+  moveit::set_logger(node->get_logger());
 
   std::unique_ptr<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter>> loader;
   try

moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h

@@ -40,6 +40,7 @@
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <kinematics_parameters.hpp>
+#include <moveit/utils/logger.hpp>
 
 namespace kinematics_plugin_loader
 {
@@ -56,7 +57,7 @@ class KinematicsPluginLoader
       well as used to read the SRDF document when needed. */
   KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node,
                          const std::string& robot_description = "robot_description")
-    : node_(node), robot_description_(robot_description)
+    : node_(node), robot_description_(robot_description), logger_(moveit::make_child_logger("kinematics_plugin_loader"))
   {
   }
 
@@ -89,5 +90,6 @@ class KinematicsPluginLoader
 
   std::vector<std::string> groups_;
   std::map<std::string, double> ik_timeout_;
+  rclcpp::Logger logger_;
 };
 }  // namespace kinematics_plugin_loader