From 5fc91b4081419721e05cf7b52d5900bdd94132dc Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 14 Apr 2020 15:40:06 +0200
Subject: [PATCH] Correctly handle multiple StaticTransformBroadcaster in a
 single process

As ROS latches only the last message from a given node to a topic,
when using multiple static broadcasters, only the last message was actually kept.
To correctly handle this, we need to share the actual publisher across
all instances within a process.
---
 .../tf2_ros/static_transform_broadcaster.h    |  7 +---
 tf2_ros/src/static_transform_broadcaster.cpp  | 37 ++++++++++++++++---
 2 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
index bc7be9171..9483bedfc 100644
--- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
+++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
@@ -42,6 +42,7 @@
 namespace tf2_ros
 {
 
+struct StaticTransformBroadcasterImpl;
 
 /** \brief This class provides an easy way to publish coordinate frame transform information.  
  * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the 
@@ -63,11 +64,7 @@ class StaticTransformBroadcaster{
   void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
 
 private:
-  /// Internal reference to ros::Node
-  ros::NodeHandle node_;
-  ros::Publisher publisher_;
-  tf2_msgs::TFMessage net_message_;
-
+  std::shared_ptr<StaticTransformBroadcasterImpl> impl_;
 };
 
 }
diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp
index 2e20842c7..a7905aa91 100644
--- a/tf2_ros/src/static_transform_broadcaster.cpp
+++ b/tf2_ros/src/static_transform_broadcaster.cpp
@@ -35,12 +35,37 @@
 #include "tf2_msgs/TFMessage.h"
 #include "tf2_ros/static_transform_broadcaster.h"
 #include <algorithm>
+#include <mutex>
 
 namespace tf2_ros {
 
+struct StaticTransformBroadcasterImpl {
+  ros::NodeHandle node_;  // internal reference to node
+  ros::Publisher publisher_;
+  tf2_msgs::TFMessage net_message_;  // message comprising all static transforms
+
+  StaticTransformBroadcasterImpl() {
+    publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf_static", 100, true);
+  }
+  StaticTransformBroadcasterImpl(const StaticTransformBroadcasterImpl& other) = delete;
+
+  static std::shared_ptr<StaticTransformBroadcasterImpl> getInstance() {
+    static std::mutex mutex;
+    static std::weak_ptr<StaticTransformBroadcasterImpl> singleton;
+
+    std::lock_guard<std::mutex> lock(mutex);
+    if (singleton.expired()) {  // create a new instance if required
+      auto result = std::make_shared<StaticTransformBroadcasterImpl>();
+      singleton = result;
+      return result;
+    }  // otherwise return existing one
+    return singleton.lock();
+  }
+};
+
 StaticTransformBroadcaster::StaticTransformBroadcaster()
 {
-  publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf_static", 100, true);
+  impl_ = StaticTransformBroadcasterImpl::getInstance();
 };
 
 void StaticTransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)
@@ -50,15 +75,17 @@ void StaticTransformBroadcaster::sendTransform(const std::vector<geometry_msgs::
     auto predicate = [&input](const geometry_msgs::TransformStamped existing) {
       return input.child_frame_id == existing.child_frame_id;
     };
-    auto existing = std::find_if(net_message_.transforms.begin(), net_message_.transforms.end(), predicate);
 
-    if (existing != net_message_.transforms.end())
+    auto& transforms = impl_->net_message_.transforms;
+    auto existing = std::find_if(transforms.begin(), transforms.end(), predicate);
+
+    if (existing != transforms.end())
       *existing = input;
     else
-      net_message_.transforms.push_back(input);
+      transforms.push_back(input);
   }
 
-  publisher_.publish(net_message_);
+  impl_->publisher_.publish(impl_->net_message_);
 }
 
 }