RoPod/Tutorials/Load heightmap in GAZEBO

Conversion from PGM

In principle Gazebo can load directly heightmaps specified in a png grayscale file. However, after following some tutorials, there is a bug in GAZEBO and the heightmap is not loaded correctly. According to comments in forums such functionality has not been paid too much attention since it is more efficient to work with mesh files when checking for collisions.

Therefore, as an alternative one can convert the heightmap into a 3D mesh file and load it in GAZEBO, which works straightforward. The conversion could be made in a program like blender, however here directions are given using open source software.

This method seems to not work anymore, so you might want to follow the SDF file part of this tutorial to create your heightmaps.

The conversion process is divided in several steps:

1. Convert PGM to SVG

The conversion is made using the program potrace. If not installed, run:

sudo apt install potrace

Next, use the resolution of your heightmap to compute the corresponding dpi resolution. Use this formula:

dpi resolution = 25.4 / #mmperpixel

For the heightmap of the lab, the resolution is 25mm/pixel, thus 1.016 dpi. Next run the potrace command as follows:

potrace -s heightmap.pgm -a 0 -r 1.016

Then a file named heightmap.svg is created.

2. Convert SVG to openSCAD file

OpenSCAD is an open source software to create mesh files, however it works with its own file types, thus we will later export the mesh to STL file. Thus, to do that, first a conversion from SVG to openSCAD is necessary.

To convert from SVG to openSCAD Inkscape needs to be installed. After installation, a plugin needs to be added to Inkscape. See step 2 on this link http://www.instructables.com/id/Make-a-3D-print-from-a-2D-drawing/#step2 . For the installation of the plugin you need to copy the paths2openscad.py AND the paths2openscad.inx files.

When creating the openSCAD file you can choose the height(mm) and the smoothing factor. Values of 1000 mm (Maximum allowed in the configuration) and 0.5 respectively work fine.

3. Render and create STL file

After installating openSCAD, follow step three from this link http://www.instructables.com/id/Make-a-3D-print-from-a-2D-drawing/#step3 . Thus, in openSCAD render the file generated in the previous step and export it as STL. The commands are slightly updated compared to the commands in the link. You should use Design-> Render and File->Export->Export as STL.

4. Create a URDF-file

Open a texteditor and create a URDF-file. Below an example is given. Important is that you refer to the STL-files generated in the previous step (here malaga_test_map) for both the collision-map and the visual-map. In the lines below it you can set the scaling in x, y and z-direction. It is IMPORTANT to scale DOWN the mesh 1000 times. This because the default units in the STL file are millimeters and in GAZEBO are meters. If you wish to have walls higher than 1000mm choose an appropriate scale in z-direction.

<?xml version="1.0"?>
<robot name="Floorplan_demo_ROPOD">


<link name="map">
	<inertial>
		<mass value="500"/>
		<inertia
			ixx="1000.0" ixy="0.0" ixz="0.0"
			iyy="1000.0" iyz="0.0"
			izz="1000.0"/>
	</inertial>	  
	
	<visual name="map_visual">
		<origin rpy="0 0 0" xyz="0 0 0"/>
		<geometry>
			<mesh 
				filename="package://ed_object_models/models/malaga_test/walls/shape/malaga_test_map.stl"
				scale="0.001 0.001 0.002"		
			/>
		</geometry>
	</visual>
	
	<collision name="map_collision">	
		<origin rpy="0 0 0" xyz="0 0 0"/>	
		<geometry>
			<mesh 
				filename="package://ed_object_models/models/malaga_test/walls/shape/malaga_test_map.stl"
				scale="0.001 0.001 0.002"		
			/>
		</geometry>
	</collision>   

</link>  

<gazebo reference="map">
	<maxContacts>10</maxContacts>
	<laserRetro>0</laserRetro>
	<dampingFactor>0.01</dampingFactor>
	<selfCollide>false</selfCollide>
</gazebo>

</robot>

5. Use URDF-file in GAZEBO

As a final step, you can use the generated STL file in GAZEBO. At the moment we do this by referring to the URDF-file in the "start.launch"-file of the corresponding robot.

SDF files

You can also use GAZEBO to build your maps. Start by opening GAZEBO from a terminal

gazebo

When in GAZEBO, press ctrl+b to open the building editor. You can now create walls and obstacles, which you can view in 2D and 3D. When finished, give the model a name in the top left. Then go to file>save as and choose a model name and location for your floorplan. If necessary, you can enter some additional metadata in the advanced options menu. After having saved this model, a model.config and model.sdf file will show up in the location you specified.

The SDF file we just created can not be used immediately, we first have to use gmapping to create a map that can be used by the robot. Open a new terminal and run:

roslaunch ropod_tue_2_bringup start_sdf.launch floorplan:=<fullpath_to_your_model> gmapping:=true

Make sure to enter the full path, so including /home/<username> and to enter the file without a file extension. The file extension is added automatically in the launch file. In the rviz window that opens up, open the configuration on the left. Find Map (It should be highlighted in yellow since there is a warning) and manually change the topic from /map to /ropod/map. Close the configuration windows again. Yuo should now see that a map has started building.

To visualize everything at the same time, have GAZEBO and rviz opened next to each other, or each program on a separate screen if possible. Now we are going to manually drive the robot around the map, so it has seen the full map once. To do this, open a new terminal and run (if you don't have the package yet):

sudo apt-get install ros-kinetic-teleop-twist-keyboard

After installing the package, or if you already had the package, you can simply run:

rosrun teleop_twist_keyboard teleop_twist.py __ns:=/ropod/

Now just follow the on-screen instructions to drive the robot around. Make sure to drive it around your whole map, and to drive over locations several times if they are not saved to the map correctly on the first run. Once you have mapped everything, go to a new terminal again and run:

rosrun map_server map_saver -f <pathtomap>/<mapname> /map:=/ropod/map

A <mapname>.pgm and <mapname>.yaml file will now be saved in the location you specified in <pathtomap>.