imaging sonar plugin frame issues

[lauralindzey]

I'm seeing two different issues with the frames in the Gazebo plugin:

1. If I attach the sensor to a link where the axes are [forward, right, down] (as specified in the definition of acoustic_msgs/SonarImage), the sensor_msgs/Image that is published will have the correct field of view but the image will be flipped left-right when viewed in image_view.

2. If we try to visualize the published pointcloud, it has the correct geometry for the sensor's field of view, but has been pitched down by 90 and then rolled by 90.

[woensug-choi]

Thank you for the report! Could you reproduce these at a sonar tank set-up?

[lauralindzey]

I tried to do so using roslaunch nps_uw_multibeam_sonar sonar_tank_oculus_m1200d_nps_multibeam.launch

However, it doesn't publish /tf, which is required to visualize the sensor data in rviz, which is how I'm seeing that it's inconsistent. Do you have a test environment with the imaging sonar attached to a robot and a launch file running robot_state_publisher?

[woensug-choi]

I've been trying to debug this and make an example case of a robot with multibeam sonar. But without success.

I tried to design a test case by benchmarking how we did it with the DVL sensor. I've made a urdf xacro file to describe the multibeam sonar sensor and attached it in the robot xacro file before default_joint_stat_publisher is called. But I get errors on Rviz when visualizing the point cloud data.

I could really use some advice or help on how to tackle this problem. @bsb808 @dtdavi1 @crvogt

Quickstart

• multibeam_sonar_test branch on dave : https://github.com/Field-Robotics-Lab/DAVE/tree/multibeam_sonar_test
• urdf_xacro branch on multibeam sonar : https://github.com/Field-Robotics-Lab/nps_uw_multibeam_sonar/tree/urdf_xacro

roslaunch uuv_dave uuv_dave_multibeam_sonar.launch

[crvogt]

@woensug-choi Investigating...

[crvogt]

@woensug-choi FYI I'm just making a standalone MBS example and from there figure out what's going on when added to the robot model :)

[crvogt]

@woensug-choi I noticed that by using the MBS plugin alone the point cloud in RVIZ reflects what @lauralindzey mentioned. I added a static_transform_publisher to the launch file which loaded the sonar and initial views looked fine. Next step is attach it to a robot/modify the xacro to show the same.

[woensug-choi]

@crvogt What do we need to do to close this issue? Adding static_transform_publisher to the launch files?

[crvogt]

@woensug-choi Sorry, just saw this. I'll make a PR for this.

[lauralindzey]

bump =)

We have another project that we're ramping up with the simulator, and this issue just came up again. Ideally, the fix would be for the sensor used to generate the image_raw/image_depth data to be properly aligned, but I think a hacky solution could be to just switch the order in which beams are added to the message. (ln 843 of gazebo_ros_multibeam_sonar.cpp)

I'd be happy to hop on a Zoom if that would be useful for debugging.

[crvogt]

@lauralindzey @woensug-choi Thanks, bump noted. Have you tried the static_transform_publisher? I'll double check, but I thought that adding that to my launch file was enough to fix the orientation.

[woensug-choi]

@lauralindzey I am not much familiar with the subject here. Could you try the solution @crvogt suggests? If it did not work, let's arrange a meeting :)

Example :
<node pkg="tf" type="static_transform_publisher" name="multibeam_sonar_base_link" args="0 0 0 0 0 0 map forward_sonar_optical_link 50" />