In the last PR, I used a workaround to avoid a segfault.
But the workaround causes chached msgs to be ignored when incoming map data has header.stamp=0. In practice, this happens almost every time when using simulation (when the map server starts at sim startup, sim_time is still 0 when the map is published).
This PR changes the lookup behavior to avoid the issue above.
Additionally, it will only find msgs with matching stamp. The latest mesh_map version ensures that stamps of geometry and costs/colors match, so this should be fine.
This PR fixes caching for simulation envs.
In the last PR, I used a workaround to avoid a segfault. But the workaround causes chached msgs to be ignored when incoming map data has header.stamp=0. In practice, this happens almost every time when using simulation (when the map server starts at sim startup, sim_time is still 0 when the map is published).
This PR changes the lookup behavior to avoid the issue above. Additionally, it will only find msgs with matching stamp. The latest mesh_map version ensures that stamps of geometry and costs/colors match, so this should be fine.