tf2_ros_py StaticTransformBroadcaster Should Maintain List of Transforms

[egordon]

A StaticTransformBroadcaster should maintain a list of static transforms so that the last-published message contains all transforms managed by the node.

This is how it is done in the CPP implementation, but not the Python implementation.

This leads to an issue where a node can publish 2 static transforms but only have the last one latched.

I believe the Python implementation should be updated to match the CPP implementation in this regard (but can still keep the Union[TS, List[TS]] interface to handle multiple new TFs at once).

[clalancette]

Looking at the code in https://github.com/ros2/geometry2/blob/humble/tf2_ros_py/tf2_ros/static_transform_broadcaster.py#L64 , it does seem to be a list of transforms.

Could you provide a minimal reproducible example so we could take a look at the behavior you are seeing?

[egordon]

@clalancette

Sure, rclpy takes a list of transforms, but does not cache it on subsequent calls.

See the attached minimal CPP and Python examples. They exhibit different behaviors. CPP caches both transforms, while Python only caches the last transform. Therefore, in Python, the STB client has to maintain a list of transformations and pass them all into the STB to avoid losing old transforms.

I'm not saying one is better than the other, only that these two examples should have identical behavior.

Created using ros2 run tf2_tools view_frames after running each node: frames_rclcpp_stb.pdf frames_rclpy_stb.pdf

CPP:

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher()
  : Node("static_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms();
  }

private:
  void make_transforms()
  {
    geometry_msgs::msg::TransformStamped t1, t2;

    t2.header.stamp = t1.header.stamp = this->get_clock()->now();
    t2.header.frame_id = t1.header.frame_id = "world";
    t1.child_frame_id = "frame1";
    t2.child_frame_id = "frame2";

    tf_static_broadcaster_->sendTransform(t1);
    tf_static_broadcaster_->sendTransform(t2);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>());
  rclcpp::shutdown();
  return 0;
}

Python:

#!/usr/bin/python3

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self):
        super().__init__('static_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms()

    def make_transforms(self):
        t1 = TransformStamped()
        t2 = TransformStamped()

        t2.header.stamp = t1.header.stamp = self.get_clock().now().to_msg()
        t2.header.frame_id = t1.header.frame_id = 'world'
        t1.child_frame_id = 'frame1'
        t2.child_frame_id = 'frame2'

        self.tf_static_broadcaster.sendTransform(t1)
        self.tf_static_broadcaster.sendTransform(t2)

def main():
    logger = rclpy.logging.get_logger('logger')

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

if __name__ == '__main__':
    main()

[clalancette]

I'm not saying one is better than the other, only that these two examples should have identical behavior.

Yeah, I see what you are saying now. Thanks for the explanation. I think for now we should probably make the Python one match the C++ one, i.e. add the cache to the Python one. This has other problems (in that we need a way to invalidate the cache if we want to remove something from it), but that will be a problem we need to solve in both later.

See #634 which should fix this.

[egordon]

Thanks!

Just as a note, invalidating the cache may not be needed, since in theory I think static transforms are supposed to be valid for the lifetime of the node (and unchanging in theory, although TF2's implementation allows them to be updated, useful in my case since we have a slow-changing TF that we don't want to publish constantly).

Even if you publish a static TF without an old transform, nodes that were online for the first publication would still keep it in their own internal TF cache, i.e., there is no way to invalidate a static TF across the entire ROS network without shutting down every TF Listener and starting from scratch.

[clalancette]

Just as a note, invalidating the cache may not be needed, since in theory I think static transforms are supposed to be valid for the lifetime of the node (and unchanging in theory, although TF2's implementation allows them to be updated, useful in my case since we have a slow-changing TF that we don't want to publish constantly).

There are use cases, in my opinion, for removing static transforms. In particular, consider a manipulator with changeable tools on the end. There may be some static transforms between the manipulator and the tool, or internal to the tool itself. But when the tool gets changed out, those need to be removed and new ones substituted. You could consider these to not be "static", but there are certain advantages to making these things static (particularly bandwidth). However, that problem is just something I've been kicking around in my head for a while; I don't have a concrete use right at the moment.