Black Dots Randomly Jumping On Map Without Lidar Feedback

Fellfalla commented 3 years ago

The Setup

Ubuntu 20.04
ROS noetic

2x Hokuyo Lidars Mounted upside down in the robot

<xacro:macro name="hokuyo_plugin" params="name topic">
<!-- hokuyo -->
<gazebo reference="$(arg tf_prefix)_${name}_frame">
  <sensor type="ray" name="$(arg tf_prefix)_${name}_sensor">
    <pose>0 0 0 0 0 0</pose>
    <visualize>true</visualize>
    <update_rate>20</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>720</samples>
          <resolution>1</resolution>
          <min_angle>-${2*PI*102.5/360}</min_angle>
          <max_angle>${2*PI*130/360}</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.05</min>
        <max>30.0</max>
        <resolution>0.01</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <!-- Noise parameters based on published spec for Hokuyo laser
            achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
            stddev of 0.01m will put 99.7% of samples within 0.03m of the true
            reading. -->
        <mean>0.0</mean>
        <stddev>0.001</stddev>
      </noise>
    </ray>
    <plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_laser.so">
      <topicName>/$(arg skidname)/${topic}</topicName>
      <frameName>$(arg tf_prefix)_${topic}_frame</frameName>
    </plugin>
  </sensor>
</gazebo>
</xacro:macro>

Gazebo 11

How to reproduce

Start a gazebo world with objects
Start iris_lama slam_2d
Move an object in gazebo that already has been identified as static wall

Actual Behavior

On this screenshot you can see black dots around the footprint of the robot on the right hand side of the screenshot.

Those do not exist in the simulation and still make the robot think he is stuck.
No lidar points appeared where the dots appear (at least not the last 10 minutes, but at the beginning the obstacle on the left side of the screenshot was there)
The dots behavior remind of Conway's Game of Life

Note: We observe the same behavior on the real robot, in the real word with sick nanoscan3 lidars.

Expected Behavior

Map contains static obstacles only at places where lidar points appear
Dots do not appear in free space without lidar point event
When formerly static objects start to disappear on the map, they should fully disappear and not leave some blinking dots behind

eupedrosa commented 3 years ago

I think the problem is that you your lasers did not mark the free space properly. Did you make sure that, after moving the static object, the laser capture the new free space? Note that the mapping algorithm is a little conservative about occupied space, it needs more observations to mark free space than occupied space.

Fellfalla commented 3 years ago

The lidars in the example above cover 360°, so they definitely also cover the new free space. The static object slowly decades within minutes. Maybe the lidar sensors resolution and samples settings are wrong?

eupedrosa commented 3 years ago

No, I believe your settings are not wrong. The behaviour that you are having is expected. The mapping process uses a Frequency Map (see FrequencyOccupancyMap) with an occupancy threshold of 0.25. That is, any occupancy probability above 0.25 is considered occupied. This is why it is harder to change from occupied to free.

Unfortunately, at the moment, the threshold is currently hardcoded, and the behaviour can not be changed.

Fellfalla commented 3 years ago

I chaged the threshold by occ_thresh = 0.45, but this seems to me just to be a workaround that mitigates the real problem, because the problem is not there from the very beginning and becomes more significant over time. It's really heavy when people keep walking through the office. The thing is: occupied cells pop up without any lidar point detection. They popup randomly without any obvious reason.

Doesn't the FrequencyOccupancyMap have a rolling time window?

eupedrosa commented 3 years ago

The FrequencyOccupancyMap is intended for mostly static environments. It will handle small dynamic changed but, just like you reported, with a considerable ammount of people walking it may "fail".

The FrequencyOccupancyMap does not have a rolling time window. But I do not believe that a rolling time window is the best approach, unless you really want to "forget" mapped areas over time.

My suggestion is to periodically check all cells and remove the ones with a low number of visits, or cells that have no neighbours. You could also try the ProbabilisticOccupancyMap. This map will react more quickly to changes.

The thing is: occupied cells pop up without any lidar point detection. They popup randomly without any obvious reason.

Could you kindly provide a dataset where this happen? This is the first time someone report this kind of behaviour. I would like to investigate understand what is is happening.

cosmicog commented 3 years ago

@Fellfalla Just a side note,

Your robot's footprint seems a bit odd. So I assumed this might be the thing that is happening: The lidars are seeing the robot itself, probably because of a bad mount position or the lidar's gap area is obstructed by some part of the robot.

I saw this behaviour on one of my interns' robot and called it "the robot is poopin on the places that it visits"

Was this the thing happening? Did you resolve this issue?

iris-ua / iris_lama_ros