[Costmap2d] LocalCostmap's Obstacle layer stops detecting obstacles

edit

Hello,

I am using the move_base and costmap_2d for an autonomous navigation experiment.

I use only the obstacle_layer for the local_costmap. And I use the LaserScan topic (/scan) as the source for this layer.

As you can see in the following screenshots, from a certain moment, the obstacle layer stops to add any occupied grid even thought clearly the LaserScan topic confirms the existence of the obstacle in the local_costmap zone. this problem has been occurred several times recently.

I should mention that, previously, I have tested several times my costmap configuration in move_base and I had never had this problem. I do not know what causes this problem and I would appreciate your help.

Update: I have found the "origin" of this strange behavior of the local costmap but still I do not understand why ? I have a package called platform which computes and publishes the robot's wheel odometry. Recently, I have added the fusion of the IMU data into this package to publish the /odom/filtered topic which is the corrected odometry by the IMU data. Based on my tests, this part of the code is the origin of this problem (if I deactivate this data fusion and publishing the /odom/filtered topic, then I would not have any issues with the local costmap).

Thanks a lot.

-- Screenshot where the costmap works fine.

-- Screenshot where the obstacle layer stops to add the occupied grids to the local costmap even thought the LaserScan topic shows the existence of the obstacle in the local_costmap zone.

-- Here you can find my local_costmap configuration

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: true
  width: 10.0  # The width of local_costmap it was 5 meter
  height: 10.0  # The hight of local_costmap it was 5 meter
  resolution: 0.05
  transform_tolerance: 0.5

  obstacle_layer:
    enabled: true
    inf_is_valid: true
    track_unknown_space: true
    combination_method: 1
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0.25, obstacle_range: 5.0, raytrace_range: 20.0}

# Pas d'inflation layer pour le TEB
#  inflation_layer:
#    enabled:              true
#    cost_scaling_factor:  5  # exponential rate at which the obstacle cost drops off (default: 10)
#    inflation_radius:     0.2  # max. distance from an obstacle at which costs are incurred for planning paths.
#    track_unknown_space: true


  plugins:
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
#   - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}