From 8cd1d909d427df7a2bd3cb1ba4c128a11ff38a26 Mon Sep 17 00:00:00 2001 From: Joe Wang Date: Sat, 17 Jan 2026 21:42:56 +0000 Subject: [PATCH] Fix TF availability check in nav status checker (avoid tf2_echo timeout false positives) --- .../debugging_scripts/check_nav_status.py | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/nav/nav_stack/debugging_scripts/check_nav_status.py b/nav/nav_stack/debugging_scripts/check_nav_status.py index 007400d..fc40b24 100755 --- a/nav/nav_stack/debugging_scripts/check_nav_status.py +++ b/nav/nav_stack/debugging_scripts/check_nav_status.py @@ -9,6 +9,9 @@ import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, DurabilityPolicy +from rclpy.duration import Duration +from rclpy.time import Time +from tf2_ros import Buffer, TransformListener from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist @@ -28,6 +31,10 @@ def __init__(self): self.scan_sub = self.create_subscription( LaserScan, '/scan_modified', self.scan_cb, 10) + # TF listener + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + # Timer to check and report status self.timer = self.create_timer(2.0, self.check_status) self.check_count = 0 @@ -53,16 +60,28 @@ def check_status(self): # Check TF self.get_logger().info('\nTF Frames:') + # try: + # result = subprocess.run( + # ['ros2', 'run', 'tf2_ros', 'tf2_echo', 'map', 'base_link'], + # capture_output=True, text=True, timeout=2) + # if 'Exception' in result.stderr or 'error' in result.stderr.lower(): + # self.get_logger().info(' map -> base_link: ✗ NOT AVAILABLE') + # else: + # self.get_logger().info(' map -> base_link: ✓ AVAILABLE') + # except subprocess.TimeoutExpired: + # self.get_logger().info(' map -> base_link: ✗ TIMEOUT') + # except Exception as e: + # self.get_logger().info(f' map -> base_link: ✗ ERROR ({e})') + + # Alternative TF check using tf2_ros Buffer to avoid mistaken timeout reports. try: - result = subprocess.run( - ['ros2', 'run', 'tf2_ros', 'tf2_echo', 'map', 'base_link'], - capture_output=True, text=True, timeout=2) - if 'Exception' in result.stderr or 'error' in result.stderr.lower(): - self.get_logger().info(' map -> base_link: ✗ NOT AVAILABLE') - else: - self.get_logger().info(' map -> base_link: ✓ AVAILABLE') - except subprocess.TimeoutExpired: - self.get_logger().info(' map -> base_link: ✗ TIMEOUT') + ok = self.tf_buffer.can_transform( + target_frame='map', + source_frame='base_link', + time=Time(), + timeout=Duration(seconds=0.5), + ) + self.get_logger().info(f' map -> base_link: {"✓ AVAILABLE" if ok else "✗ NOT AVAILABLE"}') except Exception as e: self.get_logger().info(f' map -> base_link: ✗ ERROR ({e})')