diff --git a/plansys2_bringup/params/plansys2_params.yaml b/plansys2_bringup/params/plansys2_params.yaml
index 3c68e8d55..0c2d78fb4 100644
--- a/plansys2_bringup/params/plansys2_params.yaml
+++ b/plansys2_bringup/params/plansys2_params.yaml
@@ -1,13 +1,12 @@
 plansys2:
   ros__parameters:
     use_real_time: false
-    
 planner_client:
   ros__parameters:
-    plan_solver_timeout: 15.0  
+    plan_solver_timeout: 15.0
 planner:
   ros__parameters:
-    plan_solver_timeout: 15.0  
+    plan_solver_timeout: 15.0
     plan_solver_plugins: ["POPF"]
     POPF:
       plugin: "plansys2/POPFPlanSolver"
diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp
index e87d7ceba..b7576a9f9 100644
--- a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp
+++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp
@@ -130,12 +130,12 @@ class BTAction : public plansys2::ActionExecutorClient
    * @param tree BT to monitor
    * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
    */
-  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+  void add_groot_monitoring(BT::Tree * tree, uint16_t server_port);
 
   /**
    * @brief Reset Groot2 monitor
    */
-  void resetGrootMonitor();
+  void reset_groot_monitor();
 
   /**
    * @brief Factory for creating Behavior Trees instances.
diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp
index b35cba974..551bf36b5 100644
--- a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp
+++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp
@@ -56,12 +56,20 @@ class BtActionNode : public BT::ActionNodeBase
     if (!config().blackboard->get("node", node_)) {
       RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard");
     }
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
+    auto bt_loop_duration =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
     getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->get<std::chrono::milliseconds>("wait_for_service_timeout");
 
+    // Timeout should be less than bt_loop_duration to be able to finish the current tick
+    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
+
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
@@ -88,7 +96,7 @@ class BtActionNode : public BT::ActionNodeBase
   bool createActionClient(const std::string & action_name)
   {
     // 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);
+    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
 
     // Make sure the server is actually there before continuing
     RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
@@ -116,10 +124,10 @@ class BtActionNode : public BT::ActionNodeBase
   {
     BT::PortsList basic = {
       BT::InputPort<std::string>("server_name", "Action server name"),
-      BT::InputPort<double>(
+      BT::InputPort<std::chrono::milliseconds>(
         "server_timeout",
-        5.0,
-        "The amount of time to wait for a response from the action server, in seconds")
+        5000,
+        "The amount of time to wait for a response from the action server, in milliseconds")
     };
     // The user defined ports are added to the basic ports
     basic.insert(addition.begin(), addition.end());
@@ -221,14 +229,15 @@ class BtActionNode : public BT::ActionNodeBase
       case GOAL_SENT:
         {
           RCLCPP_DEBUG(node_->get_logger(), "%s GOAL_SENT", node_->get_name());
-          if (future_goal_handle_.valid()) {
-            goal_handle_ = future_goal_handle_.get();
-
-            if (!goal_handle_) {
+          if (future_goal_handle_) {
+            auto elapsed =
+              (node_->now() - goal_sent_ts_).template to_chrono<std::chrono::milliseconds>();
+            if (!is_future_goal_handle_complete(elapsed)) {
               RCLCPP_ERROR(
                 node_->get_logger(),
                 "Goal was rejected by action server %s", action_name_.c_str());
               state_ = GOAL_FAILURE;
+              future_goal_handle_.reset();
               return BT::NodeStatus::FAILURE;
             } else {
               state_ = GOAL_EXECUTING;
@@ -240,6 +249,7 @@ class BtActionNode : public BT::ActionNodeBase
                 node_->get_logger(),
                 "Failed to send goal to action server %s", action_name_.c_str());
               state_ = GOAL_FAILURE;
+              future_goal_handle_.reset();
               return BT::NodeStatus::FAILURE;
             } else {
               return BT::NodeStatus::RUNNING;
@@ -267,6 +277,8 @@ class BtActionNode : public BT::ActionNodeBase
             state_ = GOAL_SENT;
           }
 
+          callback_group_executor_.spin_some();
+
           if (goal_result_available_) {
             state_ = GOAL_FINISHING;
           }
@@ -308,6 +320,7 @@ class BtActionNode : public BT::ActionNodeBase
               node_->get_logger(),
               "Failed to cancel action server for %s", action_name_.c_str());
             state_ = GOAL_FAILURE;
+            future_goal_handle_.reset();
             return BT::NodeStatus::FAILURE;
           }
         }
@@ -316,6 +329,7 @@ class BtActionNode : public BT::ActionNodeBase
       case GOAL_FINISHED:
         {
           RCLCPP_DEBUG(node_->get_logger(), "%s GOAL_FINISHED", node_->get_name());
+          state_ = IDLE;
           return BT::NodeStatus::SUCCESS;
         }
         break;
@@ -323,6 +337,7 @@ class BtActionNode : public BT::ActionNodeBase
       case GOAL_FAILURE:
         {
           RCLCPP_DEBUG(node_->get_logger(), "%s GOAL_FAILURE", node_->get_name());
+          state_ = IDLE;
           return BT::NodeStatus::FAILURE;
         }
         break;
@@ -331,6 +346,7 @@ class BtActionNode : public BT::ActionNodeBase
         break;
     }
 
+    goal_handle_.reset();
     return BT::NodeStatus::RUNNING;
   }
 
@@ -355,8 +371,15 @@ class BtActionNode : public BT::ActionNodeBase
    */
   void cancel_goal()
   {
-    if (goal_handle_ == nullptr) {
+    if (!goal_handle_) {
       future_cancer_handle_ = action_client_->async_cancel_goal(goal_handle_);
+      if (callback_group_executor_.spin_until_future_complete(future_cancer_handle_,
+          server_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
+      {
+        RCLCPP_ERROR(
+          node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str());
+      }
+
     } else {
       if (!createActionClient(action_name_)) {
         RCLCPP_ERROR(node_->get_logger(), "Failed to create action client");
@@ -380,6 +403,7 @@ class BtActionNode : public BT::ActionNodeBase
       return true;
     }
 
+    callback_group_executor_.spin_some();
     auto status = goal_handle_->get_status();
 
     // Check if the goal is still executing
@@ -411,14 +435,52 @@ class BtActionNode : public BT::ActionNodeBase
       };
 
     RCLCPP_INFO(
-      node_->get_logger(),
-      "Sending goal to action server %s",
-      action_name_.c_str());
+      node_->get_logger(), "Sending goal to action server %s", action_name_.c_str());
 
-    future_goal_handle_ = action_client_->async_send_goal(goal_, send_goal_options);
+    future_goal_handle_ = std::make_shared<
+      std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
+      action_client_->async_send_goal(goal_, send_goal_options));
     goal_sent_ts_ = node_->now();
   }
 
+/**
+   * @brief Function to check if the action server acknowledged a new goal
+   * @param elapsed Duration since the last goal was sent and future goal handle has not completed.
+   * After waiting for the future to complete, this value is incremented with the timeout value.
+   * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
+   */
+  bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
+  {
+    auto remaining = server_timeout_ - elapsed;
+
+    // server has already timed out, no need to sleep
+    if (remaining <= std::chrono::milliseconds(0)) {
+      future_goal_handle_.reset();
+      return false;
+    }
+
+    auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
+    auto result =
+      callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
+    elapsed += timeout;
+
+    if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
+      future_goal_handle_.reset();
+      throw std::runtime_error("send_goal failed");
+    }
+
+    if (result == rclcpp::FutureReturnCode::SUCCESS) {
+      goal_handle_ = future_goal_handle_->get();
+      future_goal_handle_.reset();
+      if (!goal_handle_) {
+        throw std::runtime_error("Goal was rejected by the action server");
+      }
+      return true;
+    }
+
+    return false;
+  }
+
   /**
    * @brief Function to increment recovery count on blackboard if this node wraps a recovery
    */
@@ -437,7 +499,7 @@ class BtActionNode : public BT::ActionNodeBase
   typename ActionT::Goal goal_;
   bool goal_updated_{false};
   bool goal_result_available_{false};
-  std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>
+  std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
   future_goal_handle_;
   std::shared_future<typename ActionT::Impl::CancelGoalService::Response::SharedPtr>
   future_cancer_handle_;
@@ -447,11 +509,16 @@ class BtActionNode : public BT::ActionNodeBase
 
   // The node that will be used for any ROS operations
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   // The timeout value while waiting for response from a server when a
   // new action goal is sent or canceled
   std::chrono::milliseconds server_timeout_;
 
+  // The timeout value for BT loop execution
+  std::chrono::milliseconds max_timeout_;
+
   // The timeout value for waiting for a service to response
   std::chrono::milliseconds wait_for_service_timeout_;
 
diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp
index ab421b429..5938e2add 100644
--- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp
+++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp
@@ -41,7 +41,7 @@ BTAction::BTAction(const std::string & action)
   declare_parameter<bool>("bt_minitrace_logging", false);
   declare_parameter<bool>("enable_groot_monitoring", false);
   declare_parameter<int>("server_port", -1);
-  declare_parameter<int>("server_timeout", 20);
+  declare_parameter<int>("server_timeout", 5000);
   declare_parameter<int>("wait_for_service_timeout", 1000);
 }
 
@@ -54,7 +54,7 @@ BTAction::BTAction(const std::string & action, const std::chrono::nanoseconds &
   declare_parameter<bool>("bt_minitrace_logging", false);
   declare_parameter<bool>("enable_groot_monitoring", false);
   declare_parameter<int>("server_port", -1);
-  declare_parameter<int>("server_timeout", 20);
+  declare_parameter<int>("server_timeout", 5000);
   declare_parameter<int>("wait_for_service_timeout", 1000);
 }
 
@@ -111,7 +111,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
 {
   // If a new tree is created, than the Groot2 Publisher must be destroyed
-  resetGrootMonitor();
+  reset_groot_monitor();
 
   try {
     tree_ = factory_.createTreeFromFile(bt_xml_file_, blackboard_);
@@ -171,7 +171,7 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
       RCLCPP_WARN(get_logger(), "Groot2 monitoring port not provided, disabling it");
     } else {
       RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", server_port);
-      addGrootMonitoring(&tree_, server_port);
+      add_groot_monitoring(&tree_, server_port);
     }
   }
 
@@ -185,7 +185,7 @@ BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state)
   bt_minitrace_logger_.reset();
   bt_file_logger_.reset();
   tree_.haltTree();
-  resetGrootMonitor();
+  reset_groot_monitor();
 
   return ActionExecutorClient::on_deactivate(previous_state);
 }
@@ -222,7 +222,7 @@ void BTAction::do_work()
   }
 }
 
-void BTAction::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+void BTAction::add_groot_monitoring(BT::Tree * tree, uint16_t server_port)
 {
   // This logger publish status changes using Groot2
   groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
@@ -232,7 +232,7 @@ void BTAction::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
   BT::RegisterJsonDefinition<std_msgs::msg::Header>();
 }
 
-void BTAction::resetGrootMonitor()
+void BTAction::reset_groot_monitor()
 {
   if (groot_monitor_) {
     groot_monitor_.reset();
diff --git a/plansys2_bt_actions/test/unit/bt_action_test.cpp b/plansys2_bt_actions/test/unit/bt_action_test.cpp
index 9d561f5d6..30a524888 100644
--- a/plansys2_bt_actions/test/unit/bt_action_test.cpp
+++ b/plansys2_bt_actions/test/unit/bt_action_test.cpp
@@ -152,8 +152,9 @@ TEST_F(BTActionsTestCase, load_plugins)
 
   auto blackboard = BT::Blackboard::create();
   blackboard->set("node", node);
-  blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
+  blackboard->set<std::chrono::milliseconds>("server_timeout", 250ms);
   blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout", 10ms);
+  blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 100ms);
   BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
 
   rclcpp::Rate rate(10);
@@ -191,8 +192,9 @@ TEST_F(BTActionsTestCase, on_tick_failure)
   BT::assignDefaultRemapping<plansys2_bt_tests::OnTickFail>(config);
   auto bb = BT::Blackboard::create();
   bb->set("node", node);
-  bb->set<std::chrono::milliseconds>("server_timeout", 20ms);
+  bb->set<std::chrono::milliseconds>("server_timeout", 250ms);
   bb->set<std::chrono::milliseconds>("wait_for_service_timeout", 10ms);
+  bb->set<std::chrono::milliseconds>("bt_loop_duration", 100ms);
   config.blackboard = bb;
 
   plansys2_bt_tests::OnTickFail failure_node("OnTickFail", "move", config);
@@ -234,8 +236,9 @@ TEST_F(BTActionsTestCase, on_feedback_failure)
   BT::assignDefaultRemapping<plansys2_bt_tests::OnFeedbackFail>(config);
   auto bb = BT::Blackboard::create();
   bb->set("node", node);
-  bb->set<std::chrono::milliseconds>("server_timeout", 20ms);
+  bb->set<std::chrono::milliseconds>("server_timeout", 250ms);
   bb->set<std::chrono::milliseconds>("wait_for_service_timeout", 10ms);
+  bb->set<std::chrono::milliseconds>("bt_loop_duration", 100ms);
   config.blackboard = bb;
 
   plansys2_bt_tests::OnFeedbackFail failure_node("OnFeedbackFail",
diff --git a/plansys2_executor/include/plansys2_executor/BTUtils.hpp b/plansys2_executor/include/plansys2_executor/BTUtils.hpp
new file mode 100644
index 000000000..603b710cf
--- /dev/null
+++ b/plansys2_executor/include/plansys2_executor/BTUtils.hpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2024 Davide Faconti
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANSYS2_EXECUTOR__BTUTILS_HPP_
+#define PLANSYS2_EXECUTOR__BTUTILS_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp/behavior_tree.h"
+
+namespace BT
+{
+
+/**
+ * @brief Parse XML string to std::chrono::milliseconds
+ * @param key XML string
+ * @return std::chrono::milliseconds
+ */
+template<>
+inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
+{
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::chrono::milliseconds>(new_key);
+  }
+
+  return std::chrono::milliseconds(std::stoul(key.data()));
+}
+
+/**
+ * @brief Try reading an import port first, and if that doesn't work
+ * fallback to reading directly the blackboard.
+ * The blackboard must be passed explicitly because config() is private in BT.CPP 4.X
+ *
+ * @param bt_node node
+ * @param blackboard the blackboard obtained with node->config().blackboard
+ * @param param_name std::string
+ * @param behavior_tree_node the node
+ * @return <T>
+ */
+template<typename T> inline
+bool getInputPortOrBlackboard(
+  const BT::TreeNode & bt_node,
+  const BT::Blackboard & blackboard,
+  const std::string & param_name,
+  T & value)
+{
+  if (bt_node.getInput<T>(param_name, value)) {
+    return true;
+  }
+  if (blackboard.get<T>(param_name, value)) {
+    return true;
+  }
+  return false;
+}
+
+// Macro to remove boiler plate when using getInputPortOrBlackboard
+#define getInputOrBlackboard(name, value) \
+  getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value);
+
+}  // namespace BT
+
+#endif  // PLANSYS2_EXECUTOR__BTUTILS_HPP_
diff --git a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp
index 5dfe0c0ae..5e882563f 100644
--- a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp
+++ b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp
@@ -169,12 +169,12 @@ class ComputeBT : public rclcpp_lifecycle::LifecycleNode
    * @param tree BT to monitor
    * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
    */
-  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+  void add_groot_monitoring(BT::Tree * tree, uint16_t server_port);
 
   /**
    * @brief Reset Groot2 monitor
    */
-  void resetGrootMonitor();
+  void reset_groot_monitor();
 
   std::string action_bt_xml_;
   std::string start_action_bt_xml_;
diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp
index 8965af22e..74083c859 100644
--- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp
+++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp
@@ -302,6 +302,18 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
    */
   void cancel_all_running_actions(PlanRuntineInfo & runtime_info);
 
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void add_groot_monitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset Groot2 monitor
+   */
+  void reset_groot_monitor();
+
   std::string action_bt_xml_;
   std::string start_action_bt_xml_;
   std::string end_action_bt_xml_;
@@ -348,18 +360,6 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
 
   PlanRuntineInfo runtime_info_;
 
-  /**
-   * @brief Add Groot2 monitor to publish BT status changes
-   * @param tree BT to monitor
-   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
-   */
-  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
-
-  /**
-   * @brief Reset Groot2 monitor
-   */
-  void resetGrootMonitor();
-
   // Groot2 monitor
   std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
diff --git a/plansys2_executor/include/plansys2_executor/JSONUtils.hpp b/plansys2_executor/include/plansys2_executor/JSONUtils.hpp
new file mode 100644
index 000000000..f11416532
--- /dev/null
+++ b/plansys2_executor/include/plansys2_executor/JSONUtils.hpp
@@ -0,0 +1,98 @@
+// Copyright (c) 2025 Alberto J. Tudela Roldán
+// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANSYS2_EXECUTOR__JSONUTILS_HPP_
+#define PLANSYS2_EXECUTOR__JSONUTILS_HPP_
+
+#include <string>
+#include <unordered_map>
+
+#include "behaviortree_cpp/json_export.h"
+#include "builtin_interfaces/msg/time.hpp"
+#include "std_msgs/msg/header.hpp"
+
+// The follow templates are required when using Groot 2 to visualize the BT. They
+// convert the data types into JSON format easy for visualization.
+
+namespace builtin_interfaces::msg
+{
+BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg)
+{
+  add_field("sec", &msg.sec);
+  add_field("nanosec", &msg.nanosec);
+}
+
+}  // namespace builtin_interfaces::msg
+
+namespace std_msgs::msg
+{
+
+BT_JSON_CONVERTER(std_msgs::msg::Header, msg)
+{
+  add_field("stamp", &msg.stamp);
+  add_field("frame_id", &msg.frame_id);
+}
+
+}  // namespace std_msgs::msg
+
+namespace std
+{
+
+inline void from_json(const nlohmann::json & js, std::chrono::milliseconds & dest)
+{
+  if (js.contains("ms")) {
+    dest = std::chrono::milliseconds(js.at("ms").get<int>());
+  } else {
+    throw std::runtime_error("Invalid JSON for std::chrono::milliseconds");
+  }
+}
+
+inline void to_json(nlohmann::json & js, const std::chrono::milliseconds & src)
+{
+  js["__type"] = "std::chrono::milliseconds";
+  js["ms"] = src.count();
+}
+
+inline void from_json(
+  const nlohmann::json & js,
+  std::unordered_map<std::string, std::string> & dest)
+{
+  if (js.is_object()) {
+    dest.clear();
+    for (auto & [key, value] : js.items()) {
+      if (value.is_string()) {
+        dest[key] = value.get<std::string>();
+      } else {
+        throw std::runtime_error(
+            "Invalid JSON for std::unordered_map<std::string, std::string>: value must be string");
+      }
+    }
+  } else {
+    throw std::runtime_error(
+        "Invalid JSON for std::unordered_map<std::string, std::string>: must be object");
+  }
+}
+
+inline void to_json(nlohmann::json & js, const std::unordered_map<std::string, std::string> & src)
+{
+  js = nlohmann::json::object();
+  for (const auto & [key, value] : src) {
+    js[key] = value;
+  }
+}
+
+}  // namespace std
+
+#endif  // PLANSYS2_EXECUTOR__JSONUTILS_HPP_
diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp
index fbaa39bd7..69eb8d62d 100644
--- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp
+++ b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp
@@ -41,6 +41,8 @@
 #include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp"
 #include "plansys2_executor/behavior_tree/restore_atstart_effect_node.hpp"
 #include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp"
+#include "plansys2_executor/BTUtils.hpp"
+#include "plansys2_executor/JSONUtils.hpp"
 
 namespace plansys2
 {
@@ -178,6 +180,7 @@ ComputeBT::on_cleanup(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name());
   dotgraph_pub_.reset();
+  reset_groot_monitor();
   RCLCPP_INFO(get_logger(), "[%s] Cleaned up", get_name());
   return CallbackReturnT::SUCCESS;
 }
@@ -347,6 +350,9 @@ ComputeBT::computeBTCallback(
   (*action_map)[":0"].at_start_effects_applied_time = now();
   (*action_map)[":0"].at_end_effects_applied_time = now();
 
+  // If a new tree is created, than the Groot2 Publisher must be destroyed
+  reset_groot_monitor();
+
   auto blackboard = BT::Blackboard::create();
   blackboard->set("action_map", action_map);
   blackboard->set("action_graph", action_graph);
@@ -361,7 +367,7 @@ ComputeBT::computeBTCallback(
   int server_port = get_parameter("server_port").as_int();
   if (enable_groot_monitoring) {
     RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %i", server_port);
-    addGrootMonitoring(&tree, server_port);
+    add_groot_monitoring(&tree, server_port);
   }
 
   finish = true;
@@ -422,13 +428,17 @@ ComputeBT::saveDotGraph(const std::string & dotgraph, const std::string & filena
   }
 }
 
-void ComputeBT::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+void ComputeBT::add_groot_monitoring(BT::Tree * tree, uint16_t server_port)
 {
   // This logger publish status changes using Groot2
   groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+
+  // Register common types JSON definitions
+  BT::RegisterJsonDefinition<builtin_interfaces::msg::Time>();
+  BT::RegisterJsonDefinition<std_msgs::msg::Header>();
 }
 
-void ComputeBT::resetGrootMonitor()
+void ComputeBT::reset_groot_monitor()
 {
   if (groot_monitor_) {
     groot_monitor_.reset();
diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp
index 6fc3998eb..6d624ce55 100644
--- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp
+++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp
@@ -50,6 +50,8 @@
 #include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp"
 #include "plansys2_executor/behavior_tree/restore_atstart_effect_node.hpp"
 #include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp"
+#include "plansys2_executor/BTUtils.hpp"
+#include "plansys2_executor/JSONUtils.hpp"
 
 namespace plansys2
 {
@@ -218,6 +220,7 @@ ExecutorNode::on_deactivate(const rclcpp_lifecycle::State & state)
   dotgraph_pub_->on_deactivate();
   executing_plan_pub_->on_deactivate();
   remaining_plan_pub_->on_deactivate();
+  reset_groot_monitor();
   RCLCPP_INFO(get_logger(), "[%s] Deactivated", get_name());
 
   return CallbackReturnT::SUCCESS;
@@ -461,6 +464,13 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info)
   blackboard->set("domain_client", domain_client_);
   blackboard->set("problem_client", problem_client_);
   blackboard->set("bt_builder", bt_builder);
+  // Added blackboard keys for compatibility with other nodes
+  blackboard->set("bt_loop_duration", std::chrono::milliseconds(200));
+  blackboard->set("server_timeout", std::chrono::milliseconds(250));
+  blackboard->set("wait_for_service_timeout", std::chrono::milliseconds(1000));
+
+  // If a new tree is created, than the Groot2 Publisher must be destroyed
+  reset_groot_monitor();
 
   runtime_info.current_tree = std::make_shared<TreeInfo>();
   *runtime_info.current_tree = {
@@ -469,8 +479,8 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info)
   bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool();
   int server_port = get_parameter("server_port").as_int();
   if (enable_groot_monitoring) {
-    RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", get_name(), server_port);
-    addGrootMonitoring(&runtime_info.current_tree->tree, server_port);
+    RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", server_port);
+    add_groot_monitoring(&runtime_info.current_tree->tree, server_port);
   }
 
   return runtime_info.current_tree != nullptr;
@@ -867,13 +877,17 @@ ExecutorNode::execution_cycle()
   }
 }
 
-void ExecutorNode::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+void ExecutorNode::add_groot_monitoring(BT::Tree * tree, uint16_t server_port)
 {
   // This logger publish status changes using Groot2
   groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+
+  // Register common types JSON definitions
+  BT::RegisterJsonDefinition<builtin_interfaces::msg::Time>();
+  BT::RegisterJsonDefinition<std_msgs::msg::Header>();
 }
 
-void ExecutorNode::resetGrootMonitor()
+void ExecutorNode::reset_groot_monitor()
 {
   if (groot_monitor_) {
     groot_monitor_.reset();