Fixed bugs in createUpdatedGoal. WORKS IN GAZEBO!!!!!1111einseins