Robot Blinking Like an UFO in RViz and Costmap Issues, Potential Problem with odom to base_footprint Broadcast for Real Robot

Question

I'm experiencing an issue with my ROS2 setup where my robot appears to blink like a UFO in RViz, and the costmap blinks intermittently. Here’s a detailed breakdown of my situation:

1 - Setup

• I have a ROS2 bag collected from my real robot while it was static, including IMU, odom, and GPS data.
• I'm using EKF and navsat_transform nodes for localization.
• The robot’s movement data is only available as velocity (vx) from the CAN bus, without explicit x or y positions.

2- Issues

• My current static transform broadcaster works when the robot is static but fails dynamically.
• When I run a script to broadcast a transform dynamically, the robot blinks back and forth in RViz like a UFO, and the costmap in RViz blinks around every 10 seconds (not regular time).
• After the ROS2 bag finishes playing or I kill the transform script, the robot seems to "compensate" by moving forward and backward, and the costmaps appear more consistently. It seems like old odometry data in the query cache is being processed when new data stops coming in, causing this delayed compensation effect.
• I suspect there may be an issue with the odom to base_footprint broadcast. This was previously handled by the Gazebo plugin (see source), but for the real robot, I need to implement this using a dynamic TF2 broadcaster as outlined in this tutorial.

3- Attempted solution

• I wrote a simple Python script to broadcast transforms based on velocity, but it didn't resolve the issue.
• Here’s my current script: https://pastebin.com/FDhiAQsA.
• We fixed the costmap and localization issues when the robot is static based on this solution, but the problem persists when the robot is moving.

4- Logs

• Complete log available in This Link

• The logs show multiple errors related to transforms, including TF_OLD_DATA and missing frames. Here’s a snippet:

  [planner_server-9] [INFO] [1716764701.265721102] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [rviz2-1] [INFO] [1716764703.459606032] [rviz]: Message Filter dropping message: frame 'odom' at time 1716764701,399 for reason 'discarding message because the queue is full' [ekf_node-5] Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1716571173.978938 according to authority Authority undetectable

5- Demonstranting Video

• Here is a video demonstrating the issue: Broadcast Odom to Base Robot Issue

6- Questions:

• How can I properly implement a dynamic TF broadcaster given that I only have velocity (vx) data?

• Is there a recommended approach or example code (Real Robot Transform Github using GPS data to fusion) that handles this scenario?

In my case I use GPS, I am aware that If I used SLAM they have a proper mechanism to handle this TF from odom to base_footprint by means of lidar matching and Slam Toolbox usage

7- Provided Material to reproduce my issue

• ROS2 Bag of Inertial data of my vehicle moving forward (IMU, GPS, ODOM)

Answer 1

My broadcast script, which performs the transformation between the robot base (base_footprint) and the odometry frame(odom), emulating what Gazebo broadcasts (but now for a real robot) were wrong. I was sending fixed data instead of real values data of imu and odom. The code of this broadcaster is below if someone needs to do something similar.

2- Used message_filters pkg in this broadcaster to synchronize the timestamp of odom and imu data

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import math
from sensor_msgs.msg import Imu 
from nav_msgs.msg import Odometry
#import tf_transformations
import transforms3d.euler
from message_filters import ApproximateTimeSynchronizer, Subscriber
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class DynamicTransformBroadcaster(Node):
    def __init__(self):
        super().__init__('tf2_dynamic_broadcaster')
        self.broadcaster = TransformBroadcaster(self)
        #Timer not requeired when using message_fitlers synch time.
        #self.timer = self.create_timer(0.1, self.broadcast_transforms)
        
        self._imu_data = None
        self._odom_data = None

        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=100
        )

        imu_sub = Subscriber(self, Imu, 'imu', qos_profile = qos_profile)
        odom_sub = Subscriber(self, Odometry, 'odom',  qos_profile = qos_profile)

        self.ts = ApproximateTimeSynchronizer([imu_sub, odom_sub], queue_size=100, slop=0.2)
        self.ts.registerCallback(self.callback)
    
    def callback(self, imu_msg, odom_msg):
        self._imu_data = imu_msg.orientation
        self._odom_data = odom_msg
        self.broadcast_transforms()
    

    def get_yaw_from_quaternion(self, q):
        #euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])
        euler = transforms3d.euler.quat2euler([q.w, q.x, q.y, q.z])
        return euler[2]  # yaw

    def broadcast_transforms(self):
        now = self.get_clock().now().to_msg()

        if self._imu_data is None or self._odom_data is None:
            self.get_logger().info("Waiting for IMU and Odometry data...")
            return

        # Transform from odom to base_footprint
        odom_to_base_footprint = TransformStamped()
        odom_to_base_footprint.header.stamp = now
        odom_to_base_footprint.header.frame_id = 'odom'
        odom_to_base_footprint.child_frame_id = 'base_footprint'
        odom_to_base_footprint.transform.translation.x = self._odom_data.pose.pose.position.x
        odom_to_base_footprint.transform.translation.y = self._odom_data.pose.pose.position.y
        odom_to_base_footprint.transform.translation.z = 0.0

        yaw =  self.get_yaw_from_quaternion(self._imu_data) # math.radians(0) 
        
        odom_to_base_footprint.transform.rotation.x = 0.0
        odom_to_base_footprint.transform.rotation.y = 0.0
        odom_to_base_footprint.transform.rotation.z = math.sin(yaw / 2)
        odom_to_base_footprint.transform.rotation.w = math.cos(yaw / 2)

        # Broadcasting the transforms
        self.broadcaster.sendTransform(odom_to_base_footprint)

def main(args=None):
    rclpy.init(args=args)
    node = DynamicTransformBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3- Increased the queue_size on the navsat params file from 10 to 100. This avoided jumps.

    # For parameter descriptions, please refer to the template parameter files for each node.
    
    ekf_filter_node_odom:
      ros__parameters:
        frequency: 30.0
        two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
        publish_acceleration: false 
        #print_diagnostics: true
        debug: false
        publish_tf: true
    
    # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
    #     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
    # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
    #    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
    # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
    #    observations) then:
    #     3a. Set your "world_frame" to your map_frame value
    #     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
    #     
    
    
        map_frame: map
        odom_frame: odom
        base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
        world_frame: map #odom # it seemst that for GPS this is how must be used : https://robotics.stackexchange.com/questions/101504/robot-localization-not-publishing-odom-base-link
    
        odom0: /odom
        odom0_config: [false, false, false,   # Position X, Y, Z
                      false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                      true, false, false,   # Velocity X dot, Y dot, Z dot
                      false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                      false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
        odom0_differential: false  # Typically false for odometry, as it's usually more accurate.
        odom0_nodelay: true # Ignore delays in the odometry data.
        odom0_relative: false
        odom0_queue_size: 100
    
        imu0: /imu
        imu0_config: [false, false, false,   # Position X, Y, Z
                      false, false, true,    # Orientation roll, pitch, yaw (only yaw is used)
                      false, false, false,   # Velocity X dot, Y dot, Z dot
                      false, false, true,   # Angular Velocity roll dot, pitch dot, yaw dot
                      false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
        imu0_differential: true  # Set to true for real robot IMU data due to typical inaccuracies.
      # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
        imu0_nodelay: true # Ignore delays in the imu data.
        imu0_relative: false
        imu0_queue_size: 100
        imu0_remove_gravitational_acceleration: true
    
        use_control: false
    
        process_noise_covariance: [1e-3, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.01,  0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,   0.0,    0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,   0.0,    0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,   0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,   0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]
    
    ekf_filter_node_map:
      ros__parameters:
        use_sim_time: false 
        sensor_timeout: 0.25  # Typical value: 0.1-0.5 seconds. Tolerance for sensor synchronization.
        transform_time_offset: 0.0  # Typical value: 0.0 seconds. Time offset for transforms.
        transform_timeout: 0.25  # Typical value: 0.1-0.5 seconds. Time to wait for a transform.
        freq

uency: 30.0
    two_d_mode: false #true  # Recommended to use 2d mode for nav2 in mostly planar environments
    #print_diagnostics: true
    publish_acceleration: false 
    debug: false
    publish_tf: true
    reset_on_time_jump: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
    world_frame: map

    odom0: /odom #/odometry/local # = "fused odometry + Imu in previous node"
    odom0_config: [true, true, false,   # Position X, Y, Z
                  false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                  true, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    odom0_queue_size: 100
    odom0_nodelay: true 
    odom0_differential: false  # Typically false for odometry, as it's usually more accurate.
    odom0_relative: false

    imu0: /imu
    imu0_config: [false, false, false,   # Position X, Y, Z
                  false, false, true,    # Orientation roll, pitch, yaw (only yaw is used)
                  false, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    imu0_differential: true  # Set to true for real robot IMU data due to typical inaccuracies.
    imu0_nodelay: false # Ignore delays in the imu data.
    imu0_relative: false
    imu0_queue_size: 100
    imu0_remove_gravitational_acceleration: true


#O problema esta aqui, o gazebo publica gps namespace, porém ele deve poder englobar diferentes tipos de dados (Odometry) e também receber o (NavSatFix). Já quando eu uso real_gps/fix aqui
#Eu envio apeans NavSatFix, mas não tenho um namespace para receber outros tipos de mensagens que vem da Fusão anterior (odometry/local) que o ekf_filter_node_odom está entregaando.

    odom1: odometry/gps # try gps if not fix
    odom1_config: [true, true, false,   # Position X, Y, Z
                  false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                  false, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    odom1_queue_size: 100
    odom1_nodelay: true # Ignore delays in the odometry/gps data.
    odom1_differential: false  # Typically false for odometry, as it's usually more accurate.
    odom1_relative: false


    use_control: false

    process_noise_covariance: [1.0,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    1.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]

navsat_transform:
  ros__parameters:
    use_sim_time: false
    frequency: 30.0
    delay: 3.0  # Typical value: 0.0-3.0 seconds. Accounts for delay in the sensor data. GPS usually has inherent delays
    magnetic_declination_radians: 0.266512 #0.0
    yaw_offset: 0.0
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false #true
    wait_for_datum: true #true 
    datum: [-23.659684066666667, -46.59128556666667, 0.0] # pre-set datum if needed, [lat, lon, yaw]

However as you can check in the video below, I still have some jumping on the beginning of localization (I guess may be related to GPS wrong synchronization with imu and odom, but not sure), and I still didn't figure out how to fix it. If someone knows please let me know.

Fixed Dynamic Broadcaster

I consider the issue is 99% fixed.

Initial Pose Jump

-----------------UPDATE-------------------------

Fixed increasing both covariance matrixes diagonals values for 10 times each value!

No initial Jump