Skip to content

Fix TF availability check in nav status checker (avoid tf2_echo timeout false positives) #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
joewang0430 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix TF availability check in nav status checker (avoid tf2_echo timeout false positives) #9

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 28 additions & 9 deletions nav/nav_stack/debugging_scripts/check_nav_status.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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})')

Expand Down