Problem when detecting if the goal position is reached

[Heenne]

Fehler muss in den Methoden behoben werden, die in fpc_ros.cpp line 63-74 aufgerufen werden.