Probability occupancy grid with Turtlebot's Gmapping?

asked 2018-04-28 10:02:27 -0500

Anonymous

Kinetic, Ubuntu 16.04

Turtlebot3 using Gmapping

Gmapping uses a threshold to turn its maps into a binary map of cells that have either 'occupied' or 'free' (or 'unknown'). However, I need to use the probabilistic occupancy grid map with its full range of values. I've looked around the code, and previous answers

The problem is that I don't have a gmapping package; I have a turtlebot3 package. I can't find code where it applies the treshold to a probability map to turn it into a binary map. I just have files like:

<launch>
  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch" />
  <!-- Gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
    <param name="base_frame" value="base_footprint"/>
    <param name="odom_frame" value="odom"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="8.0"/>
    <param name="minimumScore" value="100"/>
    <param name="linearUpdate" value="0.2"/>
    <param name="angularUpdate" value="0.2"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="delta" value="0.05"/>
    <param name="lskip" value="0"/>
    <param name="particles" value="120"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.0"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    </node>
</launch>

and

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>

  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
    <arg name="model" value="$(arg model)" />
  </include>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="50.0" />
  </node>
</launch>

(Lots of files, I cant post all of the files in here, but can add if required)

I was told by my supervisor that I should be able to get the probability maps "in two or three lines of code", but remain in the dark as to how to go about it. I can't seem to find where Turtlebot's Gmapping is storing these probability maps / topics.

How do I get the probability maps?

edit retag flag offensive close merge delete

Comments

@anonymous Were you able to figure this out? I'm also looking for ways to get a probability map

distro ( 2022-01-02 13:45:28 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2023-05-04 11:41:44 -0500

hunterlineage1
24 ●8 ●17 ●24

updated 2023-05-04 11:42:21 -0500

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. 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;
    }
  }