Mesh Display Fixes

[Cakem1x]

Hi! This PR adds a bunch of fixes for the mesh display.

I had a few segfaults here and there when I tried to visualize a mesh map that rviz gets from the mesh_navigation_server via topics.

Changes:

• In order to understand all the different states and state transitions in the plugin, I refactored some of the methods.
• Remove a few message_filters that seemed unnecessary to me. I.e. the tf2_ros message filter only makes sense for geometry, since colors or costs do not need to get transformed into different frames - they attach to the location via vertex ids in geometry.
• Actually use the caches for color and cost msgs, this fixes the (ROS1) issue that colors need re-subscription to display
  • This happened when the colors arrive before the geometry. This still can happen, but via the cache mechanism, we can apply colors when geometry arrives
• Fix segfault when loading Mesh Display from config, somehow caused by the Transformer changing (see SIGNAL(transformerChanged(std::shared_ptr<rviz_common::transformation::FrameTransformer>))). I looked this up someplace deep in the rviz repo. This was quite a pain to find out... Segfault was someplace deep in the tf2 message filter or std::tuple :/
• Use latched QoS in subscribers

Related vid https://drive.google.com/file/d/1VSo_PGoJJM1n4maeS5M8u6g1W44vbPf9/view?usp=sharing

[amock]

Great change!

I tested everything on my computer. It is compiling. Loading static maps from files is working as before. So I see no reason to wait for merging this PR. However, what I wish to have in the future:

• Examples for generating and publishing different types of meshes
• Minimal examples for visualizing meshes both from a file and a topic

So that we can better test that new PRs are not causing runtime errors. If we have that, we can also think of integrate them into CI, or write separate tests. But this is an issue for another time.

-- Alex