Real camera on simulated robot leads to tf time issue

[RobertZickler]

Hey everyone, I've never done a pull request so I describe the same change. Maybe it is completely nonsense but it solved my problem. I found an issue when using the package while a Gazebo simulation is being used. Situation: simulated robot with (real) camera attached to the end effector. Of course this situation is wired but there are probably other (more useful) situations ;) Anyway: When using Gazebo the time starts by 0. The "sim_time". But in the file apriltag_ros/src/common_functions.cpp on line 400 When sending the transformation of the detected tag

tf_pub_.sendTransform(tf::StampedTransform(tag_transform,
                                      tag_transform.stamp_,
                                      ...

the real (wall) time is used. So on the tf-tree two different "time zones" exist. Everything coming from the simulation starting by 0 and the tag-tf with wall time. Now when you do a lookup_transform from the tag-tf to one of the simulated frames you get an error:

Lookup would require extrapolation into the past. Requested time 27.337000000 but the earliest data is at time 1621412382.714671850, when looking up transform from frame [tag_tf] to frame [world]

Request time is the last time which is known by roscore (with Gazebo: sim time). But the earliest time in the network is the one published by the common function.

Solution: To get the common function to use the last time known, replace line 400 (tag_transform.stamp_,) with

ros::Time::now(),

Now the tf publisher is always using the last roscore time. No matter if this is sim time or wall time with a real robot. Then the package can be used on real and simulated projects.

Hope to help someone Robert

[rgreid]

Hi @umor3, I looked through this in detail and suggest that the approach currently used by apriltag_ros is correct. In this line, the timestamp from the source camera image is copied into the tag_detection.pose and is subsequently used for the tf where you noted. It does not make sense to stamp the tf with anything other than the time when the observation was made.

For your real+sim hybrid case, you may want to rewrite the image header stamp from your real camera to ros::Time::now() before feeding it into the apriltag_ros node. I'm not aware of any existing ROS nodes that can rewrite image headers, however it would be pretty easy to write one. Note that you'll want to rewrite the sensor_msgs/CameraInfo messages also.

Good luck, and if you find/write a node that updates the header stamp, feel free to let us all know! Cheers.

[wuhao-Tomas]

Thanks a lot for you remind sir!