sonars with range_sensor_layer not clearing obstacles

edit

Hi everyone,

I'm working on a turtlebot3 burger and I'm trying to integrate sonars in order to detect low obstacles in front of the robot. Currently, there are two sonars mounted to the robot, and both are publishing Range messages on separate topics (called '/sensor_sonar_left' and '/sensor_sonar_right'. Also separate joints were added in the turtlebot3 burgers urdf file in order to be able to link the distances seen by the sonars to the base frame.

In order to use the sonars for obstacle avoidance, the navigation stack for the turtlebot3 was adapted to include the range_sensor_layer.

Now, when visualizing the costmaps created by the sonar and range sensor layer, we can see that the sonar always seems to add a line of obstacles where the cone formed by the two sonars overlaps. This can mainly be seen going from the second image to the third image, where we inserted an object in front of the sonar in order to add an obstacle to the costmap, and afterwards removed it in order to clear that same space. However, as can be seen on the third image, once the obstacle is added, it is not being removed when the sonars range is further than the previously detected object.

The obstacles that are added cause the robot to get stuck and not want to move forward, although there are no more obstacles there. What am I missing in order to clear obstacles marked by the sonars?

As a reference, I'm using Ros Kinetic on Ubuntu 16.04, and I'm using the following configuration files. As can be seen, infrared sensors were added as well using a separate range_sensor_layer. They are used as fixed distance rangers in order to detect the edge of a table, and are currently working as expected.

costmap_common:

footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#robot_radius: 0.105

map_type: costmap

obstacle_layer: 
  observation_sources: scan
  scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}
  obstacle_range: 3.0
  raytrace_range: 3.5

inflation_layer:
  inflation_radius: 0.1
  cost_scaling_factor: 3.0

ir_table_layer:
  topics: ['/sensor_IR_left_range','/sensor_IR_right_range']
  no_readings_timeout: 0.0
  clear_threshold: 0.2
  mark_threshold: 0.8
  clear_on_max_reading: true

sonar_layer:
  topics: ['/sensor_sonar_left','/sensor_sonar_right']
  no_readings_timeout: 2.0
  clear_threshold: 0.4
  mark_threshold: 0.9
  clear_on_max_reading: true

plugins:
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
  - {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
  - {name: ir_table_layer, type: "range_sensor_layer::RangeSensorLayer"}

global_costmap:

global_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5

  static_map: false
  rolling_window: true
  width: 5
  height: 5
  resolution: 0.05

local_costmap

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  

  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

Example of a sonar range message:

---
header: 
  seq: 2179
  stamp: 
    secs: 1575713861
    nsecs: 293962976
  frame_id: "sonar_right"
radiation_type: 0
field_of_view: 7.0
min_range: 0.019999999553
max_range: 2.0
range: 1.16586005688
---