From d9c65b6368a1d2e87653c45e1d3f48fe1ec56dc0 Mon Sep 17 00:00:00 2001
From: bpapaspyros <b.papaspyros@gmail.com>
Date: Thu, 5 Dec 2024 13:35:43 +0100
Subject: [PATCH] fix: check if node is initialized before adding TF
 broadcasters

---
 source/modulo_controllers/src/BaseControllerInterface.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp
index 7963d819..3dcfdba8 100644
--- a/source/modulo_controllers/src/BaseControllerInterface.cpp
+++ b/source/modulo_controllers/src/BaseControllerInterface.cpp
@@ -598,6 +598,9 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo
 }
 
 void BaseControllerInterface::add_tf_broadcaster() {
+  if (this->get_node() == nullptr) {
+    throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
+  }
   if (this->tf_broadcaster_ == nullptr) {
     RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster.");
     console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
@@ -609,6 +612,9 @@ void BaseControllerInterface::add_tf_broadcaster() {
 }
 
 void BaseControllerInterface::add_static_tf_broadcaster() {
+  if (this->get_node() == nullptr) {
+    throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
+  }
   if (this->static_tf_broadcaster_ == nullptr) {
     RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster.");
     console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);