Local costmap not clearing dynamic obstacle
Hi,
I have a stereo camera that publishes a PointCloud2 message. This message gets passed into an Voxel filter to reduce the amount of points. However, when some object passes through the local costmap (like a person walking by), the local costmap does not get cleared. In the attached image, you can see that there are no obstacles visible in the point cloud. There are only obstacles visible outside of the costmap. However, the local costmap still shows obstacles. Why are my obstacles not cleared after they move out of the local costmap frame?
Here's my code:
local_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 30
publish_frequency: 30
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.2
origin_x: -2.0
origin_y: -2.0
observation_sources: zed2 infrared
infrared: {
sensor_frame: base_link,
data_type: PointCloud2,
topic: infrared,
marking: true,
clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 99999.0}
# assuming receiving a cloud from rtabmap_ros/obstacles_detection node
zed2: {
sensor_frame: base_link,
data_type: PointCloud2,
topic: obstacles,
marking: true,
clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 99999.0}
--
<node pkg="nodelet" type="nodelet" name="voxel_grid_1" args="load pcl/VoxelGrid obstacle_manager" output="screen">
<remap from="~input" to="/zed_wrapper/point_cloud/cloud_registered"/>
<remap from="~output" to="/voxel_filter"/>
<rosparam>
filter_field_name: z
filter_limit_min: -0.1
filter_limit_max: 2.0
filter_limit_negative: False
leaf_size: 0.01
</rosparam>
</node>
<node pkg="nodelet" type="nodelet" name="voxel_grid_2" args="load pcl/VoxelGrid obstacle_manager" output="screen">
<remap from="~input" to="/voxel_filter"/>
<rosparam>
filter_field_name: x
filter_limit_min: 0
filter_limit_max: 3.0
filter_limit_negative: False
leaf_size: 0.01
</rosparam>
</node>