Reworked and Stable Sonar Object/Wall Detection (#109)

* Sonar data from pool test on 10/19 added to /onboard/src/sonar/sweep_data. Docker-build.sh changed because of issues with personal computer.

* Renamed sweep data

* Added skeletons for fourier

* Added fourier transform to sonar image processing

* changed return of fourier transform to include the gradian space image

* Changing Sonar Package

* Sonar Denoising

Added Sonar Denoiser Class which can process sonar sweeps. Added a publisher to allow viewing data and testing if the processing is actually working

* Ready for 11/8 Pool Test program - added comments to indicate temporary code for the test

* Fixed the Pool Test error with wrong variable name

* Move methods from Sonar class which are more utility than interfacing with the sonar device to sonar_utils.py (#97)

- Skipping checks because this is just to merge two of my branches together so that I can work out of one.

* Ported over and began documenting code to find the regressions and distances of objects

* Update Sonar Object Detection

* Fixed the program so that sonar request does not crash

* Ported over normal and object detection code

* Finished porting over get_xy_of_object_in_sweep

* Ported one last method from sonar to sonar_utils (convert_to_ros_compressed_img, code in task_planning is from Pranav Bijith - add rotate_to_normal and rotate_to_angle_from_normal

* Update sonar so that it runs on land, need to test for pool

* Transform poses & angles to account for cartesian forward being along y=x

* Transform pose into posestamped

* Fixed sonar issues: the main issue was passing in ints to the pose instead of floats

* Clean up Sonar Imports and make sonar_image from int array be a uint8 array

* Added scikit-image to dockerfile for sonar

* Changed topic names to better reflect what is being published, make constant_sweep method align with the new sonar pipeline

* Fix linting issues

* Created a README for the sonar package

* Fixed Sonar pose estimation from 1/11 pool test

* Added wall angle publisher

* Testing

* Linting sonar.py, sonar_object_detection.py, and sonar_tasks.py

* Linting pt 2: sonar_tasks.py, sonar_utils.py, and sonar.py

* cv is broken so tried dead reckon prequal

* Fixed sonar/wall/angle topic and make it publish relevant wall angle data

* removed get_normal_angle task

* Re-added normal angle getter method

* Revert spillover from prequal branch

* sonar tasks debuggers updated

* Fixed linting errors we determined to be real errors (a few linting errors that are questionable)

* Linting test

* fixed the logger

* added an await for the future

* converted normal_angle to degrees

* Fixed sonar tasks using land testing, added more robust checks and removed async errors

* Removed semicolons on return statements

* fixed f strings

* fixed errors using linting

* Added wall angle onto SonarSweepRequest's response

* fixed linting issue in get_xy_of_object_in_sweep

* Added new sonar task to align to a wall, need to check my geometry, though

* Fixing rotate_to_normal

* Added cases for rotate_to_normal

* Made merges more likely

* Revert "Added cases for rotate_to_normal"

This reverts commit bec9f5b.

* Made merges even more likely

* move await to rotate_to_normal so we directly interface with the future

* Testing on Oogway + Fix DockerFile

* Fixed Future awaited twice error, issue in task.py primarily

* Cleaning up Sonar from pool test code

* Fixed sonar

* Linting to prepare for PR

* Updated Readme to include types and removed floating point errors

* Made SonarSegmentation2 name more descriptive, reverted prequal change, removed dvl_timestamps

* Apply suggestions from code review

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Follow suggestion on valueerror

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Apply suggestions from code review

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Fixed GlobalSonarSegmentation issue

* Fix linting issues

* Remove unnecessary code and packages

* anthony changes

---------

Co-authored-by: Oogway <oogway@duke-robotics.com>
Co-authored-by: Pranav Bijith <pranavbijith1@gmail.com>
Co-authored-by: Viet Hung Le <brandon.viethungRI2001@gmail.com>
Co-authored-by: Mathew Chu <mathew.chu@duke.edu>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

Author: 6 people

core/src/custom_msgs/msg/SonarSweepResponse.msg

@@ -4,5 +4,8 @@ geometry_msgs/Pose pose
 # Normal angle
 float64 normal_angle
 
+# Angle of wall
+float64 angle_of_wall
+
 # Object is there
-bool is_object
+bool is_object

core/src/custom_msgs/srv/SonarSweepRequest.srv

@@ -10,3 +10,4 @@ bool is_object
 
 geometry_msgs/PoseStamped pose
 float64 normal_angle
+float64 angle_of_wall

onboard/src/sonar/README.md

@@ -0,0 +1,39 @@
+# Sonar
+
+The sonar package listens from sonar ping data from the Blue Robotics Ping360. We then run a denoising pipeline which primarily utilizes 2D FFTs to allow for object detection within the scan. The package publishes sonar scans as they are run.
+
+## Structure
+The following are the folders and files in the sonar package:
+`config`: Robot-specific config files for the sonar package.
+`launch`: Contains the launch files for the Sonar package.
+`resource`: Empty ROS2 resource directory.
+`sonar`:
+- `decode_ping_python_360.py`: Directly interfaces with the Ping360 to decode messages.
+- `sonar_image_processing.py`: Contains utility methods to process Sonar images.
+- `sonar_object_detection.py`: Contains pipeline for sonar denoising and object detection.
+- `sonar_utils.py`: Contains utility sonar methods for sonar calculations.
+- `sonar_test_client`: Creates client node to send sonar sweep requests.
+- `sonar.py`: Contains logic to initialize and run the sonar node.
+`sweep_data`: Sample raw sonar scan data.
+
+## Config
+The `config` directory contains robot-specific `.yaml` files. The format is as follows:
+```yaml
+ftdi: <string> FTDI device serial number of the USB-to-serial adapter used by the Ping360
+center_gradians: <float> Referencing heading for center direction of the sonar
+increase_ccw: <bool> Whether angle values increases counterclockwise or not
+```
+
+## Topics
+
+### Published
+- `/sonar/image/raw`
+  - When the sonar pipeline runs, it publishes the raw sonar image to this topic
+  - Type: `sensor_msgs/CompressedImage`
+- `/sonar/image/compressed`
+  - When the sonar pipeline runs, it publishes the denoised image to this topic
+  - Type: `sensor_msgs/CompressedImage`
+- `/sonar/wall/angle`
+  - When the sonar pipeline runs, it publishes the relative angle of a wall (if found) to the robot
+  - When it faces directly at a wall: 0 radians, if it is parallel with the wall on the right side: pi/2 radians, if it is parallel with the wall on the left side: -pi/2 radians.
+  - Type: `std_msgs/msg/Float32`