Allow using latest time for transform lookups

[pepisg]

Hi!

I was recently playing around with possible failures that can be induced on a system, and realized that with the current implementation of nav2, a freeze in the tf tree does not cause the stack to stop immediately (ex: the node publishing odom->baselink died).

I'm submitting this proposal to raise a failure immediately when that happens. Is there something i may be missing out?

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_controller/src/controller_server.cpp	80.48% <100.00%> (ø)
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	100.00% <ø> (ø)
nav2_costmap_2d/src/costmap_2d_ros.cpp	90.00% <100.00%> (-0.21%)	⬇️

... and 11 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

Yes, this removes the fine time synchronization of data in a system which can cause issues with robots moving at high speeds, tight sensitive control loops, or leading the system's global localization in rare cases. Anything we do a "now" call its basically ignoring the time information available in the request or the context of operation.

Overall, I think this is the wrong solution to the problem.

a freeze in the tf tree does not cause the stack to stop immediately

I wouldn't necessarily expect that to mean we should halt immediately. Not all odometry or global localization sources update at a regular frequency. As a matter of general purpose implementation, we cannot know when the lack of update is meaningful due to lack of new data / corrections or a failure.

However things like AMCL/SLAM will publish a transform into the future some milliseconds (basically the update rate duration) so that there's a valid transform at all times during normal operations until the next transform is made available. If however there's an issue, then that global transformation becomes outdated and then you again would stop due to the lack of valid data.

It seems like your future transform publisher needs to have something similarly done with more bounds so that it stops once its clear that there's no data coming from a key source -- but also your robot should be tolerant of your global localization system not updating for a short period (that's why we have odometry). Then the system would stop when its clear there's an issue.

ex: the node publishing odom->baselink died

In addition, the best solution would actually to use Bond or another watchdog system to watch over the system and halt navigation / other actions when core nodes are not responding (due to crash, deadlock, or other error). That way you can immediately detect it explicitly instead of relying on derived information like TF updates which are somewhat ambiguous and themselves not going to be quick things to detect. This is actually what we do in Nav2 in the lifecycle manager; we have a bond connection that we check and if anything crashes or is unresponsive, we bring down the entire stack right then and there to a safe fault state.

[pepisg]

Sorry for the late reply here.

That makes sense. Do you think it would still be useful to add a a maximum age for transforms? I don't think the controller being able to operate silently with transforms that are multiple seconds old is a good default behavior. I understand the right solution would be to use Bond on the localization system, however I think this could push users away of most of the open source alternatives in the community (RL, fuse, etc) and force them to basically build their own stack from the start.

I'd be happy to rework this to not pass now() as an argument for looking up transform, but exposing a max transform age to raise the same error on the case of missing transforms for a time the user deems reasonable for their application.

Let me know!

[SteveMacenski]

Does the transform_tolerance_ not take care of this? That would ensure its in the bounds, no?

[pepisg]

I think when you pass time point zero it doesn't. From what I understand, the transform tolerance refers to the amount of time tf can wait for the requested transform to be available at the given time. If you pass a request with time point zero, if there's already a transform in the buffer, it will return that immediately regardless of its age, and the tolerance will comply because the buffer had to wait less than transform_tolerance time for the transform.

I made this simple script to test it out:

import time

import rclpy
from geometry_msgs.msg import TransformStamped
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.time import Time
from tf2_ros import Buffer, TransformBroadcaster, TransformException, TransformListener


class TfLatestLookupMRE(Node):
    def __init__(self) -> None:
        super().__init__("tf_latest_lookup_mre")
        self.broadcaster = TransformBroadcaster(self)
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)

    def publish_once(self) -> None:
        msg = TransformStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "map"
        msg.child_frame_id = "base_link"
        msg.transform.translation.x = 1.0
        self.broadcaster.sendTransform(msg)
        self.get_logger().info("Published one transform: map -> base_link")

    def lookup_latest_with_timeout(self) -> None:
        timeout = Duration(seconds=0.5)
        try:
            transform = self.buffer.lookup_transform(
                "map",
                "base_link",
                Time(),  # Time point zero: latest transform
                timeout,
            )
            tf_stamp = Time.from_msg(transform.header.stamp)
            age_sec = (self.get_clock().now() - tf_stamp).nanoseconds / 1e9
            self.get_logger().info(
                "LOOKUP SUCCEEDED with time=0 and timeout=0.5s. "
                f"Returned transform age: {age_sec:.3f}s: {transform}"
            )
        except TransformException as exc:
            self.get_logger().error(
                "LOOKUP FAILED with time=0 and timeout=0.5s: "
                f"{type(exc).__name__}: {exc}"
            )


def main() -> None:
    rclpy.init()
    node = TfLatestLookupMRE()
    try:
        node.publish_once()

        # Allow DDS/TF propagation for the first message.
        end = time.time() + 0.3
        while time.time() < end:
            rclpy.spin_once(node, timeout_sec=0.05)

        node.get_logger().info("Waiting 5 seconds without publishing updates...")
        end = time.time() + 5.0
        while time.time() < end:
            rclpy.spin_once(node, timeout_sec=0.1)

        node.lookup_latest_with_timeout()
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

and looks like the behavior matches this, despite being 5s old, when requested at timepoint zero with 0.5s tolerance, the lookup succeeds.

[SteveMacenski]

Interesting - where / how are we using a zero time point and can that have a meaningfully added timestamp from its call context? Seems like this is a usage issue we should fix then potentially. Might be something to audit more broadly to see if we do things like that frequently.

I agree then, this seems like an issue. But I think the solve is to use timestamps in the requests anywhere where required (which I would think is most if not all). In this case, I think getRobotPose() can use the stamp from the input pose and then audit users of it to give it a stamp.

Thanks for pointing this out!

[SteveMacenski]

@pepisg any update? This seems like a good thing to get nailed down before the Lyrical freeze in a few weeks

[SteveMacenski]

@pepisg last call....

[pepisg]

hey! sorry! i'm not sure i can make time for this during this weel

[SteveMacenski]

Do you want to file a ticket to describe this conversation? Maybe with some exposed discussion we can pick it up a future time and/or another contributor may take it up that we just need to test/approve :-)

It sounds like we just need to propogate timestamps in a few places better to use 'real' times rather than Time(0)'s, which should actually be easy to find