pure rviz launch has empty message frame

[2006wu]

[rviz2-1] [INFO] [1767716222.506471867] [rviz]: Message Filter dropping message: frame '' at time 0.000 for reason 'the frame id of the message is empty'

[JustinShih0918]

@2006wu please look into TEB controller, see which frame id is not well set.

[JustinShih0918]

comparing opening sim_launch.py, in local machine, the message didn;t pop out, it is highly expect that there are sensor msg publish from other group that containing empty frame id

[JustinShih0918]

by inspection script, found:
TOPICS WITH EMPTY FRAME_ID (4):
🚨 THESE ARE THE PROBLEM TOPICS! 🚨
• /bond (bond/msg/Status)
• /diagnostics (diagnostic_msgs/msg/DiagnosticArray)
• /final_pose_nav (nav_msgs/msg/Odometry)
• /imu/data_cov (sensor_msgs/msg/Imu)

[JustinShih0918]

I would keep the code here

#!/usr/bin/env python3
"""
Script to find ROS2 topics publishing messages with empty frame_id
"""
import rclpy
from rclpy.node import Node
import sys
import time
from collections import defaultdict

class FrameIdChecker(Node):
    def __init__(self):
        super().__init__('frame_id_checker')
        self.results = defaultdict(list)
        self.checked_topics = set()
        
    def check_all_topics(self):
        # Get all topics
        topic_list = self.get_topic_names_and_types()
        
        print(f"\n🔍 Found {len(topic_list)} topics in ROS2 graph")
        print("="*60)
        
        # Debug: show all topics first
        print("\n📋 All available topics:")
        for topic_name, topic_types in topic_list:
            print(f"   • {topic_name} [{topic_types[0]}]")
        
        print("\n" + "="*60)
        print("Starting detailed checks...")
        print("="*60)
        
        checked_count = 0
        skipped_count = 0
        
        for topic_name, topic_types in topic_list:
            if topic_name in self.checked_topics:
                continue
                
            # Skip parameter and rosout topics
            if '/parameter' in topic_name or '/rosout' in topic_name:
                print(f"⏭️  Skipping: {topic_name} (system topic)")
                skipped_count += 1
                continue
            
            print(f"\n🔎 Checking [{checked_count + 1}/{len(topic_list) - skipped_count}]: {topic_name}")
            print(f"   Type: {topic_types[0]}")
            self.check_topic(topic_name, topic_types[0])
            self.checked_topics.add(topic_name)
            checked_count += 1
        
        print(f"\n\n📊 Checked {checked_count} topics (skipped {skipped_count} system topics)")
        self.print_results()
    
    def check_topic(self, topic_name, topic_type):
        try:
            # Import the message type dynamically
            # ROS2 format: package/msg/MessageType or package/srv/ServiceType
            module_parts = topic_type.split('/')
            
            if len(module_parts) == 3:
                # Standard ROS2 format: package/msg/MessageType
                package, interface_type, msg_name = module_parts
                print(f"   📦 Loading message type: {package}.{interface_type}.{msg_name}")
            elif len(module_parts) == 2:
                # Legacy format: package/MessageType
                package, msg_name = module_parts
                interface_type = 'msg'
                print(f"   📦 Loading message type: {package}.msg.{msg_name}")
            else:
                print(f"   ⚠️  Invalid topic type format: {topic_type}")
                self.results['❌ Error'].append(f"{topic_name} ({topic_type}): Invalid format")
                return
            
            module = __import__(f"{package}.{interface_type}", fromlist=[msg_name])
            msg_class = getattr(module, msg_name)
            
            # Create a one-shot subscriber
            received = {'msg': None, 'done': False}
            
            def callback(msg):
                if not received['done']:
                    received['msg'] = msg
                    received['done'] = True
            
            sub = self.create_subscription(msg_class, topic_name, callback, 10)
            print(f"   ⏳ Waiting for message (max 2s)...")
            
            # Wait for one message (max 2 seconds)
            start_time = time.time()
            while not received['done'] and (time.time() - start_time) < 2.0:
                rclpy.spin_once(self, timeout_sec=0.1)
            
            wait_time = time.time() - start_time
            
            # Check if message has header with frame_id
            if received['msg'] is not None:
                print(f"   ✅ Received message after {wait_time:.2f}s")
                if hasattr(received['msg'], 'header'):
                    header = received['msg'].header
                    frame_id = header.frame_id
                    print(f"   📋 Has header field:")
                    print(f"      header:")
                    print(f"        stamp:")
                    print(f"          sec: {header.stamp.sec}")
                    print(f"          nanosec: {header.stamp.nanosec}")
                    print(f"        frame_id: '{frame_id}'")
                    
                    if frame_id == '' or frame_id is None:
                        self.results['❌ EMPTY FRAME_ID'].append(f"{topic_name} ({topic_type})")
                        print(f"   ❌ ⚠️  EMPTY FRAME_ID DETECTED!")
                    else:
                        self.results['✅ Valid frame_id'].append(f"{topic_name} → '{frame_id}'")
                        print(f"   ✅ Valid frame_id")
                elif hasattr(received['msg'], 'pose') and hasattr(received['msg'].pose, 'header'):
                    # Check for PoseStamped-like messages
                    header = received['msg'].pose.header
                    frame_id = header.frame_id
                    print(f"   📋 Has pose.header field:")
                    print(f"      pose.header:")
                    print(f"        stamp:")
                    print(f"          sec: {header.stamp.sec}")
                    print(f"          nanosec: {header.stamp.nanosec}")
                    print(f"        frame_id: '{frame_id}'")
                    
                    if frame_id == '' or frame_id is None:
                        self.results['❌ EMPTY FRAME_ID'].append(f"{topic_name} ({topic_type})")
                        print(f"   ❌ ⚠️  EMPTY FRAME_ID DETECTED!")
                    else:
                        self.results['✅ Valid frame_id'].append(f"{topic_name} → '{frame_id}'")
                        print(f"   ✅ Valid frame_id")
                else:
                    # No header field
                    print(f"   ℹ️  No header field (not a stamped message)")
                    self.results['ℹ️  No header'].append(f"{topic_name} ({topic_type})")
            else:
                print(f"   ⏸️  No message received in {wait_time:.2f}s (topic may be inactive)")
                self.results['⏸️  Inactive'].append(f"{topic_name} ({topic_type})")
            
            self.destroy_subscription(sub)
            
        except Exception as e:
            print(f"   ❌ Error: {str(e)}")
            self.results['❌ Error'].append(f"{topic_name} ({topic_type}): {str(e)}")
    
    def print_results(self):
        print("\n\n" + "="*60)
        print("📊 FRAME_ID CHECK RESULTS")
        print("="*60)
        
        # Critical issues
        if self.results['❌ EMPTY FRAME_ID']:
            print(f"\n⚠️  TOPICS WITH EMPTY FRAME_ID ({len(self.results['❌ EMPTY FRAME_ID'])}):")
            print("    🚨 THESE ARE THE PROBLEM TOPICS! 🚨")
            for topic in self.results['❌ EMPTY FRAME_ID']:
                print(f"   • {topic}")
        else:
            print("\n✅ No topics with empty frame_id found!")
        
        # Valid topics
        if self.results['✅ Valid frame_id']:
            print(f"\n✅ Topics with valid frame_id ({len(self.results['✅ Valid frame_id'])}):")
            for topic in self.results['✅ Valid frame_id']:
                print(f"   • {topic}")
        
        # Inactive topics
        if self.results['⏸️  Inactive']:
            print(f"\n⏸️  Inactive topics ({len(self.results['⏸️  Inactive'])}):")
            for topic in self.results['⏸️  Inactive'][:5]:
                print(f"   • {topic}")
            if len(self.results['⏸️  Inactive']) > 5:
                print(f"   ... and {len(self.results['⏸️  Inactive']) - 5} more")
        
        # Topics without headers
        if self.results['ℹ️  No header']:
            print(f"\nℹ️  Topics without header field ({len(self.results['ℹ️  No header'])}):")
            for topic in self.results['ℹ️  No header'][:5]:
                print(f"   • {topic}")
            if len(self.results['ℹ️  No header']) > 5:
                print(f"   ... and {len(self.results['ℹ️  No header']) - 5} more")
        
        # Errors
        if self.results['❌ Error']:
            print(f"\n❌ Topics with errors ({len(self.results['❌ Error'])}):")
            for topic in self.results['❌ Error']:
                print(f"   • {topic}")
        
        print("\n" + "="*60)
        print(f"📈 Summary:")
        print(f"   • Empty frame_id: {len(self.results['❌ EMPTY FRAME_ID'])}")
        print(f"   • Valid frame_id: {len(self.results['✅ Valid frame_id'])}")
        print(f"   • No header: {len(self.results['ℹ️  No header'])}")
        print(f"   • Inactive: {len(self.results['⏸️  Inactive'])}")
        print(f"   • Errors: {len(self.results['❌ Error'])}")
        print("="*60 + "\n")

def main(args=None):
    rclpy.init(args=args)
    
    try:
        checker = FrameIdChecker()
        checker.check_all_topics()
    except KeyboardInterrupt:
        print("\n⚠️  Interrupted by user")
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()