tfbug.py

import rosbag
import rospy
from time import time
from geometry_msgs.msg import Pose, Transform, TransformStamped
from visualization_msgs.msg import Marker, MarkerArray
from pprint import pprint

now = round(time())
bag = rosbag.Bag('bug.bag', 'w')

ROBOT_DISTANCE_FROM_ORIGIN = 5_000_000

try:
    for secs in range(0, 10):
        for nsecs in range(0, 1_000_000_000, 100_000_000):
            # publish transform to map
            tf = TransformStamped()
            tf.header.frame_id = 'map'
            tf.header.stamp.secs = now + secs
            tf.header.stamp.nsecs = nsecs
            tf.child_frame_id = 'base_link'
            tf.transform.translation.x = ROBOT_DISTANCE_FROM_ORIGIN
            bag.write('/tf', tf, tf.header.stamp)
    
            # create Marker
            marker = Marker()
            marker.header.frame_id = 'base_link'
            marker.header.stamp = tf.header.stamp
            marker.type = Marker.CUBE
            marker.pose.position.x = secs + nsecs/1_000_000_000 # 1 m/s
            marker.scale.x = 1
            marker.scale.y = 1
            marker.scale.z = 1
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 1.0
            marker.color.a = 0.7
            bag.write('/marker', marker, tf.header.stamp)
            
    print('\nsuccess')
finally:
    bag.close()