Costmap2D "forget" obstacles after some time
Hi, I am trying to implement a PointCloud2 based obstacle avoidance layer for use with move_base. I have taken the camera's point cloud, down sampled it using a VoxelGrid, and then filtered it using a PassThrough filter. I then use the resulting point cloud in a costmap_2d::ObstacleLayer
. This works fine (for the most part), but after it has detected an obstacle, and has moved past it, I would like that obstacle to reset after some time, is this possible?
I know a rolling window will forget it after a certain distance, but I have some constraints on how the overall costmaps are created. I have a robot that has a fairly well defined startup sequence and want to add this costmap "on-the-fly". I have managed to do this by fetching all the move_base costmap parameters when I want to turn depth-based obstacle avoidance on, update them with my new layers, and then restart the move_base node which has respawn=true
. This then regenerates all the costmaps with my new layers in. Once I am done I reset the params to their original state and restart move_base once more, and all is back to normal. I looked for a way to update this at runtime, and could not find a way, so if this is wrong or there is another way please let me know.
I have tried countless combinations of raytrace and obstacle range values, I have played with the observation_persistence parameter to no avail, I have tried adding footprint_clearing_enabled and track_unknown_space. I have also tried inf_is_valid: true
but no use. Any help in this regard would be greatly appreciated!
I can also upload the rest of the costmap layers if that will help with the debugging process.
obstacle_avoidance.launch
...
<!-- VoxelGrid downsample -->
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager">
<remap from="~input" to="/throttle_filtering_points/filtered_points" />
<rosparam>
filter_field_name: z
filter_limit_min: 0.0
filter_limit_max: 12.0
filter_limit_negative: False
leaf_size: 0.02
</rosparam>
</node>
<!-- Passthrough obstacles -->
<node pkg="nodelet" type="nodelet" name="passthrough" args="load pcl/PassThrough pcl_manager">
<remap from="~input" to="voxel_grid/output" />
<remap from="~output" to="/obstacle_cloud" />
<rosparam>
filter_field_name: z
filter_limit_min: 0.3
filter_limit_max: 1.5
filter_limit_negative: False
input_frame: base_link
</rosparam>
</node>
global_costmap.yaml
costmap:
plugins:
- {name: obstacle_avoidance_layer, type: "costmap_2d::ObstacleLayer"}
obstacle_avoidance_layer:
enabled: true
observation_sources: obstacle_avoidance
combination_method: 1
obstacle_avoidance:
sensor_frame: base_link
data_type: PointCloud2
topic: obstacle_cloud
expected_update_rate: 0.0 # TODO: set this once figured out the rate
observation_persistence: 0
# inf_is_valid: true
raytrace_range: 1000.0
obstacle_range: 2.0
marking: true
clearing: true
min_obstacle_height: 0.3
max_obstacle_height: 1.5