Generate paths from 3d map
Anyone knows a how can I from a 3D map (described as a .sdf gazebo file) get a data structure as output that difference obstacles from free way in order I can give to my robot a path to it follows (not necessary the best one)? Is there a library for that?
Please, correct me if I'm wrong. You have a Gazebo world file with a few models of the world. I'm assuming that there are walls and other obstacles. You want to generate a data structure that captures the occupancy of the world. Is that right? If so, you may want to discretize your world into a 2D grid where each cell has a binary value (free or obstacle). One way do this might be to check the bounding box of each model and translate it to grid coordinates for marking them as obstacle.
Of course this only works if your obstacles are static, otherwise you have to rerun this process for the dynamic models periodically. If the objects are not solid you will need a different solution. You could cast vertical rays and check if they collide with the ground plane (free) or with a different model (obstacle).
I'm new on that @CarlosAguëro. Do you know any way to discretize a 3D world?
You need to figure out how to translate from 2D world coordinates to the right cell in your 2D grid. Imagine that you have a 2 meter X 2 meter world and you want to discretize it in cells of 1 squared meter. You'll have a total of 4 cells with cell (0,0)=(-0.5, 0.5), cell(0,1)=(0.5,0.5), cell(1,0)=(-0.5,-0.5) and cell(1,1)=(0.5, -0.5). That means that in order to find out if cell (0,0) is free, you have to check if there's an object in (-0.5, 0.5, any_height) in Gazebo world coordinates.
Do you think it would be worth use octrees (like is done in OctoMap Framework) and fill voxels with a binary value to indicates the occupancy volume?
Definitely, that sounds like a good idea but I'd start with a very simple custom 2D grid solution to make sure that you have a good understanding of all the steps first. Then, you can incrementally move to octomaps or ROS occupancy grids. Please, share a video with us when you finish it :)