SonarImage must be attached to a Fore-Stbd-Down frame

[amarburg]

Data in the SonarImage message is defined to be in a Fore-Starboard-Down ("down") frame where X is "forward" from the sonar, and Z is "down". In the conventional case where a forward looking sonar is installed horizontally, azimuth values comply with the nautical standard where negative azimuths are left of forward, and positive are right of forward.

The NPS sonar simulator plugin is built on top of the Gazebo depth camera plugin, which is attached to an Fore-Left-Up ("upright") frame.

In the image below, the sonar is pointed slightly to the left of the cinder block, and is attached to the upright frame shown in RViz. The image from the depth camera (lower center) is upright, with the brick to the right of center, and the sonar image (lower left, as drawn by sonar_image_proc) is also right of center.

However, the frame_id placed in the header of SonarImage data is the frame of the depth camera, which is upright. If the data is reprojected into space using this upright frame, azimuths are interpreted incorrectly with negative right of forward and positive left of forward, and the data is mirrored left to right.

This can be seen in the image below, where the sonar data has been projected as a pointcloud relative to the frame named in the SonarImage header, the upright depth camera frame. Viewed from above, the simulated cinderblock is at the origin, whereas the reprojected sonar data is mirrored left to right.

The simplest solution is to define that the plugin must be attached to an down frame. But this has the following consequences:

• The frame in the SonarImage message, which it receives from the simulator, will be consistent with the message specification
• The depth camera image will rolled by 180 deg. This doesn't affect plugin output unless the sonar is given a vertical aperture which is asymmetrical up vs down -- in which case the sense of the plus- and minus- aperture will be upside down from expected.
• The SonarImages generated by the plugin must be flipped in software left to right relative to previous behavior
• This mirroring of the SonarImage is a breaking change for existing users of the plugin with the sonar attached to an upright frame

After making these changes, the equivalent screenshot is as follows. The simulator is now attached to the fore-right-down frame shown in RViz. The sonar imagery (left) appears the same, while the depth camera image (right) is now upside down.

If we observe the sonar data reprojected into RViz, the cinder block is now correctly reprojected onto the origin:

The alternative is for later processing to explicitly ignore the frame specified in the SonarImage data and instead substitute a different, manually specified frame.

[woensug-choi]

Thank you for trying and suggesting! I totally agree with the fact that the frames should be nautical convention. I vote to have upside down depth camera image as it is not the final product of this plugin.

I don't think there is any other user who has been attaching the sonar plugin to actual vehicles yet. It seems you are the pioneer here to test things out. I would love to merge this as it is.

Any note suggestions that could be beneficial for future users to read on the wiki page?

[amarburg]

Definitely, I'll look at the wiki / docs and make a suggestion...