ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
It's not gmapping that is using only three discrete values, but the nav_msgs/OccupancyGrid.msg message format. Internally, gmapping uses a probabilistic cell values, but in the slam_gmapping.cpp source, starting in line 631, those are converted to above linked message format like so:
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
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;
}
Haven't played around with the code, but the occ variable should hold the probability of occupancy.
See also this question/answer about visualization in rviz.
2 | No.2 Revision |
It's not gmapping that is using only three discrete values, but the nav_msgs/OccupancyGrid.msg message format. Internally, gmapping uses a probabilistic cell values, but in the slam_gmapping.cpp source, starting in line 631, those are converted to above linked message format like so:
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
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;
}
Haven't played around with the code, but the occ variable should hold the probability of occupancy.
See also this question/answer about visualization in rviz.