Local costmap not clearing dynamic obstacle

asked 2021-05-26 10:37:41 -0500

mmaissan gravatar image

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>

https://i.imgur.com/WWnksSW.png

image description

edit retag flag offensive close merge delete