From e1b919d35b51d50334eaa5c51cfbb3336bf113cf Mon Sep 17 00:00:00 2001
From: Tyler Weaver <maybe@tylerjw.dev>
Date: Tue, 24 Oct 2023 09:51:22 -0600
Subject: [PATCH] Use unordered map instead of vector to avoid re-creating
 nodes

Signed-off-by: Tyler Weaver <maybe@tylerjw.dev>
---
 moveit_core/utils/include/moveit/utils/logger.hpp | 10 +++++-----
 moveit_core/utils/src/logger.cpp                  | 15 ++++++++-------
 2 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/moveit_core/utils/include/moveit/utils/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp
index 7ef63c8362..40c88093c7 100644
--- a/moveit_core/utils/include/moveit/utils/logger.hpp
+++ b/moveit_core/utils/include/moveit/utils/logger.hpp
@@ -42,16 +42,16 @@
 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
+// 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.
 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
+// 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 make_child_logger(const std::string& name);
 
-// Mutable access to global logger for setting to node logger
+// Mutable access to global logger for setting to node logger.
 rclcpp::Logger& get_logger_mut();
 
 }  // namespace moveit
diff --git a/moveit_core/utils/src/logger.cpp b/moveit_core/utils/src/logger.cpp
index f0cd880d74..a6a314f160 100644
--- a/moveit_core/utils/src/logger.cpp
+++ b/moveit_core/utils/src/logger.cpp
@@ -37,13 +37,12 @@
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/utils/logger.hpp>
 #include <rclcpp/version.h>
-#include <vector>
+#include <unordered_map>
+#include <string>
 
 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();
@@ -58,15 +57,17 @@ rclcpp::Logger make_child_logger(const std::string& name)
   // 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<std::shared_ptr<rclcpp::Node>> child_nodes;
-  std::string ns = get_logger().get_name();
-  child_nodes.push_back(std::make_shared<rclcpp::Node>(name, ns));
+  static std::unordered_map<std::string, std::shared_ptr<rclcpp::Node>> child_nodes;
+  if (child_nodes.find(name) == child_nodes.end())
+  {
+    std::string ns = get_logger().get_name();
+    child_nodes[name] = std::make_shared<rclcpp::Node>(name, ns);
+  }
 #endif
 
   return get_logger_mut().get_child(name);
 }
 
-// Mutable access to global logger for setting to node logger
 rclcpp::Logger& get_logger_mut()
 {
   static rclcpp::Logger logger = rclcpp::get_logger("moveit");