ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
As far as I know, gmapping maintains occupancy values internally. However, the ROS wrapper thresholds them into binary free/occupied values.
Take a look at this file:
Starting at line 625:
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
}
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
The default value for occ_thresh_
is 0.25. It appears that if you want the occupancy values, you can simply revert the line which was commented out, and change the code to:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
I have created a ticket here.
2 | No.2 Revision |
As far as I know, gmapping maintains occupancy values internally. However, the ROS wrapper thresholds them into binary free/occupied values.
Take a look at this file:
Starting at line 625:
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
}
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
The default value for occ_thresh_
is 0.25. It appears that if you want the occupancy values, you can simply revert the line which was commented out, and change the code to:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
I have created a ticket here.
EDIT: response to comment
I'm not sure whether amcl currently assumes binary values or not.
With laser sensors, you typically reach saturation probabilities (0 or 1) pretty fast, since lasers have a small uncertainty spread. My intuition is that since the nav stack is mainly developed for laser-equipped ground robots, the binary value is a good approximation.