diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index bb408646..5a17b988 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/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} ) diff --git a/rtabmap_util/scripts/republish_tf_static.py b/rtabmap_util/scripts/republish_tf_static.py new file mode 100755 index 00000000..13874cae --- /dev/null +++ b/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()