From db834d661a61026b49016c6ab182dcb7f34e43e9 Mon Sep 17 00:00:00 2001
From: bpapaspyros <b.papaspyros@gmail.com>
Date: Thu, 5 Dec 2024 15:07:27 +0100
Subject: [PATCH] fix: add helper function to check if node is initialized

---
 .../modulo_controllers/BaseControllerInterface.hpp  |  6 ++++++
 .../src/BaseControllerInterface.cpp                 | 13 +++++++++++--
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
index c2e17ec4..0b50651c 100644
--- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
+++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
@@ -405,6 +405,12 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   std::timed_mutex& get_command_mutex();
 
+  /**
+   * @brief Check if the node has been initialized or not.
+   * @return True if the node is initialized, false otherwise
+   */
+  bool is_node_initialized() const;
+
 private:
   /**
    * @brief Parameter validation function
diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp
index 3fc9637f..ce3f410c 100644
--- a/source/modulo_controllers/src/BaseControllerInterface.cpp
+++ b/source/modulo_controllers/src/BaseControllerInterface.cpp
@@ -8,6 +8,7 @@
 
 #include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/message_readers.hpp>
+#include <stdexcept>
 
 template<class... Ts>
 struct overloaded : Ts... {
@@ -543,10 +544,9 @@ void BaseControllerInterface::add_service(
 }
 
 void BaseControllerInterface::add_tf_listener() {
-  if (this->get_node() == nullptr) {
+  if (!is_node_initialized()) {
     throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
   }
-
   if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
     RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF buffer and listener.");
     console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
@@ -654,4 +654,13 @@ std::timed_mutex& BaseControllerInterface::get_command_mutex() {
   return command_mutex_;
 }
 
+bool BaseControllerInterface::is_node_initialized() const {
+  try {
+    get_node();
+    return true;
+  } catch (const std::runtime_error& ex) {
+    return false;
+  }
+}
+
 }// namespace modulo_controllers