sbpl_lattice_planner_layer_3d: intermediate progress on octomap_layer_projector