Working with ROS Bag Files: Time Issues and Solutions
Introduction
Working with ROS bag files is a fundamental skill for robotics developers, but several time-related issues can trip up even experienced engineers. In this blog post, I’ll walk through three common challenges and their solutions: controlling bag playback timing parameters, ensuring nodes properly use simulated time, and handling tf_static transforms when starting bag files from different positions.
1. Controlling Bag Playback Parameters
When replaying bag files, you can control several time-related parameters to suit your needs:
Clock Rate
Adjusting the playback speed is straightforward using the --clock and --rate parameters:
rosbag play --clock --rate=0.5 my_dataset.bagThis plays the bag at half-speed while publishing the /clock topic, which is essential for simulated time.
Start Time
To start playback from a specific timestamp within the bag:
rosbag play --clock --start=10 my_dataset.bagThis skips the first 10 seconds of recorded data.
Duration
To play only a certain duration from the bag:
rosbag play --clock --duration=30 my_dataset.bagThis plays only 30 seconds of the bag file from your starting point.
Combining Parameters
These parameters can be combined for precise control:
rosbag play --clock --rate=1.5 --start=15 --duration=45 my_dataset.bagThis plays 45 seconds of data, starting 15 seconds into the bag, at 1.5x speed.
2. Running Nodes with use_sim_time
When working with bag files, it’s crucial to configure your nodes to use simulated time rather than the system clock.
Setting the Parameter
The key parameter is use_sim_time, which must be set to true:
rosparam set use_sim_time trueYou can also set this in your launch file:
<param name="/use_sim_time" value="true" />Or within individual node configurations:
<node pkg="my_package" type="my_node" name="node_name">
<param name="use_sim_time" value="true" />
</node>Common Issues
If your nodes aren’t properly synchronized with the bag playback, check that:
- The bag is being played with the
--clockflag - The
use_sim_timeparameter is set totruebefore starting your nodes - Your nodes are checking for this parameter and using the
/clocktopic
A common mistake is starting nodes before setting use_sim_time, causing them to initialize with system time and then suddenly jump when they receive simulated time messages.
3. Solving tf_static Problems When Starting Bags at Different Positions
One of the trickiest issues with bag files involves tf_static transforms when starting playback from different positions.
Understanding the Problem
The tf_static topic contains transforms that don’t change over time (e.g., the position of a camera relative to a robot base). When you start a bag file from the middle:
- Messages on the
/tf_statictopic might have been published before your start point - Nodes that need these transforms will be waiting for data that was “missed”
- This leads to transform lookup failures and errors like “Could not find transform”
Solutions
Use the –pause Flag
Start the bag with the --pause flag to load everything before starting playback:
rosbag play --clock --pause my_dataset.bagThis loads the entire bag, including static transforms, before you unpause it with the spacebar.
Republish tf_static
You can also extract and republish static transforms:
rostopic echo -b my_dataset.bag -p /tf_static > static_transforms.txt
rostopic pub /tf_static tf2_msgs/TFMessage "$(cat static_transforms.txt)" --onceUsing tf2_ros Buffer
If you’re writing a node that relies on tf_static, increase your tf buffer timeout and implement retry logic:
import tf2_ros
buffer = tf2_ros.Buffer(rospy.Duration(30.0)) # Longer buffer time
tf_listener = tf2_ros.TransformListener(buffer)
def get_transform_with_retry(target_frame, source_frame, timeout=5.0):
start_time = rospy.Time.now()
while (rospy.Time.now() - start_time).to_sec() < timeout:
try:
return buffer.lookup_transform(target_frame, source_frame, rospy.Time(0))
except (tf2_ros.LookupException, tf2_ros.ExtrapolationException) as e:
rospy.logwarn("Transform lookup failed, retrying... %s", str(e))
rospy.sleep(0.1)
raise Exception(f"Failed to find transform from {source_frame} to {target_frame}")OR another alternative
#!/usr/bin/env python3
import rospy
from tf2_msgs.msg import TFMessage
def callback(data):
rospy.loginfo("Received /tf_static_old and republising latched /tf_static")
pub.publish(data)
if __name__ == '__main__':
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("tf_static_old", TFMessage, callback)
pub = rospy.Publisher('tf_static', TFMessage, queue_size=10, latch=True)
rospy.spin()Fix /tf_static not latched when replaying a rosbag (see https://answers.ros.org/question/207551/how-to-save-static-transforms-in-bag-files/?answer=207647#post-id-207647).
Start this script before running a rosbag, then do: rosbag play --clock tf_static:=tf_static_old
Advanced Solution: Custom tf_static Publisher
For a more robust solution, create a dedicated node that republishes all static transforms from your bag file:
#!/usr/bin/env python
import rospy
import rosbag
from tf2_msgs.msg import TFMessage
rospy.init_node('tf_static_republisher')
pub = rospy.Publisher('/tf_static', TFMessage, queue_size=10, latch=True)
bag = rosbag.Bag('my_dataset.bag')
static_transforms = None
for topic, msg, t in bag.read_messages(topics=['/tf_static']):
static_transforms = msg
break
bag.close()
if static_transforms:
pub.publish(static_transforms)
rospy.loginfo("Published static transforms")
else:
rospy.logerr("No static transforms found in bag file")
rospy.spin()Conclusion
Understanding these time-related issues when working with ROS bag files can save hours of debugging. By properly configuring playback parameters, ensuring nodes use simulated time, and handling static transforms correctly, you can create more robust systems that reliably replay recorded data.
Remember that small details like the order of operations and proper parameter configuration can make a big difference when working with ROS time. Take the time to understand these concepts thoroughly, and your robotics development will be much smoother.