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
</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:
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


https://imgur.com/GNwK0cs

edit retag close merge delete

Sort by » oldest newest most voted

I don't get it why you are stop/starting move base, if you can clarify that, I could give you better options. That is quite aggressive an I would avoid doing it.

What I can suggest now is:

1 - you can call a move_base service to clear layers (http://wiki.ros.org/move_base#Services)

2 - the layers have a dynamic reconfigurable parameter enabled , so you can (de)activate a single layer

3 - you can add recovery behaviour that clear a specific layer if your robot is stuck, you can choose what layer to clear and from what distance to begin clearing, but they will only be triggered if the plan fails or by oscillation conditions (check http://wiki.ros.org/move_base#Parameters).

3 - I've never used it, but it may suit you: https://github.com/DLu/decaying_obsta...

more