Re: RTAB-Map with only scan and IMU odometry

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/RTAB-Map-with-only-scan-and-IMU-odometry-tp6592p6614.html

Hi,

There is an issue with tf_static and rosbags. The tf_statiic topic is not latched, and published only one time at the beginning of the bag. If other nodes needing that transform are not started before the bag is played, they won't be able to use frames in it.

Workarounds:
1) Start all other nodes before the bag. This may not work for some nodes.
2) Republish tf_static into a latched topic. To do that, we can create a script like this, called republish_tf_static.py:

#!/usr/bin/env python
import rospy
from tf2_msgs.msg import TFMessage
        
msg=TFMessage()
        
def callback(data):
    global msg
    if len(msg.transforms) == 0:
        msg = data
    else:
        msg.transforms = msg.transforms+ data.transforms
    rospy.loginfo("Received /tf_static_old and republishing latched /tf_static")
    pub.publish(msg)

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()

Usage:

$ roscore
$ rosparam set use_sim_time true
$ python republish_tf_static.py
$ rosbag play --clock recorded.bag tf_static:=tf_static_old
...
every nodes launched after this point will see correctly the new tf_static latched.

cheers,
Mathieu