Add UTM frame mode

[peci1]

Depends on #104.

I know there have already been discussions whether explicit UTM frame is something that should be supported or not. I tried to understand the conclusions, but did not succeed fully. I'll try to explain our use-case, which should hopefully be the justification for this feature.

Our mobile robots are expected to drive through any kind of environment, so we cannot rely on good GPS reception. It happens frequently that we have fix with covariances around 100 (but we often also have the good 1e-4 ones). We run odometry fusion based on robot_localization package and a factor graph approach that puts the dead-reckoning odometry and absolute GNSS measurements into a graph, which is then optimized to yield the best available absolute pose estimate.

It seems to me this setup is not compatible with the current rviz_satellite philosophy. If I understood handling of the map frame correctly, each fix message's lat/lon is transformed to map frame using its TF frame, and the tiles are positioned based on this composite latlon-TF transform. However, in our case, the fix messages do not carry precise location information - they are the noisy raw data. So the latlon information they carry is not consistent with the optimized TF gps->map. When the robot (actually, the fix message) enters a new tile, the composite latlon-TF transform is evaluated and whatever is there is used to place all the tiles. This results in large inaccuracies in the tile positioning.

In our system, we explicitly model the utm frame in TF (robot_localization has an easy-to-configure option for it). So we publish the UTM waypoints in rviz and visualize them. This serves as the "ground truth" for evaluating whether the tiles are placed correctly. I've added a new mode to this plugin that allows to utilize this explicit utm frame and makes it possible to place the tiles directly in the UTM coordinates, rather than passing them through the imprecise pose estimation. The biggest benefit I see in using this mode is that I can be 100% sure the tiles are positioned correctly (up to the small jumps reported in #92 which I also see in the UTM mode, which might even be just rounding errors), so it is easier to visually evaluate our odometry estimates. And the quality of the estimates has no influence on the tile placement. The only thing I don't like is that tile selection is still done based on the fix messages and not using some TF frame (i.e. base_link position in UTM), but that would overcomplicate things, and as long as the fix stays at least roughly correct, it works.

The default behavior of the plugin stays as it was before (the "Via map frame" option is default).

I'm open to any suggestions on how to improve this PR. I'm not 100% sure with all the update functions. Maybe there is better wording for the newly introduced options and their documentation.

[Timple]

I didn't look at the changes yet, but I wanted to ask: navsat_transform from the robot_localization package also has the ability to republish the corrected (filtered) gps signal. We use this signal to feed rviz_satellite. It's basically the output of the filter but in GPS coordinates. Would this already be a solution as well?

[peci1]

Yes, that would be a solution. However, publishing the corrected GPS is a kind of hack in my view, and is not supported out of the box by all localization pipelines. The UTM mode added in this PR would work in any case.

Side note: if rviz_satellite only works with the corrected GPS, shouldn't this requirement be written in the readme?

[Timple]

Sorry, I hope to get to a code-review soon. But some small remarks:

publishing the corrected GPS is a kind of hack

I disagree. This is wat 'modern' gps systems do anyway. The take the satellites and fuse the imu. You're doing a better job: take the satellites and fuse some slam algorithm. Basically you're 'just' reporting your position in degrees regardless of sensor-origin.

if rviz_satellite only works with the corrected GPS, shouldn't this requirement be written in the readme?

This is certainly not a requirement. It works with uncorrected GPS just as well. But then it should be available of course.

---

Nevertheless this can be an improvement. We kind of inherited this package and I found the lat/lon requirement always a bit un-ros. Because we have tfs!

[peci1]

This is certainly not a requirement. It works with uncorrected GPS just as well. But then it should be available of course.

Am I doing something wrong then?

https://user-images.githubusercontent.com/182533/195619889-b40e0369-ccb1-45bc-bcbc-7bad21f5ed34.mp4

Nevertheless this can be an improvement. We kind of inherited this package and I found the lat/lon requirement always a bit un-ros. Because we have tfs!

Getting there! See #106 .

[Timple]

I didn't dive into the code yet. But from what I read I'd say that hasProblems and the call to tranform should use the same time regardless of the outcome of this discussion.

I think ros::Time::now() is technically more correct. But since this is visualization only I would be good with latest.

[alspitz]

@peci1 I have tested using ros::Time() in Map Frame mode and it works as expected in my use case. I'm not too certain about the frames to be honest, just that the Map Frame I want to use is a fixed north-up frame and the rviz fixed frame is an odom frame with an arbitrary offset to the map frame. The problem arose for me when I tried to use a Fixed Frame != the Map Frame. In that case, the transform would fail because any available transform between the two frames will be from a time < now(). When the fixed frame is the same as the map frame, I suppose it works because no transform is needed.

I'm actually not sure how it's supposed to work with ros::Time::now(), since it seems very unlikely the transform would be available immediately.

[peci1]

I've tested with ros::Time() in "UTM mode" and there's no noticeable difference (probably because I'm running with UTM frame as fixed frame, so the transform time is irrelevant).

So from my point of view the change would be good to go.

Although I think it is a bit of a hack. Your use-case seems to be ill-conditioned - your fixed frame odom is probably a fast, low-latency one. You can't expect the (most proably) slower and higher-latency map frame to display correctly in RViz as most of the times, no data from map frame would be transformable to your fixed frame at the time of rendering (it would require extrapolation). Usually, fixed frame should be the "slowest" frame you have. If you choose a "faster" frame, you usually get nothing for data coming in the "slower" frames. With the proposed change, what you get is a "false feeling" that everything works correctly, but actually, I can imagine using the older transform could fail in some high dynamics cases.

Maybe a better solution would be to first try to look up the correctly stamped transform, and only if it fails, fall back to ros::Time()?

[alspitz]

Unless the published transform from fixed frame to map frame is VERY fast, I don't see how using now() can succeed (unless somehow it gets published with a future timestamp?). The only alternative is if the transform is a static one published by tf_static, in which case, I believe using the 0 time would return the same result. To clarify, the fixed frame I'm using has an inherently static offset to the map frame, but that offset is estimated and published periodically. So most of the time, it doesn't seem incorrect to use the last estimate of that offset.

But I agree with a dynamic frame, it could be out of date, but then I'm not sure there's anything to do besides waiting for a new transform.

I'm not sure of how RViz handles this but maybe there is another timestamp that represents the time of the rendered view that can be used instead.

[Timple]

Static tf's do resolve on now(). And I know of methods like AMCL future-stamping their transforms to prevent these issues. Which is just as hacky, but without it, now() indeed can never be resolved.

I'm okay with using latest transform. I wouldn't implement a fallback as that doesn't change behavior for anyone I think.

[peci1]

Oh, you're both right. It seems RViz frame manager gives a special handling to Time 0:

https://github.com/ros-visualization/rviz/blob/68ce34b0c9053b3226ab16feaa09bad62d255347/src/rviz/frame_manager.cpp#L152

It seems passing a non-zero time would actually disable using the other Sync Policies (which I never understood, but nevermind).

I'm now convinced zero time is the correct one, so I'll send a PR with the fix.

[peci1]

See #114.