Added voxel grid template class, working on SDF API and library, added message and services for SDF transport