Node logger through singleton

moveit_core/utils/CMakeLists.txt

@@ -2,12 +2,13 @@ add_library(moveit_utils SHARED
   src/lexical_casts.cpp
   src/message_checks.cpp
   src/rclcpp_utils.cpp
+  src/logger.cpp
 )
 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)
+ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp)
 set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -28,5 +29,6 @@ ament_target_dependencies(moveit_test_utils
   srdfdom
   urdfdom
   urdfdom_headers
+  rclcpp
 )
 set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

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

@@ -0,0 +1,51 @@
+/*********************************************************************
+ * 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 */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace moveit
+{
+
+// Function for getting a reference to a logger object kept on the stack
+// Use get_logger_mut to set the logger to a node logger
+const rclcpp::Logger& get_logger();
+
+// Mutable access to global logger for setting to node logger
+rclcpp::Logger& get_logger_mut();
+
+}  // namespace moveit

moveit_core/utils/src/logger.cpp

@@ -0,0 +1,57 @@
+/*********************************************************************
+ * 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 <rclcpp/rclcpp.hpp>
+#include <moveit/utils/logger.hpp>
+
+namespace moveit
+{
+
+// Function for getting a reference to a logger object kept on the stack
+// Use get_logger_mut to set the logger to a node logger
+const rclcpp::Logger& get_logger()
+{
+  return get_logger_mut();
+}
+
+// Mutable access to global logger for setting to node logger
+rclcpp::Logger& get_logger_mut()
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("moveit");
+  return logger;
+}
+
+}  // namespace moveit

moveit_ros/warehouse/src/broadcast.cpp

@@ -37,6 +37,7 @@
 #include <moveit/warehouse/planning_scene_storage.h>
 #include <moveit/warehouse/constraints_storage.h>
 #include <moveit/warehouse/state_storage.h>
+#include <moveit/utils/logger.hpp>
 #include <boost/program_options/cmdline.hpp>
 #include <boost/program_options/options_description.hpp>
 #include <boost/program_options/parsers.hpp>
@@ -65,6 +66,7 @@ static const std::string CONSTRAINTS_TOPIC = "constraints";
 static const std::string STATES_TOPIC = "robot_states";
 
 using namespace std::chrono_literals;
+using moveit::get_logger;
 
 int main(int argc, char** argv)
 {
@@ -73,7 +75,7 @@ int main(int argc, char** argv)
   node_options.allow_undeclared_parameters(true);
   node_options.automatically_declare_parameters_from_overrides(true);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("publish_warehouse_data", node_options);
-  const auto logger = node->get_logger();
+  moveit::get_logger_mut() = node->get_logger();
 
   // time to wait in between publishing messages
   double delay = 0.001;
@@ -140,7 +142,7 @@ int main(int argc, char** argv)
       moveit_warehouse::PlanningSceneWithMetadata pswm;
       if (pss.getPlanningScene(pswm, scene_name))
       {
-        RCLCPP_INFO(logger, "Publishing scene '%s'",
+        RCLCPP_INFO(get_logger(), "Publishing scene '%s'",
                     pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str());
         pub_scene->publish(static_cast<const moveit_msgs::msg::PlanningScene&>(*pswm));
         executor.spin_once(0ns);
@@ -153,14 +155,14 @@ int main(int argc, char** argv)
           std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
           std::vector<std::string> query_names;
           pss.getPlanningQueries(planning_queries, query_names, pswm->name);
-          RCLCPP_INFO(logger, "There are %d planning queries associated to the scene",
+          RCLCPP_INFO(get_logger(), "There are %d planning queries associated to the scene",
                       static_cast<int>(planning_queries.size()));
           rclcpp::sleep_for(500ms);
           for (std::size_t i = 0; i < planning_queries.size(); ++i)
           {
             if (req)
             {
-              RCLCPP_INFO(logger, "Publishing query '%s'", query_names[i].c_str());
+              RCLCPP_INFO(get_logger(), "Publishing query '%s'", query_names[i].c_str());
               pub_req->publish(static_cast<const moveit_msgs::msg::MotionPlanRequest&>(*planning_queries[i]));
               executor.spin_once(0ns);
             }
@@ -194,7 +196,7 @@ int main(int argc, char** argv)
       moveit_warehouse::ConstraintsWithMetadata cwm;
       if (cs.getConstraints(cwm, cname))
       {
-        RCLCPP_INFO(logger, "Publishing constraints '%s'",
+        RCLCPP_INFO(get_logger(), "Publishing constraints '%s'",
                     cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str());
         pub_constr->publish(static_cast<const moveit_msgs::msg::Constraints&>(*cwm));
         executor.spin_once(0ns);
@@ -216,7 +218,7 @@ int main(int argc, char** argv)
       moveit_warehouse::RobotStateWithMetadata rswm;
       if (rs.getRobotState(rswm, rname))
       {
-        RCLCPP_INFO(logger, "Publishing state '%s'",
+        RCLCPP_INFO(get_logger(), "Publishing state '%s'",
                     rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
         pub_state->publish(static_cast<const moveit_msgs::msg::RobotState&>(*rswm));
         executor.spin_once(0ns);
@@ -226,7 +228,7 @@ int main(int argc, char** argv)
   }
 
   rclcpp::sleep_for(1s);
-  RCLCPP_INFO(logger, "Done.");
+  RCLCPP_INFO(get_logger(), "Done.");
 
   return 0;
 }

moveit_ros/warehouse/src/constraints_storage.cpp

@@ -35,6 +35,7 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/warehouse/constraints_storage.h>
+#include <moveit/utils/logger.hpp>
 
 #include <utility>
 
@@ -44,8 +45,7 @@ const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "c
 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id";
 const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id";
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.constraints_storage");
-
+using moveit::get_logger;
 using warehouse_ros::Metadata;
 using warehouse_ros::Query;
 
@@ -81,7 +81,7 @@ void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::msg
   metadata->append(ROBOT_NAME, robot);
   metadata->append(CONSTRAINTS_GROUP_NAME, group);
   constraints_collection_->insert(msg, metadata);
-  RCLCPP_DEBUG(LOGGER, "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
+  RCLCPP_DEBUG(get_logger(), "%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
 }
 
 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot,
@@ -158,7 +158,7 @@ void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string&
   Metadata::Ptr m = constraints_collection_->createMetadata();
   m->append(CONSTRAINTS_ID_NAME, new_name);
   constraints_collection_->modifyMetadata(q, m);
-  RCLCPP_DEBUG(LOGGER, "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
+  RCLCPP_DEBUG(get_logger(), "Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
 }
 
 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot,
@@ -171,5 +171,5 @@ void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string&
   if (!group.empty())
     q->append(CONSTRAINTS_GROUP_NAME, group);
   unsigned int rem = constraints_collection_->removeMessages(q);
-  RCLCPP_DEBUG(LOGGER, "Removed %u Constraints messages (named '%s')", rem, name.c_str());
+  RCLCPP_DEBUG(get_logger(), "Removed %u Constraints messages (named '%s')", rem, name.c_str());
 }

moveit_ros/warehouse/src/import_from_text.cpp

@@ -45,10 +45,11 @@
 #include <boost/program_options/options_description.hpp>
 #include <boost/program_options/parsers.hpp>
 #include <boost/program_options/variables_map.hpp>
+#include <moveit/utils/logger.hpp>
 
-static const std::string ROBOT_DESCRIPTION = "robot_description";
+using moveit::get_logger;
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.planning_scene_storage");
+static const std::string ROBOT_DESCRIPTION = "robot_description";
 
 void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm,
                 moveit_warehouse::RobotStateStorage* rs)
@@ -90,7 +91,7 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor*
         st.setVariablePositions(v);
         moveit_msgs::msg::RobotState msg;
         moveit::core::robotStateToRobotStateMsg(st, msg);
-        RCLCPP_INFO(LOGGER, "Parsed start state '%s'", name.c_str());
+        RCLCPP_INFO(get_logger(), "Parsed start state '%s'", name.c_str());
         rs->addRobotState(msg, name);
       }
     }
@@ -135,7 +136,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene
                                Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()));
     }
     else
-      RCLCPP_ERROR(LOGGER, "Unknown link constraint element: '%s'", type.c_str());
+      RCLCPP_ERROR(get_logger(), "Unknown link constraint element: '%s'", type.c_str());
     in >> type;
   }
 
@@ -150,7 +151,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene
     pose.header.frame_id = psm->getRobotModel()->getModelFrame();
     moveit_msgs::msg::Constraints constr = kinematic_constraints::constructGoalConstraints(link_name, pose);
     constr.name = name;
-    RCLCPP_INFO(LOGGER, "Parsed link constraint '%s'", name.c_str());
+    RCLCPP_INFO(get_logger(), "Parsed link constraint '%s'", name.c_str());
     cs->addConstraints(constr);
   }
 }
@@ -180,7 +181,7 @@ void parseGoal(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* p
         }
         else
         {
-          RCLCPP_INFO(LOGGER, "Unknown goal type: '%s'", type.c_str());
+          RCLCPP_INFO(get_logger(), "Unknown goal type: '%s'", type.c_str());
         }
       }
     }
@@ -209,7 +210,7 @@ void parseQueries(std::istream& in, planning_scene_monitor::PlanningSceneMonitor
       }
       else
       {
-        RCLCPP_ERROR(LOGGER, "Unknown query type: '%s'", type.c_str());
+        RCLCPP_ERROR(get_logger(), "Unknown query type: '%s'", type.c_str());
       }
     }
   }
@@ -222,6 +223,7 @@ int main(int argc, char** argv)
   node_options.allow_undeclared_parameters(true);
   node_options.automatically_declare_parameters_from_overrides(true);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options);
+  moveit::get_logger_mut() = node->get_logger();
 
   // clang-format off
   boost::program_options::options_description desc;
@@ -251,7 +253,7 @@ int main(int argc, char** argv)
   planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION);
   if (!psm.getPlanningScene())
   {
-    RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor");
+    RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor");
     return 1;
   }
 

moveit_ros/warehouse/src/initialize_demo_db.cpp

@@ -52,10 +52,11 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
 #include <rclcpp/utilities.hpp>
+#include <moveit/utils/logger.hpp>
 
 static const std::string ROBOT_DESCRIPTION = "robot_description";
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.initialize_demo_db");
+using moveit::get_logger;
 
 int main(int argc, char** argv)
 {
@@ -64,6 +65,7 @@ int main(int argc, char** argv)
   node_options.allow_undeclared_parameters(true);
   node_options.automatically_declare_parameters_from_overrides(true);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("initialize_demo_db", node_options);
+  moveit::get_logger_mut() = node->get_logger();
 
   boost::program_options::options_description desc;
   desc.add_options()("help", "Show help message")("host", boost::program_options::value<std::string>(),
@@ -90,7 +92,7 @@ int main(int argc, char** argv)
   planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION);
   if (!psm.getPlanningScene())
   {
-    RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor");
+    RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor");
     return 1;
   }
 
@@ -106,12 +108,12 @@ int main(int argc, char** argv)
   psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
   psmsg.name = "default";
   pss.addPlanningScene(psmsg);
-  RCLCPP_INFO(LOGGER, "Added default scene: '%s'", psmsg.name.c_str());
+  RCLCPP_INFO(get_logger(), "Added default scene: '%s'", psmsg.name.c_str());
 
   moveit_msgs::msg::RobotState rsmsg;
   moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg);
   rs.addRobotState(rsmsg, "default");
-  RCLCPP_INFO(LOGGER, "Added default state");
+  RCLCPP_INFO(get_logger(), "Added default state");
 
   const std::vector<std::string>& gnames = psm.getRobotModel()->getJointModelGroupNames();
   for (const std::string& gname : gnames)
@@ -138,9 +140,9 @@ int main(int argc, char** argv)
     cmsg.orientation_constraints.resize(1, ocm);
     cmsg.name = ocm.link_name + ":upright";
     cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName());
-    RCLCPP_INFO(LOGGER, "Added default constraint: '%s'", cmsg.name.c_str());
+    RCLCPP_INFO(get_logger(), "Added default constraint: '%s'", cmsg.name.c_str());
   }
-  RCLCPP_INFO(LOGGER, "Default MoveIt Warehouse was reset.");
+  RCLCPP_INFO(get_logger(), "Default MoveIt Warehouse was reset.");
 
   rclcpp::spin(node);