Differentiate 2D and 3D planner in local replan server

[fcladera]

Current status: When we initialize a planner instance, we always initialize it in 3D and we are setting the control inputs to zero in the z-direction. We did this for visualization purposes (because we need a 3D voxel map), but this is not optimal in terms of compute.

Ideal scenario: we would like the planner to be initialized in 2D. We may need to modify the current planner-util package to be able to use a slice of the 3D map (but we still can visualize it in rviz).