DVL Seabed Gradient

Visualize Seabed Gradient with DVL

This example demonstrates a simple method for moving a DVL sensor through a scene in Gazebo and generating a gradient map of the seabed. Two examples are available, one using the UUV-Simulator-based DVL, and the other using the WHOI-derived DVL.

Launching Example

The following launch files start the example depending on the DVL. For the WHOI DVL:

roslaunch nps_uw_sensors_gazebo whoi_teledyne_whn_standalone_gradient.launch

and for the UUV Sim DVL:

roslaunch nps_uw_sensors_gazebo uuvsim_teledyne_whn_standalone_gradient.launch

Running either 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 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.

(uuv_sim/whoi)_dvl_state_and_gradient.py

This script instantiates a node that subscribes to the following topics with the UUV-Sim DVL:

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

and to the following with the WHOI DVL:

/dvl/dvl
/gazebo/model_states
/cmd_vel

and publishes:

/dvl_gradient

WHOI The /dvl/dvl topic contains a custom DVL message, ds_sensor_msgs/Dvl, from which we can extract range values from the individual beams within the range list.

UUV Sim The /dvl/dvl_* topics 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 are 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 and interpolate the data gathered while traversing the world.