Clear obstacles of obstacle layer of costmap

edit

Hello everyone.

I have a Realsense D435 camera on my application, and I use its depth_image to generate a laser scan topic with the package depthimage_to_laserscan. This works allright.

The problem I want to solve is when I generate obstacles with this laser topic, it marks me the obstacle but when I move and the obstacle get closer it updates the position of the obstacle but it does not lcear the previus marks of the obstacle.

Here you can see screenshots of the problems:

In the image above the obstacle layer is just generating marks where the sensor sees an obstacle (green line). But in the image below the obstacle gets close and it generates the new marks, but the old marks are not being cleared.

I think is an error of configuration parameters of local_costmap, so here it is the code:

costmap_common_params.yaml

footprint: [[-0.30 , 0.38], [0.70, 0.38], [0.70, -0.38], [-0.30, -0.38]]

laser_layer: #Laser
  #track_unknown_space: false
  #footprint_clearing_enabled: true
  #combination_method: 1
  observation_sources:  laser
  laser:
    topic: /scan
    sensor_frame: camera_depth_frame
    #observation_persistance: 0.0
    expected_update_rate: 0
    data_type: LaserScan
    clearing: true
    marking: true
    #max_obstacle_height: 2.0
    #min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 5.0
    #inf_is_invalid: false


pointcloud_layer: #Nube de puntos
  #origin_z: 0.0
  #z_resolution: 0.2
  #z_voxels: 10
  #unknown_threshold: pointcloud_layer/z_voxels
  #mark_threshold: 0
  #publish_voxel_map: false
  #footprint_clearing_enabled: true
  observation_sources: pointcloud
  pointcloud:
    topic: /camera/depth/color/points
    sensor_frame: camera_depth_frame
    #observation_persistance: 0.0
    expected_update_rate: 0
    data_type: PointCloud2
    clearing: true
    marking: true
    #max_obstacle_height: 2.0
    #min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 5.0
    #inf_is_invalid: false


inflation_layer:
  inflation_radius: 0.75
  #cost_scaling_factor: 10.0

local_costmap_params.yaml

local_costmap:
  plugins:
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: laser_layer, type: "costmap_2d::ObstacleLayer"} #Laser sensors
    #- {name: pointcloud_layer, type: "costmap_2d::VoxelLayer"} #Pointcloud sensors
    #- {name: ultrasonic,       type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

  update_frequency: 2.0 #HIGH CPU usage with sensors
  publish_frequency: 50.0 #Reducir para aligerar CPU

  global_frame: "odom" #To inflate obstacles
  robot_base_frame: "base_link"

  #static_map: false
  rolling_window: true
  width: 6.0 #6
  height: 6.0 #6
  resolution: 0.05 #0.01

  #always_send_full_costmap: true

global_costmap_params.yaml

global_costmap:

  plugins:
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    #- {name: ultrasonic,   type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

  global_frame: "map"
  robot_base_frame: "base_link"

  update_frequency: 2.0 #HIGH CPU usage with sensors
  publish_frequency: 50.0 #Reducir para aligerar CPU


  resolution: 0.5 #0.01 #The resolution of the map in meters/cell.
  transform_tolerance: 0.2 #Specifies the delay in transform (tf) data that is tolerable in seconds
  map_type: costmap

  #always_send_full_costmap: true

base_local_planner_params.yaml

#recovery_behavior_enabled: false
#clearing_rotation_allowed: false
controller_frequency: 10 #Default 20 took many time

TrajectoryPlannerROS:

  max_vel_x: 0.4 #meters/sec #0.6
  min_vel_x: -0.1
  max_vel_y: 0.0  # zero for a differential drive robot
  min_vel_y: 0.0  #radians/sec
  max_vel_theta: 1.0 #3
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.1 #radians/sec, in-place rotations
  escape_vel: -0.1 #0.1
  acc_lim_x: 0.4 #meters/sec^2
  acc_lim_y: 0.0  # zero for a differential drive robot
  acc_lim_theta: 1.0 #radians/sec^2

  holonomic_robot: false

   #####Trajectory Scoring Parameters#####

  meter_scoring: true #goal_distance and path_distance are expressed in units ...