From 471741c8709d5385733eb1229c38c89db8cf6bac Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 20 Nov 2024 14:29:34 +0100 Subject: [PATCH] fix mesh map stamp when using sim time --- mesh_map/src/mesh_map.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 299d2ecb..0ec96ce1 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -265,7 +265,17 @@ bool MeshMap::readMap() } } - const rclcpp::Time map_stamp = node->now(); + rclcpp::Time map_stamp = node->now(); + // Workaround: Avoid publishing a map with zero timestamp. + // This can occur when using sim time and the node using mesh map starts to quickly, + // before whatever should publish /clock (e.g. gazebo) is ready. + // A map with timestamp zero will not get displayed in rviz. + // Generally, a zero timestamp is currently considered be an error / unintialized stamp. + // Issue https://github.com/ros2/rclcpp/issues/2025 might want to change that, though. + while (map_stamp.nanoseconds() == 0) { // TODO check whether time is zero + sleep(0.5); + map_stamp = node->now(); + } mesh_geometry_pub->publish(mesh_msgs_conversions::toMeshGeometryStamped(*mesh_ptr, global_frame, uuid_str, vertex_normals, map_stamp)); publishVertexColors(map_stamp);