Added republish_tf_static.py for convenience when replaying rosbags w…

…ith /tf_static not latched

rtabmap_util/CMakeLists.txt

@@ -131,6 +131,7 @@ catkin_install_python(PROGRAMS
   scripts/yaml_to_camera_info.py
   scripts/netvlad_tf_ros.py
   scripts/gazebo_ground_truth.py
+  scripts/republish_tf_static.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

rtabmap_util/scripts/republish_tf_static.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+import rospy
+from tf2_msgs.msg import TFMessage
+
+msg=TFMessage()
+
+def callback(data):
+    global msg
+    if len(msg.transforms) == 0:
+        msg = data
+    else:
+        msg.transforms = msg.transforms+ data.transforms
+    rospy.loginfo("Received /tf_static_old and republising latched /tf_static")
+    pub.publish(msg)
+
+if __name__ == '__main__':
+    rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber("tf_static_old", TFMessage, callback)
+    pub = rospy.Publisher('tf_static', TFMessage, queue_size=10, latch=True)
+    rospy.spin()