Potential fix for wildlife encounter animal poses

[alexperez33]

(Issue #420)
This converts the animal xyz coordinates to a global Cartesian frame before calling the SphericalFromLocal function in wildlife_scoring_plugin.cc. I am unsure exactly why this works, but I properly receive the spherical coordinates from the vrx/wildlife/animals/poses topic now.

I do notice that the function I added that converts the xyz coordinates to a global Cartesian frame (GlobalFromLocal) simply rotates the position of the animals about the Gazebo origin frame (both x and y are negated).

[caguero]

Thanks for the contribution. I just added two minor comments because the linter complains.

[caguero]

Could you split this line into two lines? The linter complains because it's > 80 chars.

[alexperez33]

No problem!

[caguero]

Could you split this line into two lines? The linter complains because it's > 80 chars.

[alexperez33]

No problem!

[j-herman]

@caguero I've been digging into the SphericalCoordinates source code this afternoon to try to figure out this issue.

Our world frame (which should be CoordinateType LOCAL) is ENU, and thus coincident with GLOBAL. I validated this with a call to HeadingOffset() that returned 0, just to be sure. So, in theory, the call to GlobalFromLocal() shouldn't do anything at all. In fact, though, conversions from LOCAL to anything else swap the signs on the X and Y inputs. This happens in both the PositionTransform() and VelocityTransform() methods.
I also validated that the sign swap is what's happening in our wildlife worlds - can provide code snippet if desired.

Why this fix appears to work: in the call to GlobalFromLocal(), the signs are swapped. Then in the call to SphericalFromLocal(), they're swapped back.

Edit... Found the root cause! This is a known issue. It would be helpful to have something included in the class documentation to point this out to future users. See gazebosim/gazebo-classic#2022 for details. I'll put together an update to include the fix.

[j-herman]

Just as a clear demonstration of the error, here's some output from the default wayfinding world. We enter the waypoints as lat/long, and convert via LocalFromSpherical(), which works correctly and places the marker where we expect. I added the reverse conversion to show what the SphericalFromLocal() function returns - you can see the differences. LocalFromSpherical assumes ENU, while SphericalFromLocal is reversed in X,Y.

[Msg] Waypoint, Spherical: Lat = -33.7227 Lon = 150.674
[Msg] Waypoint, Local: X = 525.794 Y = -171.5 Yaw = 1.21756
[Msg] Waypoint, converted from local back to spherical: -33.7258 150.685 1.24152

[Msg] Waypoint, Spherical: Lat = -33.7221 Lon = 150.674
[Msg] Waypoint, Local: X = 550.984 Y = -237.315 Yaw = 1
[Msg] Waypoint, converted from local back to spherical: -33.7264 150.686 1.02821

[Msg] Waypoint, Spherical: Lat = -33.7226 Lon = 150.675
[Msg] Waypoint, Local: X = 434.679 Y = -179.863 Yaw = 1
[Msg] Waypoint, converted from local back to spherical: -33.7258 150.684 1.01734

[j-herman]

This is an issue in the Stationkeeping scoring plugin as well, but only in the case where the goal pose is entered in cartesian coords rather than lat/long, which is probably why we haven't seen it in practice.
For style discussion before making any recommendations on this pull request, I've got proposed changes in a new branch: jessica/spherical_fix

[caguero]

@alexperez33 , @j-herman, I'm going to close this pull request in favor of #428 , as I think it properly handles this issue. Thanks a lot for the useful discussion here.