created a 3D costmap based on VoxelCostmap2D