Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

What you want to achieve could be tricky. From my understanding of the pakage find_object (and correct me if I'm wrong, it's a key point), you will be able to get the position of some objects, it will be only one poisition but not the edges of this object. That means that you would know in the occupancy grid the area where you need to change the cells data, but not the exact pose of each cells.

With that in mind, you will have to find the edges of an object given its position.

First, you will need to get the occupancy grid from gmapping. To create your new map I would use two occupancy grid : one occupancy grid will be your objects free map, I'll call it cleared_map,and the other one would be used to create the previous one, I'll call it history_map.

Why two occupancy grids ?

The idea is to use the data from gmapping to create a new map. The data from the topic map is an array containing the value of each cells of your map. With gmapping the cells have three values :

  • -1 : UNKNOWN.
  • 0 : FREE.
  • 100 : OCCUPIED.

Your goal is to create a new map by replacing the values of the cells at the pose of an object from 100 to 0, that would remove the objects from the map. You need to have another occupancy grid to save the history of your objects that have been removed. This occupancy grid would have cells with intermediate values at the pose of the objects (I'll suggest 50 but anything different than 0, -1 or 100 would be fine). This occupancy grid will be constantly overwritten (meaning the value of each cells) by the occupancy grid from gmapping to always have the updated map, unless the cells have the value 50. By doing so you won't have to deal with the same object each time. Without history_map, you wouldn't know which cell needs to be overwritten or not and you would do the same calculations over and over.

Then you will have to remove the objects and you'll be able to create the cleared_map by changing every cells of histroy_map from the value 50 to 0.

Delete your objects

You now have your history_map, the goal is now to find which cells will be set at the value 50. You do have the pose of your objects. To work from a pose with an occupancy grid you first need to convert this pose to an index to get the corresponding cell.

Now here's the tricky part. You have to find all the cells corresponding to your object. You can assume that an object in map is always a group of cells at the value -1 and the edges at 100 (because gmapping can't see inside objects so the cells are always unknown). For a cylindric paper bin as you used this example the map data should be something like that :

0     0     0     0     0     0
0     0    100   100    0     0
0    100   -1    -1    100    0
0    100   -1    -1    100    0
0     0    100   100    0     0
0     0     0     0     0     0

You may assume that the position given by find_object is the center of an object so the cell would always have the value -1. You need to check all the neighbor cells of the cell you get from the object pose having the value -1 until you find a neighbor with the value 100. Once you get the list of all the cells at the object pose you can change their value to 50 :

0     0     0     0     0     0
0     0     50    50    0     0
0     50    50    50    50    0
0     50    50    50    50    0
0     0     50    50    0     0
0     0     0     0     0     0

Now you have your history_map with an object detected, you just have to copy the occupancy grid and replace the 50 by 0 to get your cleared_map.

Note : I couldn't find ros packages doing that but you can probably identify objects using openCV, that would be more precize and easier than working direclty with the occupancy grid data. Moreover, if my first assumption about the coordinates of the objects being only one pose is wrong and you do have the edges, then the last part is trivial.

Hope it helps.