fix mesh map stamp when using sim time

[Cakem1x]

Workaround that fixes mesh map display in rviz.

Maybe we could do a cleaner check against uninitialized time in the future, when there is another way of handling unintialized Time. See ros2/rclcpp#2025.

Without this workaround, the mesh map is published with stamp zero. The tf message filter in mesh_tool's MeshDisplay rviz plugin will drop the message. This is probably due to the design docs stating that A time value of zero should be considered an error meaning that time is uninitialized.

Here are some more details:

Setup

I have

• a (move base flex) node that publishes a (static) mesh map on startup. It uses sim_time.
• something that publishes ./clock, here: gazebo
• rviz with a plugin for visualizing the map. The plugin is a display that uses tf2_ros::RVizMessageFilter to ensure the map is transformable into the right frame.

Problem

After starting up the system, the map is often not being displayed in rviz.

Analysis

The published map's header.stamp is zero.
I am relatively sure that this causes tf2_ros::RVizMessageFilter to drop the msg, hence, no map is visible.

header.stamp is set via node->now(), which is zero, because gazebo did not start up fast enough. So I end up using uninitialized time.

Workaround

My workaround consists of a sleep-loop that waits until node->now() is not zero anymore, before publishing the map.