Adding publishing of an REP-105 compliant ECEF transform

[JayHerpin]

The navsat_transform_node can now publish an ecef earth frame.

This functionality can be enabled by setting 'broadcast_earth_transform' to true. The frame ID used for the earth transform can be customized using the earth_frame_id parameter.

Note, that publishing the earth transform is only valid if use_local_cartesian is set to true.

[JayHerpin]

I should disclosed that AI was used in the creation of this patch... although, frankly, I don't know how much credit I want to give it; it did a pretty bad job at figuring out the transforms and I had to help it a lot.

[JayHerpin]

Also, as far as testing, I actually did the work on Jazzy and tested there. If/when this review is accepted, I will also put up for review the push to the jazzy and kilted branches.

[ayrton04]

This looks really great, thank you! I haven't had the cycles to validate the math, so I'm going to have to trust that it's functional. Assuming this works, I'd be a big fan of deprecating anything related to UTM.

[ayrton04]

Instead of making this method void, can we make it return a transform? You could even then make the method static, which should simplify testing.

earth_cartesian_transform_ = computeEarthToCartesian(origin_latitude_, origin_longitude_, origin_altitude_);

[JayHerpin]

made this change, and added a few trivial unit tests to exercise it

[peci1]

This is certainly an interesting addition. I still think ECEF is a bit underused in ROS and I'd like to see it used more!

However, it rather seems to me the overall approach to TF "outputs" of navsat_transform node could be changed. Currently (without this PR), there are options broadcast_cartesian_transform, broadcast_cartesian_transform_as_parent_frame and use_local_cartesian which all enable or disable certain TFs.

Adding another parameter that even limits the possible values of some of these seems to be increasing confusion.

I suggest restructuring this to something like this list of parameters:

• broadcast_utm_to_world_transform
• broadcast_world_to_utm_transform
• broadcast_local_cartesian_to_world_transform
• broadcast_world_to_local_cartesian_transform
• broadcast_ecef_to_world_transform
• broadcast_world_to_ecef_transform
• (possibly) broadcast_utm_to_local_cartesian_transform
• (possibly) broadcast_local_cartesian_to_utm_transform
• (possibly) also TFs ecef<->utm, ecef<->local_cartesian
• (no longer needed) use_local_cartesian (LLtoMap would be computed via ECEF)

Of course, from each pair of inverse parameters, only one could be set.

Many of these conversions are implemented in https://github.com/gazebosim/gz-math/blob/gz-math9/include/gz/math/SphericalCoordinates.hh . With the addition of gz_math_vendor to Jazzy+, this could simplify some of these computations. UTM is not covered, however.

---

As a side note, when digging into this, I always had this idea of having a many-frames UTM representation. So the UTM frame published by this node could actually be named utm_33q or similar. Then, there could be a very nice chain of transforms: ecef->utm_XY->enu(local cartesian referenced to North)->world->odom->base_link. At first sight, this would create a multi-parent graph in case the robot changes zone, but if the utm_XY->world transform were dynamic instead of static, it could be rather treated as a reparenting operation rather than multiparenting. And for cases when the robot never leaves its zone, the TF could easily be static. For this to work well, I think it'd be needed to use both latitude and longitude zones, otherwise the approximation error would be too large. To detect whether the user wants the UTM frame named utm or utm_XY, there could be a simple string-template parameter that would be evaluated. It would default to utm, but it would also allow e.g. utm_{lat_zone}_{lon_zone} or even my_weird_{lon_zone}_utm and similar. The other frames' names could also be configurable like this, but with no template parameters (local cartesian, ECEF).

[Timple]

That looks well thought through!

Ideally we can choose to force a UTM frame instead of reparenting. For UTM it's ok to transform if you're a few kilometers over a UTM border. A lot of robotic applications run the risk of operating across a border. But most will be within some kilometer range.

In that case it's easier to stick to one UTM frame instead of flipping everytime you cross the border.

[peci1]

I definitely don't suggest disabling sticky UTM zone. It's very practical and should still be configurable.