Call clear costmap service periodically
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
I tried this on terminal and it worked:
Now I want to execute it periodically or in the begining of each measure.
Could you please post your last edit as an answer instead?
Then accept your own answer. That will convey the fact your question was answered (albeit by yourself) much clearer.