Gazebo | Ignition | Community
Ask Your Question
0

Building height map from occupancy grid

asked 2017-10-12 08:57:32 -0500

GuillaumeHauss gravatar image

updated 2017-10-13 08:45:01 -0500

chapulina gravatar image

High all,

I've spent a few months doing SLAM and localization using an UGV and SICK LMS122. Therefore, I have plenty of 2D maps as pgm files.

Now, I'm implementing some control laws on a new kind of robot, and I would like to use Gazebo as a simulator to test my work. Moreover, I added a mono RGB camera to my robot, to have better performances in obstacles tracking and classification. But I don't want to SLAM in 3D

So, my question is pretty simple : how can I use a pgm file (occupancy 2D grid map) in order to build an height map and create a 3D environment in Gazebo ?

I've looked on ROS answers, and no one seems to have succeeded, at least not for a final 3D environment. I try to use other simulators (StageROS, DRCSim,...) but their installation on my laptop is an absolute mess.. I'm running Ubuntu 16.04 and ROS Kinetic.

Thanks in advance for your help !

Guillaume

EDIT I tried Chapulina trick today, and gzserver ends up crashing everytime... I created a parking.world file that looks like this

<?xml version="1.0" ?>
<sdf version="1.5">
   <world name="default">
   <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="heightmap">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <heightmap>
              <uri>file://home/guillaume/catkin_ws/src/gazebo_worlds/Parking_resized.png</uri>
              <size>1953 1953 10</size>
              <pos>0 0 0</pos>
            </heightmap>
          </geometry>
        </collision>
        <visual name="visual_abcedf">
          <geometry>
            <heightmap>
              <use_terrain_paging>false</use_terrain_paging>
              <texture>
                <diffuse>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</diffuse>
                <normal>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</normal>
                <size>1</size>
              </texture>
              <texture>
                <diffuse>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</diffuse>
                <normal>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</normal>
                <size>1</size>
              </texture>
              <texture>
                <diffuse>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</diffuse>
                <normal>file://home/guillaume/catkin_ws/src/gazebo_worlds/wall_texture.png</normal>
                <size>1</size>
              </texture>
              <blend>
                <min_height>2</min_height>
                <fade_dist>5</fade_dist>
              </blend>
              <blend>
                <min_height>4</min_height>
                <fade_dist>5</fade_dist>
              </blend>
              <uri>file://home/guillaume/catkin_ws/src/gazebo_worlds/Parking_resized.png</uri>
              <size>1953 1953 10</size>
              <pos>0 0 0</pos>
            </heightmap>
          </geometry>
        </visual>
      </link>
    </model>
  </world>
</sdf>

The parking_resized.png file looks like this : C:\fakepath\Parking_resized.png

and wall_texture.png is just a big white square.

Can you help me with that ? thank you

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-10-12 10:08:55 -0500

chapulina gravatar image

updated 2017-10-13 09:05:25 -0500

You could try using it as a gray-scale heightmap:

  1. Convert the PMG to a PNG
  2. Convert the colors so the ground (height == 0) is black and the occupied areas (height == max) are white
  3. Scale it to have a size of 2^n + 1 (for example 257 x 257 pixels)
  4. Use it in a <heightmap> tag as in this example

I believe you'll need to create a dummy texture for the walls, probably an all-white png would do the trick. Otherwise you'll get an invalid yellow and black texture on the heightmap.

Hope this helps!

EDIT

I tried your heightmap and you're almost there. All you need is to resize the image to be a power of 2 plus 1. Right now it is 1953x1953, the closest possible values are 1025 (2^10+1) or 2049 (2^11+1). I tried your texture with 1025 and Gazebo seemed to be struggling to handle it though. So I resized it to 257 and voila:

image description

It's not super pretty, but I hope it helps you somehow.

edit flag offensive delete link more

Comments

Thanks a lot for your idea ! It looks a bit tricky, but that's better than nothing ! I'll give it a try ASAP!

GuillaumeHauss gravatar imageGuillaumeHauss ( 2017-10-12 10:37:15 -0500 )edit

Soo.. I gave it a try and that was unsuccessful. See edit for more info..

GuillaumeHauss gravatar imageGuillaumeHauss ( 2017-10-13 02:52:31 -0500 )edit
1

Yeah, I also size did matter when my computer crashed loading a 4084*4084 image ^^ I've found this git [repo](https://github.com/MatthewVerbryke/gazebo_terrain) and this works amazingly well ! Textures are not editable, but I don't care. Make sure you have a really sharpened image otherwise, height extrapolation will be messy Anyway, thanks a lot for helping me out !

GuillaumeHauss gravatar imageGuillaumeHauss ( 2017-10-13 09:10:17 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2017-10-12 08:57:32 -0500

Seen: 4,566 times

Last updated: May 22 '20