Transform time issue (gmapping & stage)

Question

Hello,

I am running Ubuntu 20.04 (kernel 5.11.0) on a x64 AMD Dell Inspiron. I use ROS release Noetic with the following relevant package versions:

ros-noetic-simulators/focal,now 1.5.0-1focal.20211007.220413 amd64 [installed,automatic]
ros-noetic-slam-gmapping/focal,now 1.4.2-1focal.20211007.214554 amd64 [installed]
ros-noetic-stage/focal,now 4.3.0-1focal.20210423.222334 amd64 [installed,automatic]
ros-noetic-stage-ros/focal,now 1.8.0-1focal.20210922.193922 amd64 [installed,automatic]
ros-noetic-tf/focal,now 1.13.2-1focal.20210922.193350 amd64 [installed,automatic]
ros-noetic-tf-conversions/focal,now 1.13.2-1focal.20210922.204930 amd64 [installed,automatic]
ros-noetic-tf2/focal,now 0.7.5-1focal.20210423.225547 amd64 [installed,automatic]
ros-noetic-tf2-eigen/focal,now 0.7.5-1focal.20210423.225948 amd64 [installed,automatic]
ros-noetic-tf2-geometry-msgs/focal,now 0.7.5-1focal.20210922.190831 amd64 [installed,automatic]
ros-noetic-tf2-kdl/focal,now 0.7.5-1focal.20210922.190836 amd64 [installed,automatic]
ros-noetic-tf2-msgs/focal,now 0.7.5-1focal.20210423.225302 amd64 [installed,automatic]
ros-noetic-tf2-py/focal,now 0.7.5-1focal.20210922.180952 amd64 [installed,automatic]
ros-noetic-tf2-ros/focal,now 0.7.5-1focal.20210922.190247 amd64 [installed,automatic]

Here is my launch script. I am ultimately trying to use move_base, stage_ros and gmapping together. For now, nevermind my navigation stack configuration, I followed this tutorial. I am having problems with misaligned clocks between the stage simulator and gmapping. See the transform tree below:

link

Notice how the oldest transform for map -> odom by gmapping is very large (I suspect a Unix timestamp) versus every other transform published by stage which are relative to the elapsed time. This difference is causing the navigation stack to give me the following error(s):

[ WARN] [1637419863.630355767]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: Lookup would require extrapolation 1637419854.238633871s into the past. Requested time 4.700000000 but the earliest data is at time 1637419858.938633680, when looking up transform from frame [base_link] to frame [map]. canTransform returned after 0.10044 timeout was 0.1.

How do I fix the transforms published by gmapping in this case?

EDIT:

I have found a hack solution for a working-ish system by adding the following to the launch file:

<param name="/use_sim_time" value="true" />
<param name="~transform_publish_period" value="0.1" />

The first line launches a /clock server as a time reference that all nodes must use. This seems necessary since stage is using relative simulator time and gmapping is using real wall time. See this article. The second line changes how frequently gmapping publishes the map -> odom transformation. The default 0.05 caused every node to constantly print:

Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame odom at time 2.550000 according to authority unknown_publisher at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp

[ WARN] [1637431235.428923806, 2.500000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame odom at time 2.550000 according to authority unknown_publisher

Increasing to 0.1s causes only one warning in the beginning. I now have an issue with my program that I'm writing not being able to see the transform, i.e. tf.canTransform('/base_link', '/map', rospy.Time()) returns False, until a few seconds after it starts.

---

Originally posted by mxl367 on ROS Answers with karma: 11 on 2021-11-20

Post score: 1

Answer 1

I think I have solved it at last. The final launch file, ignoring the navigation stack for now, looks like this:

<launch>
    <param name="/use_sim_time" value="true"/>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mypkg)/launch/default.rviz"/>
    <node name="stage" pkg="stage_ros" type="stageros" args="--clock $(find mypkg)/data/map.world"/>

    <node name="slam" pkg="gmapping" type="slam_gmapping" args="scan:=base_scan">
        <param name="transform_publish_period" value="1"/>
    </node>
</launch>

Notice I increased the transform_publish_period to a whole second, which curiously doesn't raise any errors. Also, below is the code I mentioned earlier, which now successfully publishes the estimated pose. Some bits are unnecessary and rough (e.g. do I really need the tf_callback?) but it's a start:

#!/usr/bin/python3
import rospy
from geometry_msgs.msg import *
from nav_msgs.msg import MapMetaData, OccupancyGrid
import tf
from tf.msg import tfMessage

class Explorer(object):
    def __init__(self):
        self.tf = tf.TransformListener()
        self.tf_sub = rospy.Subscriber('/tf', tfMessage, self.tf_callback)
        self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        self.pose_pub = rospy.Publisher('/estimatedpose', PoseStamped, queue_size=1)

    def tf_callback(self, msg):
        pass

    def map_callback(self, msg):
        head = msg.header
        info = msg.info
        data = msg.data

        if self.tf.canTransform('/base_link', '/map', rospy.Time()):
            p,q = self.tf.lookupTransform('/base_link','/map',rospy.Time())
            pose = Pose(Point(*p), Quaternion(*q))
            estp = PoseStamped()
            estp.pose = pose
            estp.header.frame_id = 'map'
            print(estp)
            self.pose_pub.publish(estp)

if __name__ == '__main__':
    rospy.init_node('explore')
    node = Explorer()
    rospy.spin()

---

Originally posted by mxl367 with karma: 11 on 2021-11-20

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by osilva on 2021-11-23:
Great. You can accept your own answer by clicking on the check mark