Added path planning correction for depth information

Signed-off-by: Andoni Aguirrezabal <aaguirrezabal@live.com>