From 0b05f465ed36cdc04328e2a44ba9d07f578ad5a0 Mon Sep 17 00:00:00 2001
From: Walter Lucetti <walter.lucetti@stereolabs.com>
Date: Thu, 18 Sep 2025 13:10:44 +0200
Subject: [PATCH] Replace TF publisher by TransformBroadcaster

---
 .../include/isaac_ros_apriltag/apriltag_node.hpp          | 4 +++-
 isaac_ros_apriltag/src/apriltag_node.cpp                  | 8 +++++---
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp b/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp
index 0e1a705..6e4a193 100644
--- a/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp
+++ b/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp
@@ -81,10 +81,12 @@ class AprilTagNode : public rclcpp::Node
   message_filters::Synchronizer<ExactPolicy> camera_image_sync_;
 
   // Publishers
-  const rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf_pub_;
   const rclcpp::Publisher<isaac_ros_apriltag_interfaces::msg::AprilTagDetectionArray>::SharedPtr
     detections_pub_;
 
+  // TF broadcaster
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
   struct CUAprilTagImpl;
   struct VPIAprilTagImpl;
   struct AprilTagImpl;
diff --git a/isaac_ros_apriltag/src/apriltag_node.cpp b/isaac_ros_apriltag/src/apriltag_node.cpp
index 15c96e9..b88c208 100644
--- a/isaac_ros_apriltag/src/apriltag_node.cpp
+++ b/isaac_ros_apriltag/src/apriltag_node.cpp
@@ -353,7 +353,7 @@ struct AprilTagNode::VPIAprilTagImpl : AprilTagNode::AprilTagImpl
     }
 
     node.detections_pub_->publish(msg_detections);
-    node.tf_pub_->publish(tfs);
+    node.tf_broadcaster_->sendTransform(tfs.transforms);
 
     // Cleanup
     CHECK_STATUS(vpiArrayUnlock(detections_pose_array));
@@ -532,7 +532,7 @@ struct AprilTagNode::CUAprilTagImpl : AprilTagNode::AprilTagImpl
     }
 
     node.detections_pub_->publish(msg_detections);
-    node.tf_pub_->publish(tfs);
+    node.tf_broadcaster_->sendTransform(tfs.transforms);
   }
 
   ~CUAprilTagImpl()
@@ -555,7 +555,6 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions & options)
   image_sub_{},
   camera_info_sub_{},
   camera_image_sync_{ExactPolicy{3}, image_sub_, camera_info_sub_},
-  tf_pub_(create_publisher<tf2_msgs::msg::TFMessage>("tf", rclcpp::QoS(100))),
   detections_pub_{create_publisher<isaac_ros_apriltag_interfaces::msg::AprilTagDetectionArray>(
       "tag_detections", rclcpp::QoS(1))}
 {
@@ -583,6 +582,9 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions & options)
     throw std::runtime_error(os.str());
   }
 
+  // Create the TF broadcaster
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+
   // Setup subscripts
   camera_image_sync_.registerCallback(
     std::bind(