Added voxels visualizations to occupancy grid.
(NOTE: About to try and change from arm_navigation distance_field to moveit distance_field