Sync input & output timestamps (for e.g. /scan and /map)

[sirawats]

Feature description

Sync input & output timestamps (/scan and /map)

Implementation considerations

Offline slam tuning by ros2 bag play is publishing /tf and all topics data in old timestamps when SlamToolbox is going to publish present timestamps by this->now(), this caused TF tree discontinuous and rviz2 can't visualize /map --> /odom

[SteveMacenski]

You shouldn't have an issue if you have use_sim_time set to true for the slam toolbox node, then now() would use the ROS clock rather than the system clock. Can you try that first? That should completely solve your concerns.

While your suggestion seems very rational, I think it could cause some serious issues downstream for users (hence why I'd like you to try simulation time first). The SLAM system in ROS needs to "lead" the system clock by a little bit in order for no TF transform into future issues to come up. By post dating this, depending on the update rate of the SLAM system and the lidar itself, this could introduce transient issues where a valid TF tree couldn't be found in a real system at run-time if transform_tolerance isn't set to something reasonably large (like 1s+).

[sirawats]

I have tried set use_sim_time:=true it still doesn't work, I think because there is no ROS clock publishing. And rosbag2 hasn't had clock/simtime feature yet according to ros2/rosbag2#99 and
ros2/rosbag2#675

In my opinion, that is the purpose of transform_tolerance existence. It shouldn't be any issue if config parameters is reasonably correct (lidar update rate match transform_tolerance). But I agree with you, it might be issues downstream for users.

[SteveMacenski]

This seems like a rosbag issue then that should be resolved there.

I need to do a little looking into other localization systems in ros to see what best practices are. The major issue I see is if the robot stops, then this wouldnt trigger an update (didnt move or rotate enough) and if it stops for >10s or something, the downstream things using TF would all start to throw a ton of errors since the tf tree is old. Assuming the system is constantly moving isnt representative of most robotics usecases. It looks like you may just take all input scans timestamps but thats deceptive, if you do something like that, it should only be on processed scans.

[sirawats]

As I had a quick look at navigaion2 AMCL they also use laser_scan->header.stamp link1 link2

If robot stops but lidar is still continue publishing, there should be no issue because TF timestamp is equal latest scan timestamp. (If use processesd scan timestamp, it might be)

[SteveMacenski]

OK - I reviewed the PR and just need some small updates