Fix a mistake with the pathplanning

Signed-off-by: eldrad <nikofinas@gmail.com>