difficulties using voxel layer in costmap_2d : the 3d obstacles defined in the voxel layer are not added into the local/global costmaps and unable to visualize the voxel grid in rviz [closed]

edit

I am trying to include data from a 3d sensor into a 2d costmap knowing that the costmap is also feeded by a 2d sensor. Therefore, I have different layers, an obstacle layer using 2d laser scan data and a voxel layer using 3d point cloud data from a time of flight device. Please find below the yaml settings :

costmap_common.yaml

transform_tolerance: 0.5

robot_radius: 0.35

costmap_global.yaml

global_costmap:

 global_frame: map

 robot_base_frame: center_wheel_link

 update_frequency: 0.5

 publish_frequency: 0.1

 static_map: true

 map_type: costmap

 track_unknown_space: true

costmap_local.yaml

local_costmap:

 global_frame: map

 robot_base_frame: center_wheel_link

 publish_frequency: 1.0

 update_frequency: 2.0

 rolling_window: true

 map_type: costmap

global_costmap_plugins.yaml

global_costmap:
  width: 10.0
  height: 10.0 
  resolution: 0.03
  origin_x: -5.0
  origin_y: -5.0 

plugins:
 - {name: static_layer, type: "costmap_2d::StaticLayer"}
 - {name: laser_layer, type: "costmap_2d::ObstacleLayer"}
 - {name: tof_layer, type: "costmap_2d::VoxelLayer"}
 - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

inflation_layer:
  inflation_radius: 2.0
  cost_scaling_factor: 10.0

laser_layer:
  enabled: true
  observation_sources: base_scan
  track_unknown_space: true

  base_scan:
    data_type: LaserScan
    topic: /tim500/scan
    marking: false
    clearing: true
    obstacle_range: 20.0
    raytrace_range: 20.0
    expected_update_rate: 15

tof_layer:
  enabled: true
  origin_z: 0.0
  z_resolution: 0.03
  z_voxels: 15
  unknown_threshold: 0
  mark_threshold: 0
  combination_method: 1   
  # for debugging only, let's you see the entire voxel grid
  publish_voxel_map: true
  max_obstacle_height: 3.0
  observation_sources: 3d_cloud 

  3d_cloud:
    data_type: PointCloud2
    topic: /camera/cloud
    marking: true
    clearing: true
    obstacle_range: 3.0
    raytrace_range: 3.0
    max_obstacle_height: 3.0
    min_obstacle_height: 0.0
    expected_update_rate: 3

local_costmap_plugins.yaml

local_costmap:
  width: 2.0
  height: 2.0
  resolution: 0.03
  origin_x: -1.0 
  origin_y: -1.0

 plugins:
 - {name: static_layer, type: "costmap_2d::StaticLayer"}
 - {name: laser_layer, type: "costmap_2d::ObstacleLayer"}
 - {name: tof_layer, type: "costmap_2d::VoxelLayer"}
 - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

inflation_layer:
  inflation_radius: 0.15
  cost_scaling_factor: 10.0

 laser_layer:
   enabled: true
   observation_sources: base_scan
   track_unknown_space: true

     base_scan:
       data_type: LaserScan
       topic: /tim500/scan
       marking: true
       clearing: true
       obstacle_range: 4.0
       raytrace_range: 4.0
       expected_update_rate: 15

 tof_layer:
   enabled: true
   origin_z: 0.0
   z_resolution: 0.03
   z_voxels: 15
   unknown_threshold: 0
   mark_threshold: 0
   combination_method: 1
   # for debugging only, let's you see the entire voxel grid
   publish_voxel_map: true    
   max_obstacle_height: 3.0
   observation_sources: 3d_cloud

    3d_cloud:
      data_type: PointCloud2
      topic: /camera/cloud
      marking: true
      clearing: true
      obstacle_range: 3.0
      raytrace_range: 3.0
      max_obstacle_height: 3.0
      min_obstacle_height: 0.0
      expected_update_rate: 3

First of all, I would like to ask you few specific questions then see in general what could be wrong in my settings. I saw through different related questions that origin_x and origin_y should be set to -width/2 and -height/2 but I didn't understand why and if it is really required, since by default these values are set to 0. Should I set the map_type to voxel or keep it as costmap, knowing that I need to keep both layers active in the costmap. In the voxel layer, do we need to set a negative min_obstacle_height? I would like to precise that in the robot urdf the base_link is lower than 3d sensor which is itself lower then the laser sensor.

In results to this set of configuration, I can visualize the 3d cloud ... (more)