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]
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 ...
Duplicate: http://answers.ros.org/question/24806...