From f52f483488b3ab2aa361d02d999b8d3143451527 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Darko=20Luki=C4=87?= <lukicdarkoo@gmail.com>
Date: Sun, 14 Aug 2022 21:00:47 +0200
Subject: [PATCH] Reuse action clients (#220)

---
 .../include/mep3_behavior_tree/bt_action_node.hpp    | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp
index 9dbdd18bf..e5ad9b062 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp
+++ b/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp
@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 #include "behaviortree_cpp_v3/action_node.h"
 #include "rclcpp/executors/single_threaded_executor.hpp"
@@ -98,6 +99,12 @@ class BtActionNode : public BT::ActionNodeBase
      */
   void createActionClient(const std::string & action_name)
   {
+    if (BtActionNode::action_client_list_.find(action_name) != action_client_list_.end()) {
+      action_client_ = BtActionNode::action_client_list_[action_name];
+      RCLCPP_INFO(node_->get_logger(), "Reusing an existing \"%s\" action server", action_name.c_str());
+      return;
+    }
+
     // Now that we have the ROS node to use, create the action client for this BT action
     action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
 
@@ -107,6 +114,7 @@ class BtActionNode : public BT::ActionNodeBase
       RCLCPP_FATAL(node_->get_logger(), "Action server \"%s\" is not found", action_name.c_str());
       exit(1);
     }
+    BtActionNode::action_client_list_[action_name] = action_client_;
   }
 
   /**
@@ -404,6 +412,7 @@ class BtActionNode : public BT::ActionNodeBase
   std::string action_name_, original_action_name_;
   MirrorParam mirror_;
   typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
+  static std::unordered_map<std::string, typename std::shared_ptr<rclcpp_action::Client<ActionT>>> action_client_list_;
 
   // All ROS2 actions have a goal and a result
   typename ActionT::Goal goal_;
@@ -430,6 +439,9 @@ class BtActionNode : public BT::ActionNodeBase
   rclcpp::Time time_goal_sent_;
 };
 
+template <class ActionT>
+std::unordered_map<std::string, std::shared_ptr<rclcpp_action::Client<ActionT>>> BtActionNode<ActionT>::action_client_list_;
+
 }  // namespace mep3_behavior_tree
 
 #endif  // MEP3_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_