Fix frame jitter by splitting map and fixed-frame transforms

[beetleskin]

Fixes #56:
The jitter exists if the frequency of the NavSatFix callback is lower than RViz' framerate and the transform update rate. This is usually the case, GPS often comes with only 1Hz, while RViz" fps are usually 30-60Hz, localization transforms have usually a high frequency (>50Hz).
It origins from unsynchronized transforms: The scene node's pose is only updated on callback, but in-between callbacks, the robot might move. I.e.: the map-repaint lags behind.
In order to synchronize this, we need to transform the scene-node on every render-frame with the latest information, as usual with RViz Display plugins rendering data that is attached to a tf-frame.

So we split the transform from the tile map to the fixed-frame into two transforms, created at two locations:

• transform the tile-map from the NavSatFix tf frame into the map frame (rationale: the tiles are rigidly attached to the world by the Mercator projection, this relationship is not subject to change, independent on the robots pose)
• transform the tile-map from the map-frame into the fixed frame with every render-callback

Along with this change, we:

• changed the sequence of events for updating texture and geometry
• improved validation and actions taken when updating properties
• did a minor general cleanup
• updated the demo.launch to reflect a more comlex scenario (base_link and map are not identical any more; this enables better debugging of the transforms)

TODO:

• squash commits

Test Plan

• confirm that all the properties and their error handlers are working as before
• confirm that there is no jitter left independent of the fixed frame set:
  • set RViz' fixed frame to a robot-fixed frame, camera target frame to map
  • move the robot fast
  • set a low update rate of the gps_fix topic (1Hz)
  • set a high update rate of the robot's tf (100Hz)
• check that status is updated correctly
• check error reporting (missing tfs, missing gps_fix)
• check that we don't have new issues
• check that the map is positioned exactly as before (no offset/rotation)
• confirm that the transforms are working as expected

[schra]

Massive props, this both makes it much more readable and fixes the annoying jitter bug :)

I know you said that the you want to work on the description but I'll give you just a few thoughts on this already:

• The name of this PR and the commit should also contain that it fixes the map jittering
• The commit message should contains some insights what the fix of the jittering was, which is 1. we had to lookup the transforms in update() because otherwise the map "lags behind" when the nav sat fix frequency is low and 2. some lookups have to be done with Ros::Time() and not the timestamp of the nav sat fix message because we want to query the current position of the robot and not an old one.
• In the source file there should be a summary of what is happening. For example you put something like this at the top of the source file: "The navSatFixCallback calls the updateCenterTile function, which then caches the center tile (as an attribute) and calls transformTileToMapFrame. On each frame, update() is called, which calls transformMapTileToFixedFrame, which calculates t_map_center_tile which is (exclusively) used in transformMapTileToFixedFrame."

Btw please don't force push and just create new commits when updating this PR, so it's easier to review in GitHub. In the end we will squash-merge it.

Also /cc @m-naumann and @RonaldEnsing you might be interested in this fix

[schra]

I tested your PR finally. Here is what I did (I changed your test plan a little bit):

• confirm that all the properties and their statuses are updated accordingly
  • I found that if you just put something like "sdf" in the object uri input, that there will be no error visible in the GUI. However, this is probably not from this PR and also a minor issue anyway
  • When clearing the topic input, the error will be "No map received yet" - maybe we should change that to "No NavSatFix message received yet"?
• confirm that there is no jitter left independent of the fixed frame set:

  1. set RViz' fixed frame to a robot-fixed frame
  2. set Rviz' camera target frame to map
  3. move the robot fast
  4. set a low update rate of the gps_fix topic (1Hz)
  5. set a high update rate of the robot's tf (100Hz)

• check error reporting

  • missing tfs
  • missing gps_fix

  • When you set an invalid ff, for example "asdfasdfsf", then the map will jump. (It will not jitter, but jump). The status shows that there is an error, but the map is still displayed. I think we should hide the map if the ff is invalid?
• check that we don't have new issues
• check that the map is positioned exactly as before (no offset/rotation)
• confirm that the transforms are working as expected
• demo works

[beetleskin]

I tested your PR finally. Here is what I did (I changed your test plan a little bit):

* [x]  confirm that all the properties and their statuses are updated accordingly
  
  * I found that if you just put something like "sdf" in the object uri input, that there will be no error visible in the GUI. However, this is probably not from this PR and also a minor issue anyway

There is a specific error in the console. But feel free to open up a ticket :)

  * When clearing the topic input, the error will be "No map received yet" - maybe we should change that to "No NavSatFix message received yet"?

Lets do that in #79.

* [x]  confirm that there is no jitter left independent of the fixed frame set:
  > 1. set RViz' fixed frame to a robot-fixed frame
  > 2. set Rviz' camera target frame to map
  > 3. move the robot fast
  > 4. set a low update rate of the gps_fix topic (1Hz)
  > 5. set a high update rate of the robot's tf (100Hz)

* [x]  check error reporting
  > * missing tfs
  > * missing gps_fix
  
  
  
  * When you set an invalid ff, for example "asdfasdfsf", then the map will jump. (It will not jitter, but jump). The status shows that there is an error, but the map is still displayed. I think we should hide the map if the ff is invalid?

Its no regression, is it? If not, lets do it in #51

[beetleskin]

anything left to do?

[schra]

LGTM

[beetleskin]

\o/

would you do a release? major or minor?

[schra]

I'll do a release after we merged the two pending PRs #79 and #77 if that's ok with you. I think it should be a major release due to the "Remove NED and NWU frame conversion options" commit

[beetleskin]

ack