├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── nodes └── static_transform_mux └── package.xml /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package static_transform_mux 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.2 (2024-04-04) 6 | ------------------ 7 | * Fixed Python 3 compatibility 8 | * Contributors: Martin Pecka 9 | 10 | 1.1.1 (2023-05-13) 11 | ------------------ 12 | * Noetic compatibility. 13 | * Added possibility to publish to a different topic. 14 | * Contributors: Martin Pecka 15 | 16 | 1.1.0 (2019-03-05) 17 | ------------------ 18 | * Changed the cache key to allow restructuring the TF tree. 19 | * Contributors: Martin Pecka 20 | 21 | 1.0.1 (2018-09-06) 22 | ------------------ 23 | * Fixed initialization order so that the subscriber is the last and does not trigger the callback before constructor finishes. 24 | * Removed dependency on tf package. 25 | * Create LICENSE.txt 26 | * Contributors: Martin Pecka 27 | 28 | 1.0.0 (2018-08-30) 29 | ------------------ 30 | * Initial commit. 31 | * Contributors: Martin Pecka 32 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(static_transform_mux) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS rospy tf2_msgs 8 | ) 9 | 10 | catkin_install_python(PROGRAMS 11 | nodes/static_transform_mux 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 13 | ) 14 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Long-Term Human-Robot Teaming for Robot-Assisted Disaster Response 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Static Transform Mux 2 | 3 | This ROS node subscribes to `/tf_static`, collects all the transforms that are ever published there, and re-sends a 4 | message that contains not only the transforms from the last publisher, but all transforms ever encountered. 5 | 6 | This is a workaround for e.g. [geometry2#181](https://github.com/ros/geometry2/issues/181), or for anybody else who 7 | needs to have multiple static transform publishers in the system. 8 | 9 | ## Node `static\_transform\_mux` 10 | 11 | ### Subscribed topics 12 | 13 | * `/tf_static`: The static transforms. 14 | 15 | ### Published topics 16 | 17 | * topic defined in param `~publisher_topic` (`tf2_msgs/TFMessage` latched): Again, the static transforms. Special care 18 | is taken so that this node does not react to its own messages. 19 | * `ready` (`std_msgs/Bool` latched): This message is sent (and latched) once the first message on `/tf_static` is 20 | published by this node, so that other nodes that rely on this node's operation know that it has been brought up. 21 | 22 | ### Parameters 23 | 24 | * `~update_only_with_newer` (`bool`, defaults to `False`): If set to True, only transforms with newer timestamps can 25 | substitute an already cached transform (otherwise, the latest received tranform is used.) 26 | * `~forbidden_callerid_prefix` (`str` or `None`, defaults to `None`): This parameter can contain a prefix of 27 | [`callerid`s](http://wiki.ros.org/ROS/Master_API) that will additionally be ignored by the `/tf_static` callback. 28 | Using this parameter is required if you transmit `/tf_static` over some non-ROS connection which changes or 29 | discards the `callerid` (e.g. when using [`nimbro_network`](https://github.com/AIS-Bonn/nimbro_network/)). 30 | * `~publisher_topic` (`str`, defaults to `/tf_static`): Which topic to publish to. This can be the same topic as the 31 | input. 32 | -------------------------------------------------------------------------------- /nodes/static_transform_mux: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from collections import OrderedDict 4 | 5 | import rospy 6 | 7 | from tf2_msgs.msg import TFMessage 8 | from std_msgs.msg import Bool 9 | 10 | 11 | class StaticTransformMux(object): 12 | """This node subscribes to /tf_static, collects all the transforms that are ever published there, and re-sends a 13 | message that contains not only the transforms from the last publisher, but all transforms ever encountered. 14 | """ 15 | def __init__(self): 16 | """Initialize all publishers/subscribers.""" 17 | 18 | self._ready = False 19 | 20 | self._transforms = OrderedDict() 21 | 22 | self._update_only_with_newer = rospy.get_param("~update_only_with_newer", False) 23 | self._forbidden_callerid_prefix = rospy.get_param("~forbidden_callerid_prefix", None) 24 | self._pub_topic = rospy.get_param("~publisher_topic", "/tf_static") 25 | 26 | self._tf_publisher = rospy.Publisher(self._pub_topic, TFMessage, latch=True, queue_size=100) 27 | self._ready_publisher = rospy.Publisher("static_transform_mux/ready", Bool, latch=True, queue_size=1) 28 | self._tf_subscriber = rospy.Subscriber("/tf_static", TFMessage, self.tf_static_cb, queue_size=100) 29 | 30 | def tf_static_cb(self, tf_msg): 31 | """Callback for messages from /tf_static. 32 | 33 | Add all transforms to the cache and then publish a TF message with all the transforms. 34 | :param TFMessage tf_msg: A /tf_static message. 35 | """ 36 | 37 | # Protect against reacting to this node's own published messages, otherwise it'd end up in an infinite loop. 38 | callerid = tf_msg._connection_header['callerid'] 39 | if callerid == rospy.get_name() or \ 40 | (self._forbidden_callerid_prefix is not None and callerid.startswith(self._forbidden_callerid_prefix)): 41 | return 42 | 43 | # Process the incoming transforms, merge them with our cache. 44 | for transform in tf_msg.transforms: 45 | key = transform.child_frame_id 46 | if key not in self._transforms or not self._update_only_with_newer or \ 47 | self._transforms[key].header.stamp < transform.header.stamp: 48 | rospy.logdebug("Updated transform %s->%s. Old timestamp was %s, new timestamp is %s." % ( 49 | transform.header.frame_id, transform.child_frame_id, 50 | str(self._transforms[key].header.stamp if key in self._transforms else "None"), 51 | str(transform.header.stamp))) 52 | 53 | self._transforms[key] = transform 54 | 55 | # Publish the cached messages. 56 | msg = TFMessage() 57 | msg.transforms = self._transforms.values() 58 | self._tf_publisher.publish(msg) 59 | 60 | rospy.logdebug("Sent %i transforms: %s" % (len(self._transforms), str(self._transforms.keys()))) 61 | 62 | # After publishing the first message, send also this ~/ready message, so that nodes that rely on this node's 63 | # operation know that it has been brought up. 64 | if not self._ready: 65 | self._ready = True 66 | ready_msg = Bool() 67 | ready_msg.data = True 68 | self._ready_publisher.publish(ready_msg) 69 | rospy.logdebug("Sent ready message") 70 | 71 | 72 | if __name__ == '__main__': 73 | rospy.init_node('static_transform_mux') 74 | mux = StaticTransformMux() 75 | rospy.spin() 76 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | static_transform_mux 4 | 1.1.2 5 | A helper node that makes sure everybody knows about all static transforms, even if they are published by multiple publishers. 6 | 7 | Martin Pecka 8 | 9 | BSD 10 | 11 | http://www.ros.org/wiki/static_transform_mux 12 | 13 | Martin Pecka 14 | 15 | catkin 16 | rospy 17 | tf2_msgs 18 | 19 | 20 | --------------------------------------------------------------------------------