Making your seabed world model
For the following tutorial, all the files can also be seen in the uuv_tutorial_seabed_world package.
The seabed can be critical in simulation scenarios where the objective is, for example, to use UUVs for bathymetric mapping or just to make the scenario look more realistic.
Gazebo already has a feature to generate heightmaps from grayscale images and a tutorial for it can be seen here. This is a very quick way to setup a heightmap, but it can happen that its complexity might make the simulation run slower when trying to interact with it.
Another option is to take an existent point cloud of a seabed and convert it into a mesh that can be imported in Gazebo like any other model.
Measurement data can be sparse and have outliers that need to be removed before generating the mesh. The script below is an example on how the measurement data can be interpolated into a grid and later converted into an STL file. For this example, we will use the test surface available in the matplotlib package, but you should replace it with your own point cloud data. You will also need the packages numpy, scipy and numpy-stl.
To install the necessary packages, you can use pip (sudo apt-get install python-pip to install this tool, if necessary), and then
sudo pip install numpy scipy matplotlib numpy-stlChange the code below to your needs.
import numpy as np
from scipy.interpolate import griddata
import scipy.ndimage as ndimage
from scipy.ndimage import gaussian_filter
from scipy.misc import imsave
from matplotlib import cm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from stl import mesh, Mode
import matplotlib.tri as mtri
from mpl_toolkits.mplot3d.axes3d import get_test_data
# Generating the surface
x, y, z = get_test_data(delta=0.1)
# Scale the surface for this example
z *= 0.05
# Remember that Gazebo uses ENU (east-north-up) convention, so underwater
# the Z coordinate will be negative
z -= 3
# Note: Gazebo will import your mesh in meters.
# Point clouds may not come in nice grids and can be sparse,
# so let's make it a (N, 3) matrix just to show how it can be done.
# If you have outliers or noise, you should treat those values now.
xyz = np.zeros(shape=(x.size, 3))
xyz[:, 0] = x.flatten()
xyz[:, 1] = y.flatten()
xyz[:, 2] = z.flatten()
# Generate a grid for the X and Y coordinates, change the number of points
# to your needs. Large grids can generate files that are too big for Gazebo, so
# be careful when choosing the resolution of your grid.
x_grid, y_grid = np.meshgrid(np.linspace(xyz[:, 0].min(), xyz[:, 0].max(), 300),
np.linspace(xyz[:, 1].min(), xyz[:, 1].max(), 200))
# Interpolate over the point cloud for our grid
z_grid = griddata(xyz[:, 0:2], xyz[:, 2], (x_grid, y_grid),
method='linear')
# Option to treat noise
#z_grid = gaussian_filter(z_grid, sigma=1)
# Show the resulting heightmap as an image
fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111)
plt.imshow(z_grid)
# Flatten our interpolated data for triangulation
output = np.zeros(shape=(x_grid.size, 3))
output[:, 0] = x_grid.flatten()
output[:, 1] = y_grid.flatten()
output[:, 2] = z_grid.flatten()
# Triangulation of the interpolated data
tri = mtri.Triangulation(output[:, 0], output[:, 1])
# Show the resulting surface
fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111, projection='3d')
ax.plot_trisurf(tri, output[:, 2], cmap=plt.cm.CMRmap, shade=True, linewidth=0.1)
ax.axis('equal')
# Create the mesh object
seabed_mesh = mesh.Mesh(np.zeros(tri.triangles.shape[0], dtype=mesh.Mesh.dtype))
# Set the vectors
for i, f in enumerate(tri.triangles):
for j in range(3):
seabed_mesh.vectors[i][j] = output[f[j]]
# Store the seabed as a STL file
seabed_mesh.save('seabed.stl')
plt.show()Below you can see the resulting heightmap as an image
and the triangulated grid used for the creation of the seabed
Now a seabed.stl data has been created or you can download it here. You can open it with MeshLab or Blender. Here we will use Blender to show the surface. You can also use Blender to further edit the mesh, if needed.
Even though the surface is ready, if you import it in Gazebo as it is you might not be able to have contact forces if you hit the seabed with your robot. One way to solve it is to import the STL in Blender using the STL importer tool
and once the surface is uploaded, go to the Modifiers section of the right panel as follows
and choose the option Solidify in the Add modifier popup menu.
Enter a value for the surface's thickness as shown below and click on the button Apply.
You can now export you surface again using the STL exporter tool.
Now you need to create a SDF model description of your seabed to load it in Gazebo.
For these further steps, you will need to have followed the installation and configuration instructions tutorials. For all the following steps, the files described below can be downloaded as a catkin package here. Just extract the files in you catkin workspace, e.g. ~/catkin_ws/src, and it won't be necessary to generate the files as described below. If you want a step-by-step tutorial on how to generate these files, keep reading the following sections.
To be able to load your seabed mesh in Gazebo, you need a SDF model describing it. Starting from the folder structure, let's create a new catkin package for this world description. The folder structure below is only a suggestion based on the structure used in this package.
cd ~/catkin_ws/src
catkin_create_pkg uuv_tutorial_seabed_world
cd uuv_tutorial_seabed_world
mkdir world_models worlds launch
mkdir world_models/seabedLet's create the files for our seabed model.
cd ~/catkin_ws/src/uuv_tutorial_seabed_world/worlds
touch CMakeLists.txt
cd ~/catkin_ws/src/uuv_tutorial_seabed_world/world_models
touch CMakeLists.txt
cd seabed
touch CMakeLists.txt model.config model.sdfNow copy the seabed.stl file into the folder ~/catkin_ws/src/uuv_tutorial_seabed_world/world_models/seabed. Starting with the CMakeLists.txt files, copy the text below to each file. In the file world_models/CMakeLists.txt, copy the file below
set(dirs
seabed
)
foreach(dir ${dirs})
add_subdirectory(${dir})
endforeach()In world_models/seabed/CMakeLists.txt, the following should be used
set(MODEL_NAME seabed)
set (files
model.sdf
model.config
seabed.stl
)
install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/models/${MODEL_NAME})Now the model files must be edited. For world_models/seabed/model.config
<?xml version="1.0"?>
<model>
<name>Seabed</name>
<version>1.0</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>John Doe</name>
<email>[email protected]</email>
</author>
<description>
A nice seabed.
</description>
</model>And finally, for world_models/seabed/model.sdf. The materials used below come from the ocean_box model in uuv_descriptions. You can replace them for other textures. A sea surface is also added below.
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="seabed">
<static>true</static>
<link name="seabed_link">
<visual name="surface">
<cast_shadows>false</cast_shadows>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>60 60 .1</size>
</box>
</geometry>
<material>
<script>
<uri>model://ocean_box/materials/scripts</uri>
<uri>model://ocean_box/materials/textures</uri>
<name>UUV/Water</name>
</script>
</material>
</visual>
<visual name="seafloor">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh><uri>model://seabed/seabed.stl</uri><scale>1 1 1</scale></mesh>
</geometry>
<material>
<script>
<uri>model://ocean/materials/scripts</uri>
<uri>model://ocean/materials/textures</uri>
<name>Sand</name>
</script>
</material>
</visual>
<collision name="seafloor">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh><uri>model://seabed/seabed.stl</uri><scale>1 1 1</scale></mesh>
</geometry>
</collision>
</link>
</model>
</sdf>The materials used in this example refer to the Gazebo world models already included in the UUV Simulator package. You can change them to, for example, the default materials provided by Gazebo, which are described here, such as:
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/BlueTransparentOverlay</name>
</script>
</material>Now, replace contents in the file ~/catkin_ws/src/uuv_tutorial_seabed_world/CMakeLists.txt for the following
cmake_minimum_required(VERSION 2.8.3)
project(uuv_tutorial_seabed_world)
find_package(catkin REQUIRED)
find_package(gazebo REQUIRED) # this is only required to get gazebo's directories
catkin_package()
# Install models and world descriptions
add_subdirectory(world_models)
add_subdirectory(worlds)
# Install launch files
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
PATTERN "*~" EXCLUDE)If you followed the installation instructions here and build the your workspace again as follows
cd ~/catkin_ws
catkin_make installyou will see a Seabed model in the list at the Insert tab. Open Gazebo and delete the ground_plane in the Models list, then select the Seabed model from the list at the Insert tab. The result can be seen below:
Now that the model is done, you can create a Gazebo world to load your scenario with the seabed model. Create a new world file as follows
roscd uuv_tutorial_seabed_world
cd worlds
touch example_underwater.worldand copy the following in this new example_underwater.world file
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="example_underwater">
<physics name="default_physics" default="true" type="ode">
<max_step_size>0.002</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>500</real_time_update_rate>
<ode>
<solver>
<type>quick</type>
<iters>50</iters>
<sor>0.5</sor>
</solver>
</ode>
</physics>
<scene>
<ambient>0.01 0.01 0.01 1.0</ambient>
<sky>
<clouds>
<speed>12</speed>
</clouds>
</sky>
<shadows>1</shadows>
</scene>
<!-- Global light source -->
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://seabed</uri>
<pose>0 0 0 0 0 0</pose>
</include>
<plugin name="underwater_world" filename="libuuv_underwater_world_ros_plugin.so">
<namespace>hydrodynamics</namespace>
<constant_current>
<topic>current_velocity</topic>
<velocity>
<mean>0</mean>
<min>0</min>
<max>5</max>
<mu>0.0</mu>
<noiseAmp>0.0</noiseAmp>
</velocity>
<direction>
<mean>0</mean>
<min>-1.5707963267948966</min>
<max>1.5707963267948966</max>
<mu>0.0</mu>
<noiseAmp>0.0</noiseAmp>
</direction>
</constant_current>
</plugin>
</world>
</sdf>Now edit the file ~/catkin_ws/src/uuv_tutorial_seabed_world/worlds/CMakeLists.txt and include the following
find_package(gazebo REQUIRED)
set (worlds
example_underwater.world
)
# Install all the world files
install(FILES ${worlds}
DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/worlds)If you open the world file, Gazebo will already load the scenario with the seabed, sea surface and a sky.
cd ~/catkin_ws/src/uuv_tutorial_seabed_world/worlds
gazebo example_underwater.world
In this last step, we can create a launch file for your world. Create the launch file as follows
roscd uuv_tutorial_seabed_world
touch launch/example_underwater_world.launchand include the following in the new example_underwater_world.launch file
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="worlds/example_underwater.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
<arg name="verbose" value="true"/>
</include>
</launch>Now you can launch your world using
roslaunch uuv_tutorial_seabed_world example_underwater_world.launchDone! Now the custom seabed can be used for your simulation.