Voxels disappear from local costmap_2d

asked 2020-09-02 06:52:44 -0500

grzegorz.f-16 gravatar image

updated 2021-04-24 02:34:50 -0500

miura gravatar image

HI, I'm using spatio-temporal-voxel-layer plugin to combine RGBD camera data into local costmap 2d.

Everything works fine when I use ros-melodic-costmap-2d (1.16.6) package from apt source.

The compiled version from source (also 1.16.6) looks like has some issue, because voxels disapear randomly from costmap, what you can see on video: https://youtu.be/W8B_2xXiK7s

I've tried ver. 1.16.4-7 and problem is always the same, when compiled from github repository.

Do you have any idea, what can be wrong?

EDIT: My parameter file:

local_costmap:
  transform_tolerance: 0.2
  update_frequency: 6 
  publish_frequency: 6 
  rolling_window: true
  always_send_full_costmap: false
  width: 8
  height: 8
  resolution: 0.075

  plugins: 
   # - {name: static,       type: "costmap_2d::StaticLayer"}
   - {name: obstacles_2d,   type: "costmap_2d::ObstacleLayer"} 
   - {name: obstacles_3d,   type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}
   - {name: inflation,      type: "costmap_2d::InflationLayer"}

  static:
    unknown_cost_value: -1
    lethal_cost_threshold: 100
    map_topic: '/map'  
    first_map_only: true
    subscribe_to_updates: false
    track_unknown_space: true
    use_maximum: false
    trinary_costmap: false 

  obstacles_2d:
    observation_sources: map scan 
    track_unknown_space: false 
    footprint_clearing_enabled: true 
    combination_method: 1

    scan:
      topic: /robot_0/scan
      obstacle_range: 4.5 
      raytrace_range: 10.0 
      observation_persistence: 0.0 
      expected_update_rate: 0.5 
      data_type: LaserScan
      #
      marking: true 
      clearing: true 
      inf_is_valid: True

  obstacles_3d:
    transform_tolerance:   0.2    # seconds
    origin_z:              0.0    #meters
    obstacle_range:        4.0    #meters
    observation_persistence: 0.0  #seconds
    combination_method:    1      #1=max, 0=override # 1=takes highest in layers, 0=takes current layer
    publish_voxel_map:     true   # default off
    update_footprint_enabled: true
    unknown_threshold:     8     #voxel height
    mark_threshold:        2      #voxel height 
    ##
    enabled:               true
    voxel_decay:           20  #10 for test 1    #seconds if linear, e^n if exponential
    decay_model:           0      #0=linear, 1=exponential, -1=persistent
    voxel_size:            0.075   #meters
    track_unknown_space:   true   #default space is unknown
    mapping_mode:          false  # default off, saves map not for navigation
    map_save_duration:     60     #default 60s, how often to autosave
    observation_sources:   rgbd1_clear rgbd1_mark

    rgbd1_mark:
      data_type: PointCloud2
      topic: /robot_0/camera_3d_front/depth/color/points
      marking: true
      clearing: false
      min_obstacle_height: 0.03    #default 0, meters
      max_obstacle_height: 1.8    #defaule 3, meters
      expected_update_rate: 0.1    #default 0, if not updating at this rate at least, remove from buffer
      observation_persistence: 0.1 #do testu #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 #clears measurement buffer after reading values from it
      voxel_filter: true    #default off, apply voxel filter to sensor, recommend on  #performs an approximate voxel filter over the data to reduce
      voxel_min_points: 1          #default 0, minimum points per voxel for voxel filter #minimum points per voxel for voxel filter

      statistical_filter: true     #enable/disable StatisticalFilterOutliers on measurement output
      stat_mean_k: 20              #The number of points to use for mean distance estimation. (for statistical filter)
      stat_mult_stddev: 0.5        #The standard deviation multiplier threshold. All points outside the mean +- sigma * std_mul will be considered outliers. (for statistical filter)

    rgbd1_clear:
      enabled: true                #default true, can be toggled on/off with associated service call #Whether the frustum is enabled on startup. Can be toggled with service
      data_type: PointCloud2
      topic: /robot_0/camera_3d_front/depth/color/points
      marking: false
      clearing: true
      min_obstacle_height: 0.03    #default 0, meters
      max_obstacle_height: 1.8    #defaule 3, meters
      min_z ...
(more)
edit retag flag offensive close merge delete

Comments

Post your parameter file, have you changed anything from defaults?

stevemacenski gravatar image stevemacenski  ( 2020-09-02 14:17:11 -0500 )edit

Yes, I've changed it. Post edited.

grzegorz.f-16 gravatar image grzegorz.f-16  ( 2020-09-03 04:10:01 -0500 )edit