Some Rviz fixes
Some issues with Rviz:
Rviz visuals are a bit laggy:
The pointcloud visual not showing
Frames show up sometimes, sometimes they don't.
Rviz "Nav2 goal" doesn't send a frame_id with the message it publishes on /NavigateToPose
Message should look something like:
"{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0, z: 10.0}}}}"
The frame_id should always be 'map' and should probably be fixed in navigation2/nav2_rviz_plugins/src/nav2_panel.cpp