Skip to content

Mitigate out of date transforms when rclcpp::spin() is slow to handle subscriptions #71

@sloretz

Description

@sloretz

Currently tf2_ros::TransformListener has a hard coded queue depth of 100 for the /tf topic subscription.

rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
custom_qos_profile.depth = 100;
std::function<void(const tf2_msgs::msg::TFMessage::SharedPtr)> standard_callback = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1);
message_subscription_tf_ = node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf", standard_callback, custom_qos_profile);

It also (optionally) has a dedicated thread to call rclcpp::spin(). New subscriptions seem to get messages in order with this QoS and fastrtps. When this thread cannot process subscription callbacks fast enough the transform listener's callback TransformListener::subscription_callback_impl is always being called with the oldest transform in the queue. However, there are 99 newer messages available in the queue. In the case of one tf publisher publishing at 100Hz the transforms looked up will always be 1 second out of date even though there is newer data in a lower layer. If tf2 is going to be missing messages then what it really wants the latest message in the queue.

The best solution is for rclcpp::spin to always call callbacks fast enough, but maybe there is something TransformListener could do to mitigate this case.

Say there was a maintenance task on a timer that changed the queue size by creating a new subscription, and then shutting the old one down when the new one started receiving messages. The new QoS history size could be calculated by from statistics about the transform sources. I think the sum of ( period of the transform broadcaster with the longest period between messages * the frequency of each transform broadcaster) is the minimum queue size needed to ensure no data is lost.

This would mitigate ros2/ros1_bridge#133, which appears to be caused by subscription callbacks not being processed fast enough in RViz2.

Metadata

Metadata

Assignees

No one assigned

    Labels

    enhancementNew feature or request

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions