fix: Transform obtained from GetROS2TransformTool can be old (#651)

Author: Juliaj

examples/rosbot-xl-demo.py

@@ -50,7 +50,7 @@ def initialize_agent() -> Runnable[ReActAgentState, ReActAgentState]:
         "examples/embodiments/rosbotxl_embodiment.json"
     )
 
-    connector = ROS2Connector(executor_type="multi_threaded")
+    connector = ROS2Connector(executor_type="multi_threaded", use_sim_time=True)
     tools: List[BaseTool] = [
         GetROS2TransformConfiguredTool(
             connector=connector,

src/rai_core/rai/communication/ros2/connectors/base.py

@@ -25,6 +25,7 @@
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
 from rclpy.node import Node
+from rclpy.parameter import Parameter
 from rclpy.qos import QoSProfile
 from tf2_ros import Buffer, LookupException, TransformListener, TransformStamped
 
@@ -55,6 +56,8 @@ class ROS2BaseConnector(ROS2ActionMixin, ROS2ServiceMixin, BaseConnector[T]):
         Whether to destroy subscribers after receiving a message, by default False.
     executor_type : Literal["single_threaded", "multi_threaded"], optional
         Type of executor to use for processing ROS2 callbacks, by default "multi_threaded".
+    use_sim_time : bool, optional
+        Whether to use simulation time or system time, by default False.
 
     Methods
     -------
@@ -100,6 +103,7 @@ def __init__(
         node_name: str = f"rai_ros2_connector_{str(uuid.uuid4())[-12:]}",
         destroy_subscribers: bool = False,
         executor_type: Literal["single_threaded", "multi_threaded"] = "multi_threaded",
+        use_sim_time: bool = False,
     ):
         """Initialize the ROS2BaseConnector.
 
@@ -127,6 +131,10 @@ def __init__(
             )
         self._executor_type = executor_type
         self._node = Node(node_name)
+        if use_sim_time:
+            self._node.set_parameters(
+                [Parameter("use_sim_time", Parameter.Type.BOOL, True)]
+            )
         self._topic_api = ROS2TopicAPI(self._node, destroy_subscribers)
         self._service_api = ROS2ServiceAPI(self._node)
         self._actions_api = ROS2ActionAPI(self._node)

src/rai_core/rai/tools/ros2/generic/topics.py

@@ -15,6 +15,7 @@
 import json
 from typing import Any, Dict, List, Literal, Tuple, Type
 
+import rclpy.time
 import rosidl_runtime_py.set_message
 import rosidl_runtime_py.utilities
 from cv_bridge import CvBridge
@@ -243,10 +244,34 @@ class GetROS2TransformTool(BaseROS2Tool):
     description: str = "Get the transform between two frames"
     args_schema: Type[GetROS2TransformToolInput] = GetROS2TransformToolInput
 
+    # Staleness threshold in seconds (10 missed cycles at 10Hz = 1 second)
+    STALE_TRANSFORM_THRESHOLD_SEC: float = 1.0
+
     def _run(self, target_frame: str, source_frame: str, timeout_sec: float) -> str:
         transform = self.connector.get_transform(
             target_frame=target_frame,
             source_frame=source_frame,
             timeout_sec=timeout_sec,
         )
-        return stringify_dict(ros2_message_to_dict(transform))
+
+        transform_time = rclpy.time.Time.from_msg(transform.header.stamp)
+        current_time = self.connector._node.get_clock().now()
+
+        age_seconds = (current_time - transform_time).nanoseconds / 1e9
+
+        result = stringify_dict(ros2_message_to_dict(transform))
+
+        # Add clear warning for stale data at the beginning of response
+        if abs(age_seconds) > self.STALE_TRANSFORM_THRESHOLD_SEC:
+            if age_seconds == float("inf"):
+                result = (
+                    "WARNING: Robot position data has invalid timestamp (potentially stale). The robot may have moved since then.\n\n"
+                    + result
+                )
+            else:
+                result = (
+                    f"WARNING: Robot position data is {age_seconds:.1f} seconds old (potentially stale). The robot may have moved since then.\n\n"
+                    + result
+                )
+
+        return result

tests/communication/ros2/helpers.py

@@ -28,6 +28,7 @@
 from rclpy.callback_groups import CallbackGroup, ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
+from rosgraph_msgs.msg import Clock
 from sensor_msgs.msg import Image
 from std_msgs.msg import String
 from std_srvs.srv import SetBool
@@ -232,8 +233,16 @@ def publish_message(self) -> None:
 
 
 class TransformPublisher(Node):
-    def __init__(self, topic: str):
+    def __init__(self, topic: str, use_sim_time: bool = False):
         super().__init__("test_transform_publisher")
+        if use_sim_time:
+            self.set_parameters(
+                [
+                    rclpy.parameter.Parameter(
+                        "use_sim_time", rclpy.parameter.Parameter.Type.BOOL, True
+                    )
+                ]
+            )
         self.tf_broadcaster = TransformBroadcaster(self)
         self.timer = self.create_timer(0.1, self.publish_transform)
         self.frame_id = "base_link"
@@ -254,6 +263,22 @@ def publish_transform(self) -> None:
         self.tf_broadcaster.sendTransform(msg)
 
 
+class ClockPublisher(Node):
+    def __init__(self):
+        super().__init__("test_clock_publisher")
+        self.publisher = self.create_publisher(Clock, "/clock", 10)
+        self.timer = self.create_timer(0.1, self.publish_clock)
+        self.start_time = time.time()
+
+    def publish_clock(self) -> None:
+        msg = Clock()
+        # Publish simulation time that advances faster than real time
+        sim_time = (time.time() - self.start_time) * 2.0  # 2x speed
+        msg.clock.sec = int(sim_time)
+        msg.clock.nanosec = int((sim_time % 1.0) * 1e9)
+        self.publisher.publish(msg)
+
+
 def multi_threaded_spinner(
     nodes: List[Node],
 ) -> Tuple[List[MultiThreadedExecutor], List[threading.Thread]]:
@@ -274,15 +299,37 @@ def multi_threaded_spinner(
 def shutdown_executors_and_threads(
     executors: List[MultiThreadedExecutor], threads: List[threading.Thread]
 ) -> None:
-    # First shutdown executors
+    # Cancel any ongoing tasks/guard conditions
     for executor in executors:
-        executor.shutdown()
-    # Small delay to allow executors to finish pending operations
-    time.sleep(0.5)
-    # Then join threads with a timeout
+        try:
+            if hasattr(executor, "_futures"):
+                for future in executor._futures:
+                    if not future.done():
+                        future.cancel()
+        except Exception as e:
+            print(f"Error canceling ongoing tasks: {e}")
+
+    # Join threads with a timeout
     for thread in threads:
         thread.join(timeout=2.0)
 
+    # Clean up any remaining nodes
+    for executor in executors:
+        try:
+            for node in executor.get_nodes():
+                node.destroy_node()
+        except Exception as e:
+            print(f"Error destroying node: {e}")
+
+    time.sleep(0.5)
+
+    # Shutdown executors
+    for executor in executors:
+        try:
+            executor.shutdown()
+        except Exception as e:
+            print(f"Error shutting down executor: {e}")
+
 
 @pytest.fixture(scope="function")
 def ros_setup() -> Generator[None, None, None]:

tests/tools/ros2/test_topic_tools.py

@@ -37,6 +37,7 @@
 )
 
 from tests.communication.ros2.helpers import (
+    ClockPublisher,
     ImagePublisher,
     MessagePublisher,
     MessageSubscriber,
@@ -225,3 +226,122 @@ def test_get_transform_tool(ros_setup: None, request: pytest.FixtureRequest) ->
         assert "rotation" in response
     finally:
         shutdown_executors_and_threads(executors, threads)
+
+
+def test_get_transform_tool_with_stale_tf_data(
+    ros_setup: None, request: pytest.FixtureRequest
+) -> None:
+    """Test that GetROS2TransformTool returns stale transform data with warning."""
+    topic_name = f"{request.node.originalname}_topic"  # type: ignore
+    connector = ROS2Connector()
+    publisher = TransformPublisher(topic=topic_name)
+    executors, threads = multi_threaded_spinner([publisher])
+    tool = GetROS2TransformTool(connector=connector)
+
+    # Let the publisher run for a bit to establish transforms
+    time.sleep(1.0)
+    response = tool._run(
+        source_frame="map",
+        target_frame="base_link",
+        timeout_sec=5.0,
+    )  # type: ignore
+    assert "translation" in response
+    assert "rotation" in response
+
+    # stop the /tf publisher
+    publisher.timer.cancel()
+
+    # wait for some time to pass then get the transform again
+    time.sleep(2.0)
+
+    try:
+        response = tool._run(
+            source_frame="map",
+            target_frame="base_link",
+            timeout_sec=5.0,
+        )  # type: ignore
+
+        response_lower = response.lower()
+        staleness_indicators = [
+            "stale",
+            "old",
+            "outdated",
+            "warning",
+            "invalid",
+        ]
+        staleness_warning_present = any(
+            indicator in response_lower for indicator in staleness_indicators
+        )
+
+        #  Stale data should be warned about
+        if not staleness_warning_present:
+            pytest.fail(
+                f"Response: {response}. "
+                "Expected behavior: Either include staleness warning in response or raise appropriate error."
+            )
+
+    finally:
+        # Clean shutdown
+        shutdown_executors_and_threads(executors, threads)
+
+
+def test_get_transform_tool_with_stale_tf_data_sim_time(
+    ros_setup: None, request: pytest.FixtureRequest
+) -> None:
+    """Test that GetROS2TransformTool returns stale transform data with warning when using sim time."""
+    topic_name = f"{request.node.originalname}_topic"  # type: ignore
+    connector = ROS2Connector(use_sim_time=True)
+
+    # Create clock publisher to provide simulation time
+    clock_publisher = ClockPublisher()
+    publisher = TransformPublisher(topic=topic_name, use_sim_time=True)
+    executors, threads = multi_threaded_spinner([clock_publisher, publisher])
+    tool = GetROS2TransformTool(connector=connector)
+
+    # Let the publishers run for a bit to establish transforms and clock
+    time.sleep(1.0)
+    response = tool._run(
+        source_frame="map",
+        target_frame="base_link",
+        timeout_sec=5.0,
+    )  # type: ignore
+    assert "translation" in response
+    assert "rotation" in response
+
+    # stop the /tf publisher
+    publisher.timer.cancel()
+
+    # wait for some time to pass then get the transform again
+    time.sleep(2.0)
+
+    try:
+        response = tool._run(
+            source_frame="map",
+            target_frame="base_link",
+            timeout_sec=5.0,
+        )  # type: ignore
+
+        response_lower = response.lower()
+        staleness_indicators = [
+            "stale",
+            "old",
+            "outdated",
+            "warning",
+            "invalid",
+        ]
+        staleness_warning_present = any(
+            indicator in response_lower for indicator in staleness_indicators
+        )
+
+        #  Stale data should be warned about
+        if not staleness_warning_present:
+            pytest.fail(
+                f"Response: {response}. "
+                "Expected behavior: Either include staleness warning in response or raise appropriate error."
+            )
+
+    finally:
+        clock_publisher.timer.cancel()
+        # wait for the timer to be canceled
+        time.sleep(0.1)
+        shutdown_executors_and_threads(executors, threads)