ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

To change the updateMap function to get continuous values for the probability of occupancy mapping, you need to change the way the occupancy probability is calculated and stored in the map. Currently, the occupancy probability is either 0 or 100, depending on whether it is below or above the occ_thresh parameter. To store continuous values, you can modify the code that assigns values to the map.data array.

One way to do this is to use a linear mapping between the range of occupancy probabilities [0,1] and the range of integer values [0,100], which is the range of values that can be stored in the map.data array. To do this, you can replace the following line:

map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;

with:

map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int) round(occ * 100);

This will map the occupancy probability occ to an integer value between 0 and 100, and store it in the map.data array.

Note that this assumes that the occupancy probability values are in the range [0,1]. If the values are in a different range, you will need to adjust the mapping accordingly.

This is the code block from updateMap function in slam_gmapping.cpp that needs to look like this:

for(int x=0; x < smap.getMapSizeX(); x++)
  {
    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;
    }
  }

To change the updateMap function to get continuous values for the probability of occupancy mapping, you need to change the way the occupancy probability is calculated and stored in the map. Currently, the occupancy probability is either 0 or 100, depending on whether it is below or above the occ_thresh parameter. To store continuous values, you can modify the code that assigns values to the map.data array.array. Be sure to set occ_thresh to 0.0.

One way to do this is to use a linear mapping between the range of occupancy probabilities [0,1] and the range of integer values [0,100], which is the range of values that can be stored in the map.data array. To do this, you can replace the following line:

map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;

with:

map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int) round(occ * 100);

This will map the occupancy probability occ to an integer value between 0 and 100, and store it in the map.data array.

Note that this assumes that the occupancy probability values are in the range [0,1]. If the values are in a different range, you will need to adjust the mapping accordingly.

This is the code block from updateMap function in slam_gmapping.cpp that needs to look like this:

for(int x=0; x < smap.getMapSizeX(); x++)
  {
    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;
    }
  }