DVL Seabed Gradient

Visualize Seabed Gradient with DVL

This example demonstrates a simple method for moving a DVL through a scene in Gazebo and generating a gradient map of the seabed.

Launching Example

The following launch file starts the example:

roslaunch nps_uw_sensors_gazebo whoi_teledyne_whn_standalone_gradient.launch

Running the launch script starts gazebo and loads a standalone DVL model. The ocean world contains a seabed with known slope values. The script also starts three nodes:

/twist_keyboard

/twist_dvl_state

/plot_gradient

Generating the Gradient Map

The launch script should spawn a gazebo window and a matplotlib figure containing three empty subplots.

In order to build the gradient maps, the sensor must be moved through the scene. The terminal in which the nodes are launched is running the /twist_keyboard node. The control keys are:

Moving around:
    u   i   o
    j   k   l
    m   ,   .

Strafing:
    U   I   O
    J   K   L
    M   <   >

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

After moving the sensor around the map for some time, the plots should begin to look like: The top plot displays the measured gradient magnitude where warmer colors represent a larger gradient. The central plot shows the magnitude and direction as a quiver plot. The bottom plot shows the difference between the measured and ground truth (GT) gradient magnitude, where warmer colors represent a greater difference between the measured and GT values.

Implementation Details

On launch, the teleop_twist_keyboard node is started and publishes the /cmd_vel twist topic which is used here to send twist messages to the model.

dvl_state_and_gradient.py

This script instantiates a node that subscribes to the following topics:

/dvl/dvl_sonar0
/dvl/dvl_sonar1
/dvl/dvl_sonar2
/dvl/dvl_sonar3
/gazebo/model_states
/cmd_vel

and publishes:

/dvl_gradient

The /dvl/dvl_* topics are published by the sensor plugin and contain a sensor_msgs/Range message which reports a single range value per beam. The estimated range combined with the sensor layout described in uuvsim_teledyne_whn_urdf/model.urdf allows for gradient estimation.

The /gazebo/model_states topic is published by gazebo and is required for keeping track of the model location within the world.

/gazebo/model_states/ and /cmd_vel from teleop_twist_keyboard is used to set the twist of the model with the SetModelState service.

dvl_gradient contains a geometry_msgs/Point message which packages the gradient as x and y for the 2D direction of increasing gradient in world coordinates and z for the magnitude.

dvl_gradient_plot.py

This script instantiates a node which creates the three realtime plots of the gradient measurements. The node subscribes to:

/gazebo/model_states
/dvl_gradient

The scipy.interpolate.griddata function is used to bin the data gathered while traversing the world and is linearly interpolated.