Call clear costmap service periodically

edit

Hello.

I have the ROS navigation package on Kinetic with a Realsense camera. I am transforming the depth image of the camera to laser scan, and then with that laser scan generate obstacles with the costmap_2d package. The problem arrives when the obstacle gets closer to my robot and it generates lots of marks of obstacles and it does not clear the old ones. I have the marking and clearing parameters set to true as you can see in the following code snippet:

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: 3.5
    #inf_is_invalid: false

I want to call the clear costmap service periodically (for example each 500ms) to clean the obstacles and solve the problem, but I don't the way to call it in order to make it works.

I know I must call something like this:

std_srvs::Empty srv;
clear_costmaps_client.call(srv)

But i don't know in which script or .cpp file (move_base.cpp or obstacle_layer.cpp maybe?).

Thanks in advance.

Best regards. Alessandro

EDIT

Finally I solved it using other type of layer (Spatio Temporal Voxel Layer) and generating a mark layer with the LaserScan topic with a voxel decay of 0.75 seconds. The configuration of the layer is the following:

laser_layer_temp:
  enabled:               true
  voxel_decay:           0.75     #seconds if linear, e^n if exponential
  decay_model:           0      #0=linear, 1=exponential, -1=persistent
  voxel_size:            0.05   #meters
  track_unknown_space:   true   #default space is unknown
  observation_persistence: 0.0  #seconds
  max_obstacle_height:   2.0    #meters
  unknown_threshold:     15     #voxel height
  mark_threshold:        0      #voxel height
  update_footprint_enabled: true
  combination_method:    1      #1=max, 0=override
  obstacle_range:        3.0    #meters
  origin_z:              0.0    #meters
  publish_voxel_map:     true   # default off
  transform_tolerance:   0.2    # seconds
  mapping_mode:          false  # default off, saves map not for navigation
  map_save_duration:     60     #default 60s, how often to autosave
  observation_sources:    rgbd1_mark
  rgbd1_mark:
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: false
    #min_obstacle_height: 0.3     #default 0, meters
    #max_obstacle_height: 2.0     #defaule 3, meters
    expected_update_rate: 0.0    #default 0, if not updating at this rate at least, remove from buffer
    observation_persistence: 0.0 #default 0, use all measurements taken during now-value, 0=latest
    inf_is_valid: false          #default false, for laser scans
    clear_after_reading: true    #default false, clear the buffer after the layer gets readings from it
    voxel_filter: true           #default off, apply voxel filter to sensor, recommend on
    voxel_min_points: 0          #default 0, minimum points per voxel for voxel filter

Best regards. Alessandro