Code works at identifying and placing goals with gazebo