New ray-based multibeam sonar implementation

[woensug-choi]

Progress

• Plugin 'shell' using the Depth camera @dtdavi1
• Sonar Beam/ray processing implementation @woensug-choi

To-dos (in priority order)

• Parallelization with NVIDIA GPU CUDA cores @woensug-choi
• Fix IFFT and optimization @woensug-choi
• Add simpler point scattering model and compare @woensug-choi
• ROS publisher with sonar data format (Issue link) @bsb808
• Data visualization with RVIZ @dtdavi1 @woensug-choi

Notes on Sonar Beam/ray processing implementation

• Currently, a relatively complex point scattering model is implemented
  • 92 seconds to calculate the model for 512 Beams (114 rays in elevation direction each, Blueview P900)
• IFFT not working correctly (Simple library header found on Github was a fail)
  • It seems the NVIDIA CUDA library has an FFT code of its own.
  • It still has the potential to slow the simulator down considering 10 Hz refresh rates
  • Fast external library should be considered (e.g. Intel MKL, fftw, NVIDIA Cuda FFT, etc.)
• Results obtained using MATLAB to IFFT the results
  

[dtdavi1]

@woensug-choi I'm getting a link error when I try to run everything. I've installed CUDA (cuda, cuda-nvcc-11-1, and nvidia-cuda-toolkit), and everything compiles fine with catkin_make. When I try to run the sonar_tank_blueview_p900.launch example, I'm getting a "undefined symbol: cufftPlan1d" error. Is something missing from CMakeLists.txt or the launch file I'm using?

[woensug-choi]

Core part of the sonar calculation is done! 👍

• Features
  • Physical sonar beam/ray calculation
  • NVIDIA CUDA core GPU parallelization

Progress

• Plugin 'shell' using the Depth camera @dtdavi1
• Sonar Beam/ray processing implementation @woensug-choi

To-dos (in priority order)

• Parallelization with NVIDIA GPU CUDA cores @woensug-choi
• Fix IFFT and optimization @woensug-choi
• Add simpler point scattering model and compare @woensug-choi
• Prallelize and optimize ray summation @woensug-choi
• ROS publisher with sonar data format (Issue link) @bsb808
• Data visualization with RVIZ @dtdavi1 @woensug-choi

1. Plugin diagram

2. How-to install and run

2.1. Install CUDA

Option 1. Install CUDA on the host machine (I installed latest version CUDA 11 download link)

Option 2. Use docker file located at nps_uw_sensor_gazebo/docker which includes CUDA installation (reference). The docker file at this PR builds nps_uw_sensors_nvidia_cuda docker image instead of dave_nvidia from dave project.

• build the docker image

cd ~/uuv_ws/src/nps_uw_sensors_gazebo/docker/
./build.bash .

• Run the container

cd ~/uuv_ws/src/nps_uw_sensors_gazebo/docker/
./run.bash nps_uw_sensors_nvidia_cuda
cd ~/uuv_ws
catkin_make
source devel/setup.bash

• Join the container from another terminal

cd ~/uuv_ws/src/nps_uw_sensors_gazebo/docker/
./join.bash nps_uw_sensors_nvidia_cuda
source ~/uuv_ws/devel/setup.bash

2.2. Quickstart

• Run with follwoing command to test

roslaunch nps_uw_sensors_gazebo sonar_tank_blueview_p900.launch verbose:=true

• Raw data logs are saved at /tmp/SonarRawData_000001.csv every 5 frames at default. You can turn it off or change the saving rate at models/blueeview_p900/model.sdf using bool <writeLog> and int <writeFrameInterval>.

Benchmark

• Computation time (Using Laptop with Docker installation on Linux, Intel i5, GeForce GTX 1050 Ti Mobile)
  • CPU linear (w/o FFT) : about 200 seconds/frame
  • GPU CUDA (w FFT) : about 1.5 seconds/frame
    • Ray calculation : about 0.7 seconds/frame
    • Ray summation : about 0.85 seconds/frame
    • FFT calculation : about 0.04 seconds/frame
  • Raw data write time : about 6 seconds/frame (Writing about 125 MB csv file)
• GPU RAM usage
  • With 4GB RAM on laptop, the CUDA exits with error not enough ram. Every other rays are calculated to fit in the limit of the RAM size. Skipping Beam/Ray calculations can be configured at model.sdf with int <beamSkips> and int <raySkips>.
• Results
  • Image drawn using jet colormap of the MATLAB (the script is in nps_uw_sensors_gazebo/scrips/plotRawData.m with right path for raw csv file)
  • Taking too much time to summate rays. It seems there are ways to use GPU for summation too. Will try that soon. If done with some more optimizations, the current plugin is expected to have about 2 Hz refresh rate.
    

[woensug-choi]

@woensug-choi I'm getting a link error when I try to run everything. I've installed CUDA (cuda, cuda-nvcc-11-1, and nvidia-cuda-toolkit), and everything compiles fine with catkin_make. When I try to run the sonar_tank_blueview_p900.launch example, I'm getting a "undefined symbol: cufftPlan1d" error. Is something missing from CMakeLists.txt or the launch file I'm using?

I had missing library at CMakeLists. I've cleaned everything up again and tested on clean Docker environments. Could you try it again?

[dtdavi1]

@woensug-choi Everything starts up ok now, but as soon as I move the sonar I get the following error and plugin stops.

Kernel Launch Failed
File: /home/dtdavis/ACS/acs_ros_ws/src/Field-Robotics-Lab/nps_uw_sensors_gazebo/src/sonar_calculation_cuda.cu
Line Number: 312
Reason: an illegal memory access was encountered

It's entirely likely that I installed some of the CUDA libraries incorrectly or have the wrong versions. Any suggestions?

[woensug-choi]

@dtdavi1 @seankrag @crvogt
I've reset the commits on this branch. Use this branch (gpu_sonar_re) only from now on 👍
At the rqt_image_view of the final sonar image, set range 0.25 and colormap 'hot' to obtain the image below.

@dtdavi1 Thank you for the comment on GitHub soft reset. Made soft reset and force push on this branch.

@seankrag It looks promising! just needs some iterations to match the format. Can you try it again now?
I've put sizeof(float) * nFreq * nBeams for data_size. Is their field has just int 1?, Left elevation array blank.
Linzy has shared a new repository for SonarImage.msg. What we had was the Gitlab one while the new shared repository is in Github (https://github.com/apl-ocean-engineering/acoustic_msgs). Do we need to change to Github one?

@crvogt Were you able to run it? Can you try it again? Also, it would be great if you can add some missing instructions on installation at https://github.com/Field-Robotics-Lab/dave/wiki/Multibeam-Forward-Looking-Sonar#Requirements

[crvogt]

@woensug-choi I've managed to get it running. The quickstart launch command is eating a lot of RAM for me (>10GB per min).

Commenting out <node name="rqt_images" pkg="rqt_gui" type="rqt_gui" output="screen" args = "--perspective-file $(find nps_uw_sensors_gazebo)/config/sonar_tank_blueview_p900_images.perspective" /> and RAM usage is normal.

Is there an argument I need to pass or is it collecting data in a rosbag?

[seankrag]

Linzy has shared a new repository for SonarImage.msg. What we had was the Gitlab one while the new shared repository is in Github (https://github.com/apl-ocean-engineering/acoustic_msgs). Do we need to change to Github one?

@woensug-choi , the message format is identical. I suggest we hold off on switching to the Github repository for now until I finish testing the viewer.

[woensug-choi]

@woensug-choi I've managed to get it running. The quickstart launch command is eating a lot of RAM for me (>10GB per min).

Commenting out <node name="rqt_images" pkg="rqt_gui" type="rqt_gui" output="screen" args = "--perspective-file $(find nps_uw_sensors_gazebo)/config/sonar_tank_blueview_p900_images.perspective" /> and RAM usage is normal.

Is there an argument I need to pass or is it collecting data in a rosbag?

I am not sure what could manage those large memory usages. But in practical use, the line you've commented out should be commented out. (sonar_tank_blueview_p900_images.perspective is showing real_image, depth_image, normal_image for debugging purposes). Commenting out would not hurt generating the sonar image. I will comment them out before the merge and leave notes on wikis how to enable them.

[seankrag]

@woensug-choi , I made two small changes to gazebo_ros_image_sonar.cpp for publishing SonarImage msgs:

1. set data_size=1 for consistency with uw-apl bag files; this field is the size of each datum in bytes
2. exchanged the order of for loops used to populate the intensities array in azimuth-major order

This allows me to run the uw-apl viewer, which converts an imaging_sonar_msgs/SonarImage into a sensor_msgs/Image for display by ROS image_view. I still need to investigate whether we can fine-tune the uw-apl viewer, but your OpenCV code currently produces a higher-resolution image. See comparison below.
sonar_image:

sonar_image_raw rendered by viewer:

[woensug-choi]

@seankrag Glad to see it showing the image! Thanks! Are there any methods to modify the color scale max/min?

@crvogt This looks like something we could reference! https://github.com/apl-ocean-engineering/libdraw_sonar/blob/c526cc2bde57cae0e598eaf891680d5a4fcd3e67/lib/DrawSonar.cpp

[crvogt]

@crvogt This looks like something we could reference! https://github.com/apl-ocean-engineering/libdraw_sonar/blob/c526cc2bde57cae0e598eaf891680d5a4fcd3e67/lib/DrawSonar.cpp

Haven't been able to get a sonar image at all yet. To even publish on the sonar_image topic I guess the code is looking for a subscriber to the image_depth topic before it begins to publish.

However, when I subscribe to image_depth, that's when the RAM draw goes way up (writing to /tmp? I'll check). though the sonar_image message is finally published. However, I'm not seeing an actual image yet, just getting a black screen.

EDIT: Saw your note about setting to "Hot" and "0.25m". Getting an image now! (though, still with the RAM draw)

[seankrag]

@seankrag Glad to see it showing the image! Thanks! Are there any methods to modify the color scale max/min?

@woensug-choi I will check, but much of the viewer code settings appear baked-in. Separately, I followed up with Laura Lindzey, who commented on the difference between our 32-bit OpenCV image and the APL:UW viewer's 8-bit image (it can also support16-bit images). She is happy to extend their viewer to 32-bit images if we like, but all of their sonar produce only 8- or 16-bit intensities (and she wasn't aware of actual sonars that produce higher levels of precision). We already cast our data into 8-bit values for the SonarImage msg, so this may really just a design/philosophical question: @bsb808 Do we need/want the high-precision currently used by our sonar model? Could we realize performance benefits by using 16-bit instead of 32-bit?

[woensug-choi]

Introducing new bundled viewer included in this PR (cv mapping fixed)

• Only in monocolor
• Color scalable with <plotScaler>1</plotScaler> at model.sdf

---

@crvogt The reference I've found was pretty straight forward to implement! Thank you for noticing the RAM usage! I was using a machine with 64GB RAM... I've found some not freeing memory allocation on Cuda calculation! Now it should be fixed! Could you do try user experience all over once again? I've updated Wiki page too.

@seankrag I've made a new viewer bundled in this PR referencing UW APL's viewer codes. Currently, it is using the 16-bit image msg (OpenCV's CV_16UC1 sent as msgs MONO16). I was getting exactly the same image of your post with 8-bit data (CV_8UC1 OpenCV format; MONO8 msg format). In summary, I think 16-bit looks good enough. The only advantage of having 32-bit float data on my side is that it's easy to colorize with ROS bundled rqt_image_view viewer package. It seems with MONO6 or MONO16 msg format, the color scale could not be applied.

@dtdavi1 I believe this PR is now finally ready to go for the official review! We could use the last iteration to fix things if any problems are found before our celebrating meeting.

[dtdavi1]

Everything seems to run correctly and the images are all right. I am seeing 2 "Node::Advertis(): Error advertising a topic. Did you forget to start the discovery service?" messages, but I don't know what they are associated with.

I added one commit to fix the static code checks. A different check is still failing because of an error associated with finding the APL message repo.

[woensug-choi]

Everything seems to run correctly and the images are all right. I am seeing 2 "Node::Advertis(): Error advertising a topic. Did you forget to start the discovery service?" messages, but I don't know what they are associated with.

I added one commit to fix the static code checks. A different check is still failing because of an error associated with finding the APL message repo.

Thank you for the quick review!

• "Node::Advertis(): Error
  • Where can I see the msg? I don't see anything like it on the terminal screen. I am moving this to an Issue page.
• CI check missing APL message repo
  • I've been trying to get this working for a while. But without success. This part is added to workflow file fix it. But did not work. Do you know any fixes to this?

     - name: Checkout UW imaging_sonar_msgs
       uses: actions/checkout@v2
       with:
         url: https://gitlab.com/apl-ocean-engineering/imaging_sonar_msgs.git
         path: imaging_sonar_msgs

(UPDATE)
It seems Github workflow is not allowed to use a repository outside Github. Changed SonarImage.msg repository to the newly made one at the Github. But still, the ci check fails. Since its not something to do with the functionality I will merge as it is for now.
New workflow file with Github repository