ROS nodes that use liboculus to interface with a Blueprint Subsea Oculus sonar sonar and publish to ROS topics. Currently runs on ROS Melodic and Noetic.
At present we have only tested with the Oculus M1200d sonar.
This package defines the oculus_sonar/driver
nodelet that interfaces with the sonar
over ethernet and publishes acoustic_msgs::SonarImage
and
apl_msgs::RawData
messages.
The package also builds a conventional node oculus_driver
which is a trivial wrapper around a
oculus_sonar/driver
nodelet.
** Note that to visualize the data (as a ROS Image) you need to run a sonar_image_proc/draw_sonar node/nodelet. The default_ros.launch
launchfile will launch this node if draw_sonar:=true
is passed to it.
- Either:
- Use vcstool:
- Clone this repo to
<catkin_ws>/src
cd <catkin_src>
vcs import --input oculus_sonar_driver/oculus_sonar_driver.rosinstall
- Clone this repo to
- Or install dependencies manually. Clone liboculus, hydrographic_msgs and g3log_ros to
<catkin_ws>/src
- Use vcstool:
- Use rosdep to install other dependencies
cd <catkin_ws>
rosdep install --from-paths src -i
- Run
catkin_make
orcatkin build
from<catkin_ws>
- Run
source ./devel/setup.bash
from<catkin_ws>
Running the command roslaunch oculus_sonar_driver default_ros.launch
will start the sonar driver.
See the comments in default_ros.launch
to set sonar parameters at startup. Sonar params can also be set
through dynamic reconfigure (below).
The sonar params can be modified on the fly using dynamic reconfigure. The launchfile default_ros.launch
starts the nodes in a nodelet manager /nodelet_manager
to the commands are:
$ rosrun dynamic_reconfigure dynparam get /nodelet_manager
{'send_range_as_meters': True, 'send_gain': True, 'send_simple_return': True, 'gain_assistance': False, 'all_beams': True, 'num_beams': 1, 'gamma': 127, 'ping_rate': 0, 'data_size': 0, 'freq_mode': 2, 'range': 2.0, 'gain': 50.0, 'groups': {'id': 0, 'parent': 0, 'name': 'Default', 'type': '', 'state': True, 'groups': {}, 'parameters': {}, 'send_range_as_meters': True, 'send_gain': True, 'send_simple_return': True, 'gain_assistance': False, 'num_beams': 1, 'all_beams': True, 'range': 2.0, 'gain': 50.0, 'gamma': 127, 'ping_rate': 0, 'data_size': 0, 'freq_mode': 2}}
For example, for 32-bit mode:
$ rosrun dynamic_reconfigure dynparam set /nodelet_manager data_size 2
(This is inferred from experimental data, it's not from Blueprint)
The sonar appears to have a fixed maximum number of range bins, approx 720. It also has fixed quantized range resolutions: ~2.8mm, ~5.6mm, etc. So the number of range bins N present in data at a given range R in meters is:
N = R / (2^k * 2.8mm) minimizing k s.t. N <= 720
This means, for example:
Range (m) | Bins | Resolution |
---|---|---|
2.0 | 709 | 2.8mm |
2.02 | 720 | 2.8mm |
3 | 532 | 5.6mm |
4 | 709 | 5.6mm |
4.065 | 720 | 5.6mm |
5 | 443 | 11.2mm |
6 | 532 | 11.2mm |
etc. This effect seems to be invariant of 256 v 512 beams, and 8/16/32 bit data. Haven't tested in the lower frequency mode
This repository is covered by the BSD 3-Clause License.